File content as of revision 0:c61c6e4775ca:

#include "EC.h"
#include "R1370P.h"
#include "move4wheel.h"
#include "mbed.h"
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
#include "maxonsetting.h"
#include "manual.h"
#include "can.h"

#define PI 3.141592

int id1_value[6]={0};

//-----手動用の変数宣言--------------------------------------------------------------------------//
int stick_theta;  //ジョイスティックの角度
int manual_xout,manual_yout;  //フィールド座標系のx,y方向の速度
int manual_realxout,manual_realyout;  //機体座標系のx,y方向の速度
int manual_rout;  //旋回速度

void CalManualOut(int v,int r_out)  //vはθ方向の速度、r_outは旋回速度(正の値)
                                 //PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる
{
    stick_theta = (short)((id1_value[1]<<8) | id1_value[2]);
    //debug_printf("stick = %d\n\r",stick_theta);
    
    //ジョイスティック方向の速度をフィールド座標系の速度に変換
    manual_xout = v * sin(PI*stick_theta/180);  
    manual_yout = -v * cos(PI*stick_theta/180);
    
    //フィールド座標系の速度を機体座標系の速度に変換
    manual_realxout = manual_xout * cos(PI*now_angle/180) + manual_yout * sin(PI*now_angle/180);
    manual_realyout = -manual_xout * sin(PI*now_angle/180) + manual_yout * cos(PI*now_angle/180);
    
    if(id1_value[4] == 1){  //旋回速度を代入
        manual_rout = 0;
    }else if(id1_value[4] == 2){
        manual_rout = r_out;
    }else if(id1_value[4] == 3){
        manual_rout = -r_out;
    }
    
    
}

void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r){
    
    calc_gyro();

    if(id1_value[3]==1){  //BOTTONR1押したら減速
        CalManualOut(slow_v,slow_r);
    }else{
        CalManualOut(fast_v,fast_r);
    }
                
    if(id1_value[5] == 1){  //ニュートラルポジションなら出力0
        output(0,0,0,0);
        base(manual_rout,manual_rout,manual_rout,manual_rout,4095);
    }else{
        CalMotorOut(manual_realxout,manual_realyout,0);
        base(GetMotorOut(0)+manual_rout,GetMotorOut(1)+manual_rout,GetMotorOut(2)+manual_rout,GetMotorOut(3)+manual_rout,4095);
    }
                
    MaxonControl(m1,m2,m3,m4);
    debug_printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle);
}