Diff: manual/manual.cpp

Revision:

6:26724c287387

Parent:

0:c61c6e4775ca

--- a/manual/manual.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/manual/manual.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -11,7 +11,7 @@
 
 #define PI 3.141592
 
-int id1_value[6]={0};
+int id1_value[7]= {0};
 
 //-----手動用の変数宣言--------------------------------------------------------------------------//
 int stick_theta;  //ジョイスティックの角度
@@ -20,48 +20,49 @@
 int manual_rout;  //旋回速度
 
 void CalManualOut(int v,int r_out)  //vはθ方向の速度、r_outは旋回速度(正の値)
-                                 //PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる
+//PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる
 {
     stick_theta = (short)((id1_value[1]<<8) | id1_value[2]);
     //debug_printf("stick = %d\n\r",stick_theta);
-    
+
     //ジョイスティック方向の速度をフィールド座標系の速度に変換
-    manual_xout = v * sin(PI*stick_theta/180);  
+    manual_xout = v * sin(PI*stick_theta/180);
     manual_yout = -v * cos(PI*stick_theta/180);
-    
+
     //フィールド座標系の速度を機体座標系の速度に変換
     manual_realxout = manual_xout * cos(PI*now_angle/180) + manual_yout * sin(PI*now_angle/180);
     manual_realyout = -manual_xout * sin(PI*now_angle/180) + manual_yout * cos(PI*now_angle/180);
-    
-    if(id1_value[4] == 1){  //旋回速度を代入
+
+    if(id1_value[4] == 1) { //旋回速度を代入
         manual_rout = 0;
-    }else if(id1_value[4] == 2){
+    } else if(id1_value[4] == 2) {
         manual_rout = r_out;
-    }else if(id1_value[4] == 3){
+    } else if(id1_value[4] == 3) {
         manual_rout = -r_out;
     }
-    
-    
+
+
 }
 
-void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r){
-    
+void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r)
+{
+
     calc_gyro();
 
-    if(id1_value[3]==1){  //BOTTONR1押したら減速
+    if(id1_value[3]==1) { //BOTTONR1押したら減速
         CalManualOut(slow_v,slow_r);
-    }else{
+    } else {
         CalManualOut(fast_v,fast_r);
     }
-                
-    if(id1_value[5] == 1){  //ニュートラルポジションなら出力0
+
+    if(id1_value[5] == 1) { //ニュートラルポジションなら出力0
         output(0,0,0,0);
         base(manual_rout,manual_rout,manual_rout,manual_rout,4095);
-    }else{
+    } else {
         CalMotorOut(manual_realxout,manual_realyout,0);
         base(GetMotorOut(0)+manual_rout,GetMotorOut(1)+manual_rout,GetMotorOut(2)+manual_rout,GetMotorOut(3)+manual_rout,4095);
     }
-                
+
     MaxonControl(m1,m2,m3,m4);
     debug_printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle);
 }
\ No newline at end of file