2月25日
Dependencies: uw_28015 mbed ros_lib_kinetic move4wheel2 EC CruizCore_R6093U CruizCore_R1370P
manual.cpp
00001 #include "EC.h" 00002 #include "R1370P.h" 00003 #include "move4wheel.h" 00004 #include "mbed.h" 00005 #include "math.h" 00006 #include "PathFollowing.h" 00007 #include "movement.h" 00008 #include "manual.h" 00009 #include "can.h" 00010 00011 #define PI 3.141592 00012 00013 int id1_value[7]= {0}; 00014 00015 //-----手動用の変数宣言--------------------------------------------------------------------------// 00016 int stick_theta; //ジョイスティックの角度 00017 int manual_xout,manual_yout; //フィールド座標系のx,y方向の速度 00018 int manual_realxout,manual_realyout; //機体座標系のx,y方向の速度 00019 int manual_rout; //旋回速度 00020 00021 void CalManualOut(int v,int r_out) //vはθ方向の速度、r_outは旋回速度(正の値) 00022 //PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる 00023 { 00024 stick_theta = (short)((id1_value[1]<<8) | id1_value[2]); 00025 //debug_printf("stick = %d\n\r",stick_theta); 00026 00027 //ジョイスティック方向の速度をフィールド座標系の速度に変換 00028 manual_xout = v * sin(PI*stick_theta/180); 00029 manual_yout = -v * cos(PI*stick_theta/180); 00030 00031 //フィールド座標系の速度を機体座標系の速度に変換 00032 manual_realxout = manual_xout * cos(PI*now_angle/180) + manual_yout * sin(PI*now_angle/180); 00033 manual_realyout = -manual_xout * sin(PI*now_angle/180) + manual_yout * cos(PI*now_angle/180); 00034 00035 if(id1_value[4] == 1) { //旋回速度を代入 00036 manual_rout = 0; 00037 } else if(id1_value[4] == 2) { 00038 manual_rout = r_out; 00039 } else if(id1_value[4] == 3) { 00040 manual_rout = -r_out; 00041 } 00042 00043 00044 } 00045 00046 void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r) 00047 { 00048 00049 calc_gyro(); 00050 00051 if(id1_value[3]==1) { //BOTTONR1押したら減速 00052 CalManualOut(slow_v,slow_r); 00053 } else { 00054 CalManualOut(fast_v,fast_r); 00055 } 00056 00057 if(id1_value[5] == 1) { //ニュートラルポジションなら出力0 00058 output(0,0,0,0); 00059 base(manual_rout,manual_rout,manual_rout,manual_rout,4095); 00060 } else { 00061 CalMotorOut(manual_realxout,manual_realyout,0); 00062 base(GetMotorOut(0)+manual_rout,GetMotorOut(1)+manual_rout,GetMotorOut(2)+manual_rout,GetMotorOut(3)+manual_rout,4095); 00063 } 00064 00065 //MaxonControl(m1,m2,m3,m4); 00066 // debug_printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle); 00067 printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle); 00068 }
Generated on Fri Jul 15 2022 09:53:44 by
1.7.2