Diff: main.cpp

Revision:

7:2b63f0a1b679

Parent:

6:26ac9f539e8b

Child:

8:e2dc708fc3e6

--- a/main.cpp	Sat Sep 03 08:04:59 2016 +0000
+++ b/main.cpp	Mon Sep 05 07:32:38 2016 +0000
@@ -6,14 +6,30 @@
 PwmOut M2ccw(PB_14);
 PwmOut M2cw(PB_13);
 
+DigitalIn teamSW(PC_11);
+DigitalOut teamledblue(PC_10);
+DigitalOut teamledred(PC_12);
+
+DigitalIn phase1(PA_5);
+DigitalIn phase2(PA_6);
+
 const int allowlength=5;
 const float allowdegree=0.02;
 const int rightspeed=29*2;
 const int leftspeed=31*2;
 const int turnspeed=15*2;
 
-const float PIfact=2.89;
+const float PIfact=2.88;
 
+int phaseSW(void){
+    
+    
+    phase1.mode(PullUp);
+    phase2.mode(PullUp);
+    int SW=phase1+2*phase2;
+    pc.printf("%d\n\r",SW);
+    return SW;
+}
 void initmotor()
 {
 
@@ -117,17 +133,40 @@
         }
     }
 }
-/*
-int main(){
-    setup();
-    initmotor();
+
+void turnccw80()
+{   float GODRAD=1.2;
+    float pt;
+    move(turnspeed,(-1)*turnspeed);
+
+    update();
+    pt=coordinateTheta();
 
-    for(int i=0; i < 4; i++)
-    {
-        movelength(1200);
-        turnccw();
+    while(1) {
+        update();
+        if(pt-coordinateTheta()<(-1)*GODRAD) {
+            move(0,0);
+            break;
+        }
     }
-}*/
+}
+
+void turncw80()
+{   float GODRAD=1.2;
+    float pt;
+    move((-1)*turnspeed,turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()>GODRAD) {
+            move(0,0);
+            break;
+        }
+    }
+}
 
 void pmovex(int length){
     int px,py,dx,dy;
@@ -153,15 +192,84 @@
     }
 }
 
-int main()
+int teamLED()
 {
+    teamSW.mode(PullUp);
+    if(teamSW) {
+        teamledblue=1;
+        teamledred=0;
+        return -1;
+    } else {
+        teamledblue=0;
+        teamledred=1;
+        return 1;
+    }
+}
+
+int main(){
+    int team=1,phase=0;
     setup();
     initmotor();
+    team=teamLED();
+    phase=phaseSW();
     
-    movelength(600);
-    turnccw();
-    movelength(900);
-    turnccw();
-    movelength(600);
+    if(team>0){
+        
+        if(phase==0){
+            movelength(600);
+            turnccw();
+            movelength(900);
+            turnccw80();
+            movelength(600-50);
+            wait(30);
+        }
+        
+        if(phase==1){
+            movelength(900);
+            turnccw();
+            movelength(900);
+            turnccw80();
+            movelength(900-50);
+            wait(30);
+        }
+
+        if(phase==2){
+            movelength(1200);
+            turnccw();
+            movelength(1200);
+            turnccw80();
+            movelength(1200-50);
+            wait(30);
+        }
+    }
 
+    if(team<0){
+        
+        if(phase==0){
+            movelength(600);
+            turncw();
+            movelength(900);
+            turncw();
+            movelength(600);
+            wait(30);
+        }
+        
+        if(phase==1){
+            movelength(900);
+            turncw();
+            movelength(900);
+            turncw();
+            movelength(900);
+            wait(30);   
+    }
+    
+        if(phase==2){
+            movelength(1200);
+            turncw();
+            movelength(600);
+            turncw();
+            movelength(1200);
+            wait(30);
+        }
+    }
 }
\ No newline at end of file