Diff: main.cpp

Revision:

17:5b9fbb6133ad

Parent:

16:a9885a66048f

Child:

18:aa8c08f6f474

--- a/main.cpp	Fri Sep 09 11:41:27 2016 +0000
+++ b/main.cpp	Sat Sep 10 13:32:25 2016 +0000
@@ -10,89 +10,133 @@
 DigitalOut ledr(PC_0); //red
 
 
-const int dist=30;
-const int slidedist=150;
-const int space=100;
+const int dist=150;//タイヤ、アーム間の距離
+const int slidedist=75;
+const int space=0;//壁やばそうなときのよゆう
+const float btime=1.7;
+const float allowdegree=0.2;
+const float allowlength=0.2;
+
+
+int main(){
+        setup();
+    initmotor();
+    close_arm();
+    while(1){
+    pmove(600,0); 
+    pmove2(300,600); 
+    pmove2(0,0);     
+    }
+    
+}
+
+/*
 
 int main()
 {
-    int team=1;
+    
+    int team=1,phase=0;
+    team=teamLED();
     setup();
     initmotor();
+    close_arm();
     
+    while(start()){}
    int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
    int targety[9] = { team*(300+slidedist),team*(600+slidedist),team*(900+slidedist),team*(1200+slidedist),team*(900+slidedist),team*(600+slidedist),team*(600+slidedist),team*(0+slidedist),team*(300+slidedist) };
    int goalx=0,goaly1=1200+150,goaly2=900+slidedist,goaly3=600+slidedist,goaly4=300+slidedist;
     
+    phase=phaseSW();
+
+    //phaseごとの初期位置修正
     
+    
+    //
 
 //オブジェクト0,1,2回収
-
+    if(phase==0){
        pmove(targetx[0],0);
        open_hand();
        pmove(targetx[0],targety[0]);
        close_hand();
        beltup();
-       wait(5);
+       wait(btime);
        beltstop();
        open_hand();
        //回収
        pmove(targetx[1],targety[1]);
        close_hand();
        beltup();
-       wait(5);
+       wait(btime);
        beltstop();
        open_hand();
        //回収
        pmove(targetx[2],targety[2]);
        //回収
-       
-       pmove(600,1200+slidedist);//次につなげるハブ
-       
+       close_hand();
+       pmove(600,1200+slidedist-space*3);//次につなげるハブ
        pmove(goalx,goaly1);
        //リリース
+       open_arm();
+       open_hand();
        back300();//(300,1200)なう
-       /****************/
+    turnrad_ccw(2*PI);
+       phase=1;    
        
+    }
+    
+    if(phase==1){
        //オブジェクト3,4回収
-       pmove(targetx[3]+space,targety[3]);
-       //回収
-       pmove(1200+space,900+slidedist);//ハブ
+       open_arm();
+       pmove(targetx[3],targety[3]);
+       close_arm();
+       close_hand();
+       beltup();
+       wait(btime);
+       beltstop();
+       pmove(1200,900+slidedist);//ハブ
+       open_hand();
        pmove(targetx[4],targety[4]);
-       //回収?
+       
        pmove(goalx,goaly2);
+       open_arm();
+       open_hand();
        //リリース
        back300();
-       /***************/
-       
-       //オブジェクト5,7回収
+    
+        phase=2;
+    }
+    
+    if(phase==2){
+       //オブジェクト5,6回収
+       open_arm();
        pmove(1200,900+slidedist);//ハブ
        pmove(targetx[5],targety[5]);
        //回収
-       pmove(targetx[7],targety[7]);
+       pmove(targetx[6],targety[6]);
        //回収?
-       pmove(600,0+slidedist);
-       //リリース
-       pmove(600,1200+slidedist);//ハブ
-       
-       pmove(goalx,goaly1);
+
+       pmove(goalx,goaly3);
        back300();
-       /**********************/
-       
-       //オブジェクト6,8回収
-       pmove(300,0+slidedist);//ハブ
-       pmove(900,0+slidedist);//ハブ
-       pmove(targetx[8],targety[8]);
+       phase=3;
+    }
+    
+    if(phase==3){
+       //オブジェクト7,8回収
+       pmove(1200,600+slidedist);//ハブ
+
+       pmove(targetx[7],targety[7]);
        //回収
-        pmove(targetx[6],targety[6]);
+        pmove(900,0);
         //回収
-        pmove(900,1200+slidedist);
+        pmove(targetx[8],targety[8]);
        //回収?
-       pmove(goalx,goaly1);
+       pmove(goalx,300+slidedist+30);
        //リリース
        back300();
-       /**********************/
        
+       phase=4;
+       }
 
 
-}
+}*/
\ No newline at end of file