File content as of revision 26:91dd637de4d4:

#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "User.h"
#include "mbed.h"
#define _USE_MATH_DEFINES

#include "math.h"

#define Pi 3.14159
int RSX,RSY,LSX,LSY,BSU,BSL;
//これより下に関数外に書く要素を記入する
//BUS通信用
BusOut out(p5,p6,p7,p8);
int num;

//オムニホイール

/*　　　正転の向き
　　　　ｌ↙　　　↖ｆ　　　　
　　　　　　　　　　　　　　
　　　　　 　→
　　　　　　ｒ　　　　　　 */
PwmOut motor_f_1(p21);
PwmOut motor_f_2(p22);
PwmOut motor_l_1(p23);
PwmOut motor_l_2(p24);
PwmOut motor_r_1(p25);
PwmOut motor_r_2(p26);

double fai=60;//φ
//個体差で出力調整
double power_f=0.32;
double power_l=0.32;
double power_r=0.32;

double M1;
double M2;
double M3;

double accel=1.3;
//ジョイスティックの中心座標
double center=127;

//ジョイスティック閾値
double delta=90;
double bound_p=center+delta;
double bound_m=center-delta;

//回転の比
 //移動中の回転
double roll_spd=0.4;
 //横移動中の回転
double roll_spd2=0.1;
 //横移動のスピード
double yoko_spd=0.9;

//モーターの動作
void motor_act()
{
    //絶対値をつける関数
    if(M1 >=0) {
        motor_f_1=M1;
        motor_f_2=0;
    } else {
        motor_f_1=0;
        motor_f_2=-M1;
    }
    if(M2 >=0) {
        motor_l_1=M2;
        motor_l_2=0;
    } else {
        motor_l_1=0;
        motor_l_2=-M2;
    }
    if(M3 >=0) {
        motor_r_1=M3;
        motor_r_2=0;
    } else {
        motor_r_1=0;
        motor_r_2=-M3;
    }
}
//ジョイスティック入力値の偏角
double sita;
//関数代入用の角度調整
double sita_2;

void UserLoopSetting()
{
    motor_f_1.period_us(100);
    motor_f_2.period_us(100);
    motor_l_1.period_us(100);
    motor_l_2.period_us(100);
    motor_r_1.period_us(100);
    motor_r_2.period_us(100);
}

void UserLoop(char n,const u8* data)
{
    u16 ButtonState;
    if(n==0) { //有線Ps3USB.cpp
        RSX = ((ps3report*)data)->RightStickX;
        RSY = ((ps3report*)data)->RightStickY;
        LSX = ((ps3report*)data)->LeftStickX;
        LSY = ((ps3report*)data)->LeftStickY;
        BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
        BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
        //ボタンの処理
        ButtonState =  ((ps3report*)data)->ButtonState;
    } else {//無線TestShell.cpp
        RSX = ((ps3report*)(data + 1))->RightStickX;
        RSY = ((ps3report*)(data + 1))->RightStickY;
        LSX = ((ps3report*)(data + 1))->LeftStickX;
        LSY = ((ps3report*)(data + 1))->LeftStickY;
        BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
        BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
        //ボタンの処理
        ButtonState =  ((ps3report*)(data + 1))->ButtonState;
    }
    //ここより下にプログラムを書く
    //BUS用プログラム
    int flag=0;
    num=0;
    if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く
        num = 1;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く
        num = 2;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く
        num = 3;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く
        num = 4;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONUP)&1 == 1) { //対応するボタンを書く
        num = 5;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く
        num = 6;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く
        num = 7;
        flag=flag+1;
    }
    if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く
        num = 8;
        flag=flag+1;
    }
    if(flag >=2) {
        num=0;
    }
    out=num;
    printf("%d\r\n",num);
    //オムニホイールのプログラム

    if((ButtonState >> BUTTONRANALOG)&1 == 1) {//押し込みで速度変化
        roll_spd=0.8;
    } else {
        roll_spd=0.4;
    }
    if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
        M1=0;
        M2=0;
        M3=0;
        if(RSX>=bound_p && RSX<=255) {//右スティック右
            M1=power_f*roll_spd;
            M2=power_l*roll_spd;
            M3=power_r*roll_spd;
        } else if(RSX>=0 && RSX<=bound_m) {//右スティック左
            M1=-1.0*power_f*roll_spd;
            M2=-1.0*power_l*roll_spd;
            M3=-1.0*power_r*roll_spd;
        }
        sita=0;

    } else {//左スティック全方向
        sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
        sita_2=90-sita;
        M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
        M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
        M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
        if(RSX>=bound_p && RSX<=255) {//進行中の右スティック右
            M1=M1+power_f*roll_spd;
            M2=M2+power_l*roll_spd;
            M3=M3+power_r*roll_spd;
        } else if(RSX>=0 && RSX<=bound_m) {//進行中の右スティック左
            M1=M1+-1.0*power_f*roll_spd;
            M2=M2+-1.0*power_l*roll_spd;
            M3=M3+-1.0*power_r*roll_spd;
        }
    }
    //真横移動
    if((ButtonState >> BUTTONLEFT)&1 == 1) { //十字キー左
        sita = 180;
        sita_2=90-sita;
        M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
        M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
        M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
        if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
            M1=M1+power_f*roll_spd2;
            M2=M2+power_l*roll_spd2;
            M3=M3+power_r*roll_spd2;
        } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
            M1=M1+-1.0*power_f*roll_spd2;
            M2=M2+-1.0*power_l*roll_spd2;
            M3=M3+-1.0*power_r*roll_spd2;
        }
    }
    if((ButtonState >> BUTTONRIGHT)&1 == 1) { //十字キー右
        sita = 0;
        sita_2=90-sita;
        M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
        M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
        M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
        if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
            M1=M1+power_f*roll_spd2;
            M2=M2+power_l*roll_spd2;
            M3=M3+power_r*roll_spd2;
        } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
            M1=M1+-1.0*power_f*roll_spd2;
            M2=M2+-1.0*power_l*roll_spd2;
            M3=M3+-1.0*power_r*roll_spd2;
        }
    }
    motor_act();


    printf("motor_f:%.4f\t\motor_l:%.4f\t\motor_r:%.4f\t\sita:%f\r\n",M1,M2,M3,sita);

}