Diff: PathFollowing.cpp

Revision:

11:8407c616b1a8

Parent:

10:107386dd097b

--- a/PathFollowing.cpp	Thu Feb 07 02:31:15 2019 +0000
+++ b/PathFollowing.cpp	Fri Feb 08 06:53:01 2019 +0000
@@ -76,8 +76,8 @@
     
      gyro.initialize();
    motor_tick.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
-    EC1.setDiameter_mm(48);
-    EC2.setDiameter_mm(48);  //測定輪半径//後で測定
+    EC1.setDiameter_mm(51);
+    EC2.setDiameter_mm(51);  //測定輪半径//後で測定
     
 }
 
@@ -568,13 +568,14 @@
    calc_xy(tgt_angle, u, v);
 
    while(1){ //機体の位置を目標領域（目標座標＋許容誤差）に収める
-    gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,100,0,5,0.1,10,0.1,500,tgt_angle);
+    gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,130,5,0.1,10,0.1,500,tgt_angle);
     MotorControl(0,0,0,0);
     
     calc_xy(tgt_angle, u, v);
     
     r=hypot(now_x - tgt_x, now_y - tgt_y);
     
+    
     if(r < R) break;
     
    }
@@ -585,19 +586,23 @@
        
              out = 10 * (tgt_angle - now_angle);
 
-                if(out > 300) {  //0～179°のときは時計回りに回転
-                     MotorControl(300,300,300,300);
+                if(out > 300) {  
+                     MotorControl(-300,-300,-300,-300);
                 }else if(out < -300){
-                     MotorControl(-300,-300,-300,-300);   
-                }else if(out <= 300 && out > -300) {
-                     MotorControl(out,out,out,out);
+                     MotorControl(300,300,300,300);   
+                }else if((out <= 300 && out >= 30) || (out <= -30 && out > -300)) {
+                     MotorControl(-out,-out,-out,-out);
+                }else if(0 <= out && out < 30){
+                     MotorControl(-30,-30,-30,-30);
+                }else if(-30 < out && out < 0){
+                     MotorControl(30,30,30,30);
                 }
 
-     if(tgt_angle - 2 < now_angle && now_angle < tgt_angle + 2){ // 目標角度からの許容誤差内に機体の角度が収まった時、補正終了
-        //printf("end\n\r");
+     if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5){ // 目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+        printf("end\n\r");
         break;
      }
-    // printf("continue\n\r");
+     printf("continue\n\r");
     printf("pos_correction// now_angle = %f",now_angle);
    }
    MotorControl(0,0,0,0);