Diff: main.cpp

Revision:

7:e269985951bf

Parent:

6:14cb400f99f7

diff -r 14cb400f99f7 -r e269985951bf main.cpp
--- a/main.cpp	Sat Dec 01 05:17:16 2018 +0000
+++ b/main.cpp	Tue Dec 04 13:24:56 2018 +0000
@@ -100,7 +100,7 @@
     m4=FR;
 }
 
-void base(double FL,double BL,double BR,double FR,double Max)
+/*void base(double FL,double BL,double BR,double FR,double Max)
 //いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
 //マクソンは-4095～4095だからMax=4095にする//最速スピードを出すための関数になってる
 {
@@ -108,8 +108,23 @@
     else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL));
     else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR));
     else                                                               output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR));
+}*/
+
+void base(double FL,double BL,double BR,double FR,double Max)
+//いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
+//マクソンは-4095～4095だからMax=4095にする//最速スピードを出すための関数になってる
+{
+    if(fabs(FL)>=Max||fabs(BL)>=Max||fabs(BR)>=Max||fabs(FR)>=Max) {
+
+        if     (fabs(FL)>=fabs(BL)&&fabs(FL)>=fabs(BR)&&fabs(FL)>=fabs(FR))output(Max*FL/fabs(FL),Max*BL/fabs(FL),Max*BR/fabs(FL),Max*FR/fabs(FL));
+        else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL));
+        else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR));
+        else                                                               output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR));
+    } else {
+        output(FL,BL,BR,FR);
+    }
+
 }
-
 void calc_xy()
 {
     now_angle=gyro.getAngle();  //ジャイロの値読み込み
@@ -325,7 +340,7 @@
         CalMotorOut(x_out,y_out,r_out);  //move4wheel内のモーター番号定義または成分分解が違うかも？
         //CalMotorOut(plotvx[t], plotvy[t],0);
 
-        //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out);
+        debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out);
         //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
 
         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000);  //m1～m4に代入
@@ -335,7 +350,7 @@
 
         MotorControl(m1,m2,m3,m4);  //出力
 
-        debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f, angle = %f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle);
+        //debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f, angle = %f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle);
 
     }
 }
@@ -355,7 +370,7 @@
 
         //Debug_Control();
         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);
+        //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);
         //printf("out1=%lf, out2=%lf, out3=%lf, out4=%lf\n",GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
@@ -437,21 +452,23 @@
         MotorControl(0,0,0,0);
     }*/
 
-   int a=0;
+    int a=0;
     while(1) {
         set_target_angle(a);
-        gogo_straight(2,0,0,0,-1500);
+        gogo_straight(1,0,0,0,-1200);
+        gogo_straight(2,0,-1200,0,-1500);
         MotorControl(0,0,0,0);
         wait(1);
 
-        a=a+90;
+        //a=a+90;
 
         set_target_angle(a);
-        gogo_straight(2,0,-1500,0,0);
+        gogo_straight(1,0,-1500,0,-300);
+        gogo_straight(2,0,-300,0,0);
         MotorControl(0,0,0,0);
         wait(1);
 
-        a=a+90;
+        //a=a+90;
 
     }
 
@@ -477,7 +494,7 @@
     EC1.setDiameter_mm(48);
     EC2.setDiameter_mm(48);  //測定輪半径
 //-----PathFollowingのパラメーター設定-----//
-    set_p_out(800);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
+    set_p_out(1000);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
     q_setPDparam(5,0.1);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
     r_setPDparam(10,0.1);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
     set_r_out(600);  //旋回時の最大出力値設定関数