Diff: PathFollowing.cpp

Revision:

10:107386dd097b

Parent:

9:135bbd007509

Child:

11:8407c616b1a8

--- a/PathFollowing.cpp	Wed Feb 06 04:25:31 2019 +0000
+++ b/PathFollowing.cpp	Thu Feb 07 02:31:15 2019 +0000
@@ -26,8 +26,8 @@
 //Ticker ticker;  //for enc
 Timer timer;
 
-Ec EC1(p8,p26,NC,500,0.05);
-Ec EC2(p21,p22,NC,500,0.05);
+Ec EC1(p8,p26,NC,1024,0.05);
+Ec EC2(p21,p22,NC,1024,0.05);
 R1370P gyro(p28,p27);
 
 double new_dist1=0,new_dist2=0;
@@ -383,7 +383,7 @@
                 double point_x1,double point_y1,
                 double point_x2,double point_y2,
                 int theta,
-                double speed,
+                double speed, 
                 double q_p,double q_d,
                 double r_p,double r_d,
                 double r_out_max,
@@ -540,7 +540,7 @@
 
         //can_read();
         
-        printf("%f\n",usw_data3);
+        //printf("%f\n",usw_data3);
         
         calc_xy(target_angle,u,v);
 
@@ -557,7 +557,7 @@
         debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m_1,m_2,m_3,m_4,now_x,now_y,now_angle);
 
         if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
-    }
+    } 
 }
 
 void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v){ //位置補正（使用前にMotorControl(0,0,0,0)を入れる）
@@ -593,8 +593,14 @@
                      MotorControl(out,out,out,out);
                 }
 
-     if(tgt_angle - 2 < now_angle && now_angle < tgt_angle + 2) break; // 目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+     if(tgt_angle - 2 < now_angle && now_angle < tgt_angle + 2){ // 目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+        //printf("end\n\r");
+        break;
+     }
+    // printf("continue\n\r");
+    printf("pos_correction// now_angle = %f",now_angle);
    }
    MotorControl(0,0,0,0);
 }
 
+