Diff: main.cpp

Revision:

8:84d10508818a

Parent:

7:e269985951bf

--- a/main.cpp	Tue Dec 04 13:24:56 2018 +0000
+++ b/main.cpp	Wed Dec 05 07:02:46 2018 +0000
@@ -149,10 +149,11 @@
 //通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
 //ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね
 
-void purecurve(int type,double X,double Y,double r,int theta,double speed/*,double v*/)
+void purecurve(int type,double X,double Y,double r,int theta,double speed1,double speed2)
 {
 //正面を変えずに円弧を描いて90°曲がる
 //X=円弧の中心座標、Y=円弧の中心座標、r=円弧の半径、theta=plotの間隔(0～90°)、v=目標速度
+//speed1:初速度, speed2:目標速度
 
     int s;
     int t = 0;
@@ -175,11 +176,11 @@
             }
 
             while(1) {
-                now_angle=gyro.getAngle();  //ジャイロの値読み込み
+                //now_angle=gyro.getAngle();  //ジャイロの値読み込み
 
                 calc_xy();
 
-                XYRmotorout(1,plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+                XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed1,speed2);
                 CalMotorOut(x_out,y_out,r_out);  //move4wheel内のモーター番号定義または成分分解が違うかも？
                 //CalMotorOut(plotvx[t], plotvy[t],0);
 
@@ -206,11 +207,11 @@
 
             while(1) {
 
-                now_angle=gyro.getAngle();  //ジャイロの値読み込み
+                //now_angle=gyro.getAngle();  //ジャイロの値読み込み
 
                 calc_xy();
 
-                XYRmotorout(1,plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+                XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed1,speed2);
                 CalMotorOut(x_out,y_out,r_out);
                 base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000);
                 if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
@@ -221,7 +222,7 @@
     }
 }
 
-void purecurve2(int type,double point_x1,double point_y1,double point_x2,double point_y2,int theta/*,double speed,double v*/)
+void purecurve2(int type,double point_x1,double point_y1,double point_x2,double point_y2,int theta,double speed1,double speed2)
 {
 //正面を変えずに円弧を描いて90°曲がる
 //point_x1,point_y1=出発地点の座標  point_x2,point_x2=目標地点の座標,theta=plotの間隔(0～90°)、v=目標速度
@@ -336,7 +337,7 @@
 
         calc_xy();
 
-        XYRmotorout(1,plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+        XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed1,speed2);
         CalMotorOut(x_out,y_out,r_out);  //move4wheel内のモーター番号定義または成分分解が違うかも？
         //CalMotorOut(plotvx[t], plotvy[t],0);
 
@@ -358,7 +359,8 @@
 
 
 
-void gogo_straight(int type, double x1_point,double y1_point,double x2_point,double y2_point)   //直線運動プログラム（引数：type→(1:減速なし/2:減速あり)、出発地点の座標（x,y）、目標地点の座標(x,y)）
+void gogo_straight(double x1_point,double y1_point,double x2_point,double y2_point,double speed1,double speed2)   //直線運動プログラム
+//引数:出発地点の座標（x,y）、目標地点の座標(x,y)、初速度（speed1）、目標速度（speed2）
 {
     while (1) {
 
@@ -369,7 +371,7 @@
 
 
         //Debug_Control();
-        XYRmotorout(type,x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out);
+        XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2);
         //printf("x = %f, y = %f,angle = %f,x_out=%lf, y_out=%lf, r_out=%lf\n\r",now_x,now_y,now_angle,x_out, y_out,r_out);
 
         CalMotorOut(x_out,y_out,r_out);
@@ -379,13 +381,10 @@
         //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m1,m2,m3,m4);
         MotorControl(m1,m2,m3,m4);
 
-
-
         if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0) break;
 
     }
 
-    MotorControl(0,0,0,0);
 }
 
 void go_straight(int type,double goal_x,double goal_y,double speed,double front)   //移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
@@ -455,16 +454,16 @@
     int a=0;
     while(1) {
         set_target_angle(a);
-        gogo_straight(1,0,0,0,-1200);
-        gogo_straight(2,0,-1200,0,-1500);
+        gogo_straight(0,0,0,-1200,1000,1000);
+        gogo_straight(0,-1200,0,-1500,1000,1000);
         MotorControl(0,0,0,0);
         wait(1);
 
         //a=a+90;
 
         set_target_angle(a);
-        gogo_straight(1,0,-1500,0,-300);
-        gogo_straight(2,0,-300,0,0);
+        gogo_straight(0,-1500,0,-300,1000,1000);
+        gogo_straight(0,-300,0,0,1000,1000);
         MotorControl(0,0,0,0);
         wait(1);
 
@@ -494,7 +493,7 @@
     EC1.setDiameter_mm(48);
     EC2.setDiameter_mm(48);  //測定輪半径
 //-----PathFollowingのパラメーター設定-----//
-    set_p_out(1000);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
+    //set_p_out(1000);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
     q_setPDparam(5,0.1);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
     r_setPDparam(10,0.1);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
     set_r_out(600);  //旋回時の最大出力値設定関数