ロボステ6期 / Mbed 2 deprecated NHK2020_main_ros1_2_25

２月25日

Dependencies:   uw_28015 mbed ros_lib_kinetic move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

manual/manual.cpp@0:44f9a43e4ab2, 2020-02-25 (annotated)

Committer:
yuki0701
Date:
Tue Feb 25 01:20:43 2020 +0000
Revision:
0:44f9a43e4ab2
a;

Who changed what in which revision?

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