File content as of revision 2:820dcd23c8e3:

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

#define PI 3.141592

int id1_value[7]= {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);
    printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle);
}