Diff: manual/manual.cpp

Revision:

0:c61c6e4775ca

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/manual/manual.cpp	Wed Feb 13 03:02:19 2019 +0000
@@ -0,0 +1,67 @@
+#include "EC.h"
+#include "R1370P.h"
+#include "move4wheel.h"
+#include "mbed.h"
+#include "math.h"
+#include "PathFollowing.h"
+#include "movement.h"
+#include "maxonsetting.h"
+#include "manual.h"
+#include "can.h"
+
+#define PI 3.141592
+
+int id1_value[6]={0};
+
+//-----手動用の変数宣言--------------------------------------------------------------------------//
+int stick_theta;  //ジョイスティックの角度
+int manual_xout,manual_yout;  //フィールド座標系のx,y方向の速度
+int manual_realxout,manual_realyout;  //機体座標系のx,y方向の速度
+int manual_rout;  //旋回速度
+
+void CalManualOut(int v,int r_out)  //vはθ方向の速度、r_outは旋回速度(正の値)
+                                 //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_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){  //旋回速度を代入
+        manual_rout = 0;
+    }else if(id1_value[4] == 2){
+        manual_rout = r_out;
+    }else if(id1_value[4] == 3){
+        manual_rout = -r_out;
+    }
+    
+    
+}
+
+void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r){
+    
+    calc_gyro();
+
+    if(id1_value[3]==1){  //BOTTONR1押したら減速
+        CalManualOut(slow_v,slow_r);
+    }else{
+        CalManualOut(fast_v,fast_r);
+    }
+                
+    if(id1_value[5] == 1){  //ニュートラルポジションなら出力0
+        output(0,0,0,0);
+        base(manual_rout,manual_rout,manual_rout,manual_rout,4095);
+    }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