2月25日

Dependencies:   uw_28015 mbed ros_lib_kinetic move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers manual.cpp Source File

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 }