Diff: main.cpp

Revision:

6:14cb400f99f7

Parent:

5:7493649d098b

Child:

7:e269985951bf

--- a/main.cpp	Wed Nov 28 05:41:58 2018 +0000
+++ b/main.cpp	Sat Dec 01 05:17:16 2018 +0000
@@ -164,7 +164,7 @@
 
                 calc_xy();
 
-                XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+                XYRmotorout(1,plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
                 CalMotorOut(x_out,y_out,r_out);  //move4wheel内のモーター番号定義または成分分解が違うかも？
                 //CalMotorOut(plotvx[t], plotvy[t],0);
 
@@ -195,7 +195,7 @@
 
                 calc_xy();
 
-                XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+                XYRmotorout(1,plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
                 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++;
@@ -317,11 +317,11 @@
     }
 
     while(1) {
-        now_angle=gyro.getAngle();  //ジャイロの値読み込み
+        //now_angle=gyro.getAngle();  //ジャイロの値読み込み
 
         calc_xy();
 
-        XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+        XYRmotorout(1,plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
         CalMotorOut(x_out,y_out,r_out);  //move4wheel内のモーター番号定義または成分分解が違うかも？
         //CalMotorOut(plotvx[t], plotvy[t],0);
 
@@ -343,7 +343,7 @@
 
 
 
-void gogo_straight(double x1_point,double y1_point,double x2_point,double y2_point)   //直線運動プログラム（引数：出発地点の座標（x,y）、目標地点の座標(x,y)）
+void gogo_straight(int type, double x1_point,double y1_point,double x2_point,double y2_point)   //直線運動プログラム（引数：type→(1:減速なし/2:減速あり)、出発地点の座標（x,y）、目標地点の座標(x,y)）
 {
     while (1) {
 
@@ -354,7 +354,7 @@
 
 
         //Debug_Control();
-        XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out);
+        XYRmotorout(type,x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out);
         //printf("x=%lf, y=%lf, r=%lf",x_out, y_out,r_out);
 
         CalMotorOut(x_out,y_out,r_out);
@@ -406,6 +406,8 @@
     }
 }
 
+
+
 //////////////////////////////////////////////////////////////以下main文////////////////////////////////////////////////////////////////////////
 
 int main()
@@ -419,13 +421,39 @@
     now_x=start_x;
     now_y=start_y;
 
-    purecurve2(8,0,0,1000,-1000,9);
-    //purecurve2(8,0,0,1000,-500,9);
-    //purecurve(1,1000,0,1000,9,1000);
-    // MotorControl(0,0,0,0);
+    /*set_target_angle(0);
+    purecurve2(1,0,0,1000,1000,9);
+    MotorControl(0,0,0,0);*/
+
+    /*  set_target_angle(0);
+        while(1){
+        purecurve2(7,0,0,500,-500,9);
+        MotorControl(0,0,0,0);
+        purecurve2(1,500,-500,1000,0,9);
+        MotorControl(0,0,0,0);
+        purecurve2(3,1000,0,500,500,9);
+        MotorControl(0,0,0,0);
+        purecurve2(5,500,500,0,0,9);
+        MotorControl(0,0,0,0);
+    }*/
 
+   int a=0;
+    while(1) {
+        set_target_angle(a);
+        gogo_straight(2,0,0,0,-1500);
+        MotorControl(0,0,0,0);
+        wait(1);
 
-    //gogo_straight(0,0,1500,0);
+        a=a+90;
+
+        set_target_angle(a);
+        gogo_straight(2,0,-1500,0,0);
+        MotorControl(0,0,0,0);
+        wait(1);
+
+        a=a+90;
+
+    }
 
 }
 ///////////////////////////////////////////////////////////////////////以下マクソン関連///////////////////////////////////////////////////////////////////////////
@@ -450,9 +478,9 @@
     EC2.setDiameter_mm(48);  //測定輪半径
 //-----PathFollowingのパラメーター設定-----//
     set_p_out(800);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
-    q_setPDparam(0.1,0.1);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
+    q_setPDparam(5,0.1);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
     r_setPDparam(10,0.1);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
-    set_r_out(500);  //旋回時の最大出力値設定関数
+    set_r_out(600);  //旋回時の最大出力値設定関数
     //  set_target_angle(0);  //機体目標角度設定関数
 
 #ifdef DEBUG_MODE