Diff: main.cpp

Revision:

2:b204cf2f9b60

Parent:

1:a1e592eca305

Child:

4:dcb03da10fa7

--- a/main.cpp	Tue Aug 30 06:51:41 2016 +0000
+++ b/main.cpp	Wed Aug 31 11:09:45 2016 +0000
@@ -1,144 +1,62 @@
 //受渡前での妨害に対応したい
-
 #include"header.h"
+Serial pc(SERIAL_TX, SERIAL_RX);
 
-int team;//(-1)は青
-
-int main(void){
+void movelength(int length){
+    int px,py,pt;
+    update(-right.getPulses(), left.getPulses());
+    px=coordinateX();
+    py=coordinateY();
+    pt=coordinateTheta();
     
-    teamSW.mode(PullUp); 
-    if(teamSW){
-        team=1;
-        teamledred=1;
-        teamledblue=0;
-    }else{
-        team=-1;
-        teamledred=0;
-        teamledblue=1;
-    }
-    initmotor();
-    initencorder();
-    
-    move(0,0);
-    close_arm();
-    /*以下redteamのフィールドマップを読み込む(mm)
-    
-    blue team の場合はすべてのy座標に(-1)をかける
-    
-    */
+    move(30,30);
+    while(1){
+        update(-right.getPulses(), left.getPulses());
+        if((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY())>length*length){break;}
         
-    //オブジェクトの位置
-    //原点はスタートゾーンの中心
-    int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
-    int targety[9] = { team*300,team*600,team*900,team*1200,team*900,team*600,team*600,0,team*300 };
-    
-    //ハブの位置
-    int hub1x = 600;
-    int hub1y = 0;
-
-    int hub2x = 900;
-    int hub2y = 0;
+        }
+}
 
 
-    int goalx = 0, goaly = team*1200;//受け渡しゾーンの中心
-    //定義終了
-    
-    int retry=0;        
-    
-    
-    //ここでretryに数値を代入
-            
-    switch(retry){
-        case 0:
-            open_hand();
-            moveto(hub1x, hub1y);
-        
-            moveto(targetx[0], targety[0]);
-            close_hand();
-            lift(1);
-    
-        case 1:
-            open_hand();
-            moveto(targetx[1], targety[1]);
-            close_hand();
-            lift(1);
-    
-        case 2:
-            open_hand();
-            moveto(targetx[2], targety[2]);
-            close_hand();
-    
-            moveto(goalx,goaly);
-            open_arm();
-            release();
-            back();
-            //3つリリースした
-        case 3:
-            open_arm();
-            open_hand();
-            moveto(targetx[3], targety[3]);
-            close_arm();
-            close_hand();
-            lift(2);
-    
-        case 4:
-            open_hand();
-            moveto(targetx[4], targety[4]);
-            close_hand();
+void turncw(int degree) {
+  initencorder();
+  initmotor();
+  
+  int rad=0,np=1;
+  if(degree<0){np=-1;}
+  rad=degree*PI/180;
+  move(30*np,-30*np);
+  float t,theta;
+  int x,y;
+  update(-right.getPulses(), left.getPulses());
+  t=coordinateTheta();
+  while(1){
+
+      pc.printf("x:%d y:%d t:%f \n\r",x,y,theta*180/PI);
+      x=coordinateX();
+      y=coordinateY();
+      theta=coordinateTheta();
+      update(-right.getPulses(), left.getPulses());
+      if(theta-t > np*rad){break;}
+      if(theta-t < (-1)*np*rad){break;}
+       //   moveto (30,30);
+       /*
+               move(30,30);
+          update(-right.getPulses(), left.getPulses());
+          pc.printf("x:%d y:%d t:%f \n\r",coordinateX(), coordinateY(), coordinateTheta());
+          if(coordinateX()>300){
+              while(1){*/
+             
+  }
+  move(0,0);
+}
+
+
+int main(){
     
-            moveto(goalx,goaly);
-            open_arm();
-            release();
-            back();
-            //3つリリースした
-    
-        case 5:
-            moveto(targetx[3], targety[3]);//ハブとして利用
-    
-            open_arm();
-            open_hand();
-            moveto(targetx[5], targety[5]);
-            
-            close_arm();
-            close_hand();
-            lift(2);
-    
-        case 6:
-            open_hand();
-            moveto(targetx[6], targety[6]);
-            close_hand();
-    
-    
-            moveto(goalx,goaly);
-            open_arm();
-            release();
+    movelength(600);
+    turncw(90);
+    movelength(1200);
+    turncw(90);
     
-            back();
-    
-        case 7:
-            moveto(targetx[3], targety[3]);//ハブとして利用
-    
-            open_arm();
-            open_hand();
-            moveto(targetx[7], targety[7]);
-            close_hand();
-            lift(2);
-    
-            moveto(hub2x, hub2y);
-    
-        case 8:
-            open_hand();
-            moveto(targetx[8], targety[8]);
-            close_hand();
-    
-    
-            moveto(goalx,goaly);
-            open_arm();
-    
-            back();
-    
-            moveto(1800,team*1200);//invade
-    
-            return 0;//終了
-    }
 }
\ No newline at end of file