User.cpp

00001 #include "Utils.h"
00002 #include "USBHost.h"
00003 #include "hci.h"
00004 #include "ps3.h"
00005 #include "User.h"
00006 #include "mbed.h"
00007 #define _USE_MATH_DEFINES
00008 
00009 #include "math.h"
00010 
00011 #define Pi 3.14159
00012 int RSX,RSY,LSX,LSY,BSU,BSL;
00013 //これより下に関数外に書く要素を記入する
00014 //BUS通信用
00015 BusOut out(p5,p6,p7,p8);
00016 int num;
00017 
00018 //オムニホイール
00019 
00020 /*　　　正転の向き
00021 　　　　ｌ↙　　　↖ｆ　　　　
00022 　　　　　　　　　　　　　　
00023 　　　　　 　→
00024 　　　　　　ｒ　　　　　　 */
00025 PwmOut motor_f_1(p21);
00026 PwmOut motor_f_2(p22);
00027 PwmOut motor_l_1(p23);
00028 PwmOut motor_l_2(p24);
00029 PwmOut motor_r_1(p25);
00030 PwmOut motor_r_2(p26);
00031 
00032 double fai=60;//φ
00033 //個体差で出力調整
00034 double power_f=0.32;
00035 double power_l=0.32;
00036 double power_r=0.32;
00037 
00038 double M1;
00039 double M2;
00040 double M3;
00041 
00042 double accel=1.3;
00043 //ジョイスティックの中心座標
00044 double center=127;
00045 
00046 //ジョイスティック閾値
00047 double delta=90;
00048 double bound_p=center+delta;
00049 double bound_m=center-delta;
00050 
00051 //回転の比
00052  //移動中の回転
00053 double roll_spd=0.4;
00054  //横移動中の回転
00055 double roll_spd2=0.1;
00056  //横移動のスピード
00057 double yoko_spd=0.9;
00058 
00059 //モーターの動作
00060 void motor_act()
00061 {
00062     //絶対値をつける関数
00063     if(M1 >=0) {
00064         motor_f_1=M1;
00065         motor_f_2=0;
00066     } else {
00067         motor_f_1=0;
00068         motor_f_2=-M1;
00069     }
00070     if(M2 >=0) {
00071         motor_l_1=M2;
00072         motor_l_2=0;
00073     } else {
00074         motor_l_1=0;
00075         motor_l_2=-M2;
00076     }
00077     if(M3 >=0) {
00078         motor_r_1=M3;
00079         motor_r_2=0;
00080     } else {
00081         motor_r_1=0;
00082         motor_r_2=-M3;
00083     }
00084 }
00085 //ジョイスティック入力値の偏角
00086 double sita;
00087 //関数代入用の角度調整
00088 double sita_2;
00089 
00090 void UserLoopSetting()
00091 {
00092     motor_f_1.period_us(100);
00093     motor_f_2.period_us(100);
00094     motor_l_1.period_us(100);
00095     motor_l_2.period_us(100);
00096     motor_r_1.period_us(100);
00097     motor_r_2.period_us(100);
00098 }
00099 
00100 void UserLoop(char n,const u8* data)
00101 {
00102     u16 ButtonState;
00103     if(n==0) { //有線Ps3USB.cpp
00104         RSX = ((ps3report*)data)->RightStickX;
00105         RSY = ((ps3report*)data)->RightStickY;
00106         LSX = ((ps3report*)data)->LeftStickX;
00107         LSY = ((ps3report*)data)->LeftStickY;
00108         BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
00109         BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
00110         //ボタンの処理
00111         ButtonState =  ((ps3report*)data)->ButtonState;
00112     } else {//無線TestShell.cpp
00113         RSX = ((ps3report*)(data + 1))->RightStickX;
00114         RSY = ((ps3report*)(data + 1))->RightStickY;
00115         LSX = ((ps3report*)(data + 1))->LeftStickX;
00116         LSY = ((ps3report*)(data + 1))->LeftStickY;
00117         BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
00118         BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
00119         //ボタンの処理
00120         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
00121     }
00122     //ここより下にプログラムを書く
00123     //BUS用プログラム
00124     int flag=0;
00125     num=0;
00126     if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く
00127         num = 1;
00128         flag=flag+1;
00129     }
00130     if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く
00131         num = 2;
00132         flag=flag+1;
00133     }
00134     if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く
00135         num = 3;
00136         flag=flag+1;
00137     }
00138     if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く
00139         num = 4;
00140         flag=flag+1;
00141     }
00142     if((ButtonState >> BUTTONUP)&1 == 1) { //対応するボタンを書く
00143         num = 5;
00144         flag=flag+1;
00145     }
00146     if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く
00147         num = 6;
00148         flag=flag+1;
00149     }
00150     if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く
00151         num = 7;
00152         flag=flag+1;
00153     }
00154     if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く
00155         num = 8;
00156         flag=flag+1;
00157     }
00158     if(flag >=2) {
00159         num=0;
00160     }
00161     out=num;
00162     printf("%d\r\n",num);
00163     //オムニホイールのプログラム
00164 
00165     if((ButtonState >> BUTTONRANALOG)&1 == 1) {//押し込みで速度変化
00166         roll_spd=0.8;
00167     } else {
00168         roll_spd=0.4;
00169     }
00170     if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
00171         M1=0;
00172         M2=0;
00173         M3=0;
00174         if(RSX>=bound_p && RSX<=255) {//右スティック右
00175             M1=power_f*roll_spd;
00176             M2=power_l*roll_spd;
00177             M3=power_r*roll_spd;
00178         } else if(RSX>=0 && RSX<=bound_m) {//右スティック左
00179             M1=-1.0*power_f*roll_spd;
00180             M2=-1.0*power_l*roll_spd;
00181             M3=-1.0*power_r*roll_spd;
00182         }
00183         sita=0;
00184 
00185     } else {//左スティック全方向
00186         sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
00187         sita_2=90-sita;
00188         M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
00189         M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
00190         M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
00191         if(RSX>=bound_p && RSX<=255) {//進行中の右スティック右
00192             M1=M1+power_f*roll_spd;
00193             M2=M2+power_l*roll_spd;
00194             M3=M3+power_r*roll_spd;
00195         } else if(RSX>=0 && RSX<=bound_m) {//進行中の右スティック左
00196             M1=M1+-1.0*power_f*roll_spd;
00197             M2=M2+-1.0*power_l*roll_spd;
00198             M3=M3+-1.0*power_r*roll_spd;
00199         }
00200     }
00201     //真横移動
00202     if((ButtonState >> BUTTONLEFT)&1 == 1) { //十字キー左
00203         sita = 180;
00204         sita_2=90-sita;
00205         M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
00206         M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
00207         M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
00208         if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
00209             M1=M1+power_f*roll_spd2;
00210             M2=M2+power_l*roll_spd2;
00211             M3=M3+power_r*roll_spd2;
00212         } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
00213             M1=M1+-1.0*power_f*roll_spd2;
00214             M2=M2+-1.0*power_l*roll_spd2;
00215             M3=M3+-1.0*power_r*roll_spd2;
00216         }
00217     }
00218     if((ButtonState >> BUTTONRIGHT)&1 == 1) { //十字キー右
00219         sita = 0;
00220         sita_2=90-sita;
00221         M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
00222         M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
00223         M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
00224         if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
00225             M1=M1+power_f*roll_spd2;
00226             M2=M2+power_l*roll_spd2;
00227             M3=M3+power_r*roll_spd2;
00228         } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
00229             M1=M1+-1.0*power_f*roll_spd2;
00230             M2=M2+-1.0*power_l*roll_spd2;
00231             M3=M3+-1.0*power_r*roll_spd2;
00232         }
00233     }
00234     motor_act();
00235 
00236 
00237     printf("motor_f:%.4f\t\motor_l:%.4f\t\motor_r:%.4f\t\sita:%f\r\n",M1,M2,M3,sita);
00238 
00239 }