Diff: PathFollowing.cpp

Revision:

9:135bbd007509

Parent:

8:524d86b2073f

--- a/PathFollowing.cpp	Fri Jan 11 08:28:11 2019 +0000
+++ b/PathFollowing.cpp	Wed Feb 06 04:25:31 2019 +0000
@@ -6,6 +6,7 @@
 #include "move4wheel.h"
 #include <stdarg.h>
 #include "Maxon_setting.h"
+#include "Harurobo_CAN.h"
 
 #define PI 3.141592
 
@@ -537,6 +538,10 @@
     
     while (1) {
 
+        //can_read();
+        
+        printf("%f\n",usw_data3);
+        
         calc_xy(target_angle,u,v);
 
         XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2);
@@ -555,3 +560,41 @@
     }
 }
 
+void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v){ //位置補正（使用前にMotorControl(0,0,0,0)を入れる）
+
+   double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
+   double out;
+   
+   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);
+    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;
+    
+   }
+   
+   while(1){
+    
+     now_angle=gyro.getAngle();
+       
+             out = 10 * (tgt_angle - now_angle);
+
+                if(out > 300) {  //0～179°のときは時計回りに回転
+                     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);
+                }
+
+     if(tgt_angle - 2 < now_angle && now_angle < tgt_angle + 2) break; // 目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+   }
+   MotorControl(0,0,0,0);
+}
+