![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
主導機 mbed用のプログラムです
Fork of F3RC_syudou_master by
User.cpp
- Committer:
- yuto17320508
- Date:
- 2017-08-23
- Revision:
- 18:2579c275ef57
- Parent:
- 17:c5c41fcf316e
- Child:
- 19:a3f57c9833b6
File content as of revision 18:2579c275ef57:
#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; //これより下に関数外に書く要素を記入する //spi通信用 SPI spi(p5,p6,p7); DigitalOut cs(p8); //オムニホイール /* 正転の向き ← ↘ ↗ */ 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=1/3*Pi;//φ double power_f=1; double power_l=1; double power_r=1; double M1; double M2; double M3; 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() { spi.format(8,3); spi.frequency(1000000); motor_f_1.period_us(50); motor_f_2.period_us(50); motor_l_1.period_us(50); motor_l_2.period_us(50); motor_r_1.period_us(50); motor_r_2.period_us(50); } 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; } //ここより下にプログラムを書く //spi通信用プログラム int a,b,c,d,e,f,g,h; int send = 0; if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン a = 1; } else { a = 0; } if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン b = 2; } else { b = 0; } if((ButtonState >> BUTTONL1)&1 == 1) {//対応するボタンを書く(今回L1ボタン c = 4; } else { c = 0; } if((ButtonState >> BUTTONL2)&1 == 1) {//対応するボタンを書く(今回L2ボタン d = 8; } else { d = 0; } if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン e = 16; } else { e = 0; } if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン f = 32; } else { f = 0; } if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン g = 64; } else { g = 0; } if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン h = 128; } else { h = 0; } send = a+b+c+d+e+f+g+h; cs = 0; spi. write(send); cs = 1; printf("%d\r\n",send); if(LSX>=150 && LSX<=255 && LSY>=150 && LSY<=255) { //第一象限 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSX>=0 && LSX<=80 && LSY>=150 && LSY<=255) { //第二象限 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSX>=0 && LSX<=80 && LSY>=0 && LSY<=80) { //第三象限 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSX>=150 && LSX<=255 && LSY>=0 && LSY<=80) { //第四象限 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSX==255) { sita = 0; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSY==255) { sita = 90; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSX==0) { sita = 180; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else if(LSY==0) { sita = 270; sita_2=90-sita; M1=sin((sita_2-fai)*Pi/180)*power_f; M2=sin((sita_2-fai+240)*Pi/180)*power_l; M3=sin((sita_2-fai+120)*Pi/180)*power_r; motor_act(); } else { motor_f_1=0; motor_f_2=0; motor_l_1=0; motor_l_2=0; motor_r_1=0; motor_r_2=0; } printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita); }