![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
3/2
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
manual/manual.cpp@5:6cebe1c458a9, 2019-03-02 (annotated)
- Committer:
- yuki0701
- Date:
- Sat Mar 02 07:18:38 2019 +0000
- Revision:
- 5:6cebe1c458a9
- Parent:
- 0:c61c6e4775ca
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
la00noix | 0:c61c6e4775ca | 1 | #include "EC.h" |
la00noix | 0:c61c6e4775ca | 2 | #include "R1370P.h" |
la00noix | 0:c61c6e4775ca | 3 | #include "move4wheel.h" |
la00noix | 0:c61c6e4775ca | 4 | #include "mbed.h" |
la00noix | 0:c61c6e4775ca | 5 | #include "math.h" |
la00noix | 0:c61c6e4775ca | 6 | #include "PathFollowing.h" |
la00noix | 0:c61c6e4775ca | 7 | #include "movement.h" |
la00noix | 0:c61c6e4775ca | 8 | #include "maxonsetting.h" |
la00noix | 0:c61c6e4775ca | 9 | #include "manual.h" |
la00noix | 0:c61c6e4775ca | 10 | #include "can.h" |
la00noix | 0:c61c6e4775ca | 11 | |
la00noix | 0:c61c6e4775ca | 12 | #define PI 3.141592 |
la00noix | 0:c61c6e4775ca | 13 | |
la00noix | 0:c61c6e4775ca | 14 | int id1_value[6]={0}; |
la00noix | 0:c61c6e4775ca | 15 | |
la00noix | 0:c61c6e4775ca | 16 | //-----手動用の変数宣言--------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 17 | int stick_theta; //ジョイスティックの角度 |
la00noix | 0:c61c6e4775ca | 18 | int manual_xout,manual_yout; //フィールド座標系のx,y方向の速度 |
la00noix | 0:c61c6e4775ca | 19 | int manual_realxout,manual_realyout; //機体座標系のx,y方向の速度 |
la00noix | 0:c61c6e4775ca | 20 | int manual_rout; //旋回速度 |
la00noix | 0:c61c6e4775ca | 21 | |
la00noix | 0:c61c6e4775ca | 22 | void CalManualOut(int v,int r_out) //vはθ方向の速度、r_outは旋回速度(正の値) |
la00noix | 0:c61c6e4775ca | 23 | //PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる |
la00noix | 0:c61c6e4775ca | 24 | { |
la00noix | 0:c61c6e4775ca | 25 | stick_theta = (short)((id1_value[1]<<8) | id1_value[2]); |
la00noix | 0:c61c6e4775ca | 26 | //debug_printf("stick = %d\n\r",stick_theta); |
la00noix | 0:c61c6e4775ca | 27 | |
la00noix | 0:c61c6e4775ca | 28 | //ジョイスティック方向の速度をフィールド座標系の速度に変換 |
la00noix | 0:c61c6e4775ca | 29 | manual_xout = v * sin(PI*stick_theta/180); |
la00noix | 0:c61c6e4775ca | 30 | manual_yout = -v * cos(PI*stick_theta/180); |
la00noix | 0:c61c6e4775ca | 31 | |
la00noix | 0:c61c6e4775ca | 32 | //フィールド座標系の速度を機体座標系の速度に変換 |
la00noix | 0:c61c6e4775ca | 33 | manual_realxout = manual_xout * cos(PI*now_angle/180) + manual_yout * sin(PI*now_angle/180); |
la00noix | 0:c61c6e4775ca | 34 | manual_realyout = -manual_xout * sin(PI*now_angle/180) + manual_yout * cos(PI*now_angle/180); |
la00noix | 0:c61c6e4775ca | 35 | |
la00noix | 0:c61c6e4775ca | 36 | if(id1_value[4] == 1){ //旋回速度を代入 |
la00noix | 0:c61c6e4775ca | 37 | manual_rout = 0; |
la00noix | 0:c61c6e4775ca | 38 | }else if(id1_value[4] == 2){ |
la00noix | 0:c61c6e4775ca | 39 | manual_rout = r_out; |
la00noix | 0:c61c6e4775ca | 40 | }else if(id1_value[4] == 3){ |
la00noix | 0:c61c6e4775ca | 41 | manual_rout = -r_out; |
la00noix | 0:c61c6e4775ca | 42 | } |
la00noix | 0:c61c6e4775ca | 43 | |
la00noix | 0:c61c6e4775ca | 44 | |
la00noix | 0:c61c6e4775ca | 45 | } |
la00noix | 0:c61c6e4775ca | 46 | |
la00noix | 0:c61c6e4775ca | 47 | void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r){ |
la00noix | 0:c61c6e4775ca | 48 | |
la00noix | 0:c61c6e4775ca | 49 | calc_gyro(); |
la00noix | 0:c61c6e4775ca | 50 | |
la00noix | 0:c61c6e4775ca | 51 | if(id1_value[3]==1){ //BOTTONR1押したら減速 |
la00noix | 0:c61c6e4775ca | 52 | CalManualOut(slow_v,slow_r); |
la00noix | 0:c61c6e4775ca | 53 | }else{ |
la00noix | 0:c61c6e4775ca | 54 | CalManualOut(fast_v,fast_r); |
la00noix | 0:c61c6e4775ca | 55 | } |
la00noix | 0:c61c6e4775ca | 56 | |
la00noix | 0:c61c6e4775ca | 57 | if(id1_value[5] == 1){ //ニュートラルポジションなら出力0 |
la00noix | 0:c61c6e4775ca | 58 | output(0,0,0,0); |
la00noix | 0:c61c6e4775ca | 59 | base(manual_rout,manual_rout,manual_rout,manual_rout,4095); |
la00noix | 0:c61c6e4775ca | 60 | }else{ |
la00noix | 0:c61c6e4775ca | 61 | CalMotorOut(manual_realxout,manual_realyout,0); |
la00noix | 0:c61c6e4775ca | 62 | base(GetMotorOut(0)+manual_rout,GetMotorOut(1)+manual_rout,GetMotorOut(2)+manual_rout,GetMotorOut(3)+manual_rout,4095); |
la00noix | 0:c61c6e4775ca | 63 | } |
la00noix | 0:c61c6e4775ca | 64 | |
la00noix | 0:c61c6e4775ca | 65 | MaxonControl(m1,m2,m3,m4); |
la00noix | 0:c61c6e4775ca | 66 | debug_printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle); |
la00noix | 0:c61c6e4775ca | 67 | } |