Diff: main.cpp

Revision:

18:aa8c08f6f474

Parent:

17:5b9fbb6133ad

--- a/main.cpp	Sat Sep 10 13:32:25 2016 +0000
+++ b/main.cpp	Sun Sep 11 11:40:17 2016 +0000
@@ -11,61 +11,49 @@
 
 
 const int dist=150;//タイヤ、アーム間の距離
-const int slidedist=75;
-const int space=0;//壁やばそうなときのよゆう
-const float btime=1.7;
+const int slidedist=102-20;
+const int space=50;//壁やばそうなときのよゆう
+const float btime=1.9;
 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,phase=0;
-    team=teamLED();
-    setup();
-    initmotor();
+    int teams=1,phase=0;
+    teams=teamLED();
+    int team = 1;
+    setup(teams);
+    initmotor(teams);
+    open_arm();
+    wait(2);
     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;
+   int goalx=150,goaly=1200+(slidedist)*0;//yへのslidediatを無視
     
     phase=phaseSW();
-
-    //phaseごとの初期位置修正
-    
     
     //
 
 //オブジェクト0,1,2回収
     if(phase==0){
-       pmove(targetx[0],0);
+       pmove(targetx[0],0); //傾きをはじめから向くように
        open_hand();
-       pmove(targetx[0],targety[0]);
+       pmove2(targetx[0],targety[0]);
        close_hand();
+       wait(0.3);
        beltup();
        wait(btime);
        beltstop();
        open_hand();
        //回収
-       pmove(targetx[1],targety[1]);
+       pmove2(targetx[1],targety[1]);
        close_hand();
+       wait(0.3);
        beltup();
        wait(btime);
        beltstop();
@@ -74,69 +62,108 @@
        pmove(targetx[2],targety[2]);
        //回収
        close_hand();
-       pmove(600,1200+slidedist-space*3);//次につなげるハブ
-       pmove(goalx,goaly1);
+       pmove2(goalx,goaly);
        //リリース
+       turnrad_ccw(PI,team);
        open_arm();
        open_hand();
+       wait(0.5);
        back300();//(300,1200)なう
-    turnrad_ccw(2*PI);
-       phase=1;    
-       
-    }
-    
-    if(phase==1){
-       //オブジェクト3,4回収
+        turnrad_ccw(2*PI,team);
+//--------------------------------------------------------------------
        open_arm();
        pmove(targetx[3],targety[3]);
        close_arm();
        close_hand();
+       turnrad_cw(PI,team);
+       /*
+       open_hanf();
+       pmove(1800,team*(1200+slidedist));
+       closehand();
+       */
+       pmove(150,team*(1200+slidedist-space));//ゴール
+
+        open_arm();
+        open_hand();
+        wait(0.3);
+
+       
+        back300();
+        turnrad_ccw(2*PI,team);
+        phase=2;
+        
+       //また縦に回収
+       close_arm();
+       pmove2(900,team*(1200+slidedist+space));
+
+       open_hand();
+       pmove2(targetx[4],targety[4]);
+       close_hand();
+       wait(0.3);
+       beltup();
+       wait(btime);
+       beltstop();
+       open_hand();
+       //回収
+       pmove(targetx[6],targety[6]);
+       close_hand();
+       wait(0.3);
        beltup();
        wait(btime);
        beltstop();
-       pmove(1200,900+slidedist);//ハブ
        open_hand();
-       pmove(targetx[4],targety[4]);
+       //回収
+       pmove(targetx[8],targety[8]);
+       //回収
+       close_hand();
+       pmove2(150,team*(900+slidedist));//斜め移動
        
-       pmove(goalx,goaly2);
+       turnrad_ccw(PI,team);
        open_arm();
        open_hand();
-       //リリース
        back300();
     
-        phase=2;
+       
+       pmove2(700,(600+slidedist));//ハブ
+
+       pmove(targetx[5],targety[5]);
+       close_arm();
+       close_hand();
+       nxback300();
+       
+       pmove2(150,team*(900+slidedist));
+       open_arm();
+       open_hand();
+       
+        back300();
+        while(1){
+            buzzer(1);
+            wait(0.5);
+            buzzer(0);
+            wait(1);   
+        }
     }
     
-    if(phase==2){
-       //オブジェクト5,6回収
-       open_arm();
-       pmove(1200,900+slidedist);//ハブ
-       pmove(targetx[5],targety[5]);
-       //回収
-       pmove(targetx[6],targety[6]);
-       //回収?
-
-       pmove(goalx,goaly3);
-       back300();
-       phase=3;
+    
+    if(phase==1){
+        
+     pmove(targetx[7],targety[7]);
+     back300();
+     pmove2(goalx,goaly);
+     turnrad_ccw(PI,team);
+     open_arm();
+     open_hand();
+     back300();
+     
+     pmove2(900,600+slidedist);
+     
+        pmove(targetx[5],targety[5]);
+        pmove2(goalx,goaly);
+        turnrad_ccw(2*PI,team);
+        open_arm();
+        open_hand();
+        back300();
+        
     }
     
-    if(phase==3){
-       //オブジェクト7,8回収
-       pmove(1200,600+slidedist);//ハブ
-
-       pmove(targetx[7],targety[7]);
-       //回収
-        pmove(900,0);
-        //回収
-        pmove(targetx[8],targety[8]);
-       //回収?
-       pmove(goalx,300+slidedist+30);
-       //リリース
-       back300();
-       
-       phase=4;
-       }
-
-
-}*/
\ No newline at end of file
+}
\ No newline at end of file