![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
3/2
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
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