Diff: manual/manual.cpp

Revision:

0:b87fd8dd4322

Child:

1:26fc1b2f1c42

diff -r 000000000000 -r b87fd8dd4322 manual/manual.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/manual/manual.cpp	Tue Mar 19 13:20:23 2019 +0000
@@ -0,0 +1,68 @@
+#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[7]= {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