Diff: main.cpp

Revision:

9:f4dbffb78eb8

Parent:

8:e2dc708fc3e6

diff -r e2dc708fc3e6 -r f4dbffb78eb8 main.cpp
--- a/main.cpp	Mon Sep 05 11:55:42 2016 +0000
+++ b/main.cpp	Tue Sep 06 08:46:22 2016 +0000
@@ -6,27 +6,34 @@
 PwmOut M2ccw(PB_14);
 PwmOut M2cw(PB_13);
 
+DigitalIn start(USER_BUTTON);
+
 DigitalIn teamSW(PC_11);
 DigitalOut teamledblue(PC_10);
 DigitalOut teamledred(PC_12);
 
 DigitalIn phase1(PA_5);
 DigitalIn phase2(PA_6);
+DigitalIn phase4(PA_7);
+DigitalIn phase8(PB_12);
 
 const int allowlength=5;
 const float allowdegree=0.02;
-const int rightspeed=29*2;
-const int leftspeed=31*2;
+const int rightspeed=30*2-2;
+const int leftspeed=30*2+1;
 const int turnspeed=15*2;
 
-const float PIfact=2.88;
-
+const float PIfact=2.8;
+const float PIblue=2.8;
 int phaseSW(void){
     
     
     phase1.mode(PullUp);
     phase2.mode(PullUp);
-    int SW=phase1+2*phase2;
+    phase4.mode(PullUp);
+    phase8.mode(PullUp);    
+    
+    int SW=phase1+2*phase2+4*phase4+8*phase8;
     pc.printf("%d\n\r",SW);
     return SW;
 }
@@ -209,14 +216,13 @@
 
 
 
-
 int main(){
     int team=1,phase=0,comand,buf;
     setup();
     initmotor();
     team=teamLED();
     phase=phaseSW();
-    
+    while(start);
     if(team>0){
         
         if(phase==0){
@@ -240,20 +246,29 @@
         if(phase==2){
             movelength(1200);
             turnccw();
-            movelength(1200-150);
+            movelength(700);
+            turnccw80();
+            movelength(1200-100);
+            wait(30);
+        }
+        
+        if(phase==3){
+            movelength(1200);
             turnccw();
+            movelength(1000);
+            turnccw80();
             movelength(1200-100);
             wait(30);
         }
         
         if(phase==15){
             buf=15;
-            while(buf>phaseSW()){
+            while(buf>=phaseSW()){
                 buf=phaseSW();
                 }
                 //スイッチを上昇させると、上昇前で確定
             //この時点でbufに現在のスイッチの状態がはいる。
-            
+              pc.printf("Hello World !%d\n",buf);
             movelength(300+75*buf);
             turnccw();
             movelength(1200);
@@ -263,33 +278,60 @@
         }
     }
 
-    if(team<0){
+if(team<0){
         
         if(phase==0){
             movelength(600);
-            turncw();
+            turncw80();
             movelength(900);
-            turncw();
-            movelength(600);
+            turncw80();
+            movelength(600-50);
             wait(30);
         }
         
         if(phase==1){
             movelength(900);
-            turncw();
-            movelength(900);
-            turncw();
+            turncw80();
             movelength(900);
-            wait(30);   
-    }
-    
+            turncw80();
+            movelength(900-50);
+            wait(30);
+        }
+
         if(phase==2){
-            movelength(1200);
-            turncw();
+            movelength(1200-50);
+            turncw80();
             movelength(600);
+            turncw80();
+            movelength(1200-100);
+            wait(30);
+        }
+        
+        if(phase==3){
+            movelength(1200-50);
+            turncw80();
+            movelength(1000);
+            turncw80();
+            movelength(1200-100);
+            wait(30);
+        }
+        
+        if(phase==15){
+            buf=15;
+            while(buf>=phaseSW()){
+                buf=phaseSW();
+                }
+            
+                //スイッチを上昇させると、上昇前で確定
+            //この時点でbufに現在のスイッチの状態がはいる。
+            
+            movelength(300+75*buf);
             turncw();
             movelength(1200);
+            turncw80();
+            movelength(300+75*buf);
             wait(30);
         }
     }
+
 }
\ No newline at end of file