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