a
Dependencies: mbed ikarashiMDC PS3
Diff: main.cpp
- Revision:
- 0:193dcda90d39
diff -r 000000000000 -r 193dcda90d39 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 07 07:40:22 2019 +0000 @@ -0,0 +1,109 @@ +#include"mbed.h" +#include"ikarashiMDC.h" +#include"PS3.h" +#define PI 3.14159265 +Serial serial(PC_6,PC_7); +DigitalOut serialcontrol(D2); + + ikarashiMDC ikarashi[] { + ikarashiMDC(&serialcontrol,2,0,SM,&serial), + ikarashiMDC(&serialcontrol,2,1,SM,&serial), + ikarashiMDC(&serialcontrol,2,2,SM,&serial), + ikarashiMDC(&serialcontrol,2,3,SM,&serial), + ikarashiMDC(&serialcontrol,1,0,SM,&serial) +}; + +PS3 ps3(PC_10,PC_11); +Serial pc(USBTX,USBRX,115200); + +double way(double x,double y,double direction) +{ + double atanval; + atanval = atan2(y,x); + return sin(atanval - direction); +} + +int main() +{ + serial.baud(115200); + ikarashi[0].braking = true; + int b[12],stick[4],trigger[2],neutral[4],count = 0; + double X,Y,acc[5]; + /*ジョイスティック ニュートラル設定*/ + do{ + for(int i = 0;i < 4;i++) { + neutral[i] = ps3.getStick(i); + pc.printf("%4d",neutral[i]); + } + count++; + }while(count <= 115200); + for(int i = 0;i < 4;i++) { + neutral[i] = ps3.getStick(i); + pc.printf("%4d",neutral[i]); + } + while(1){ + /*ボタン0=上 1=下 2=左 3=右 4=L1 5=R1 6=△ 7=✖ + 8=🔲 9=◯ 10=L3 11=R3*/ + for(int i=0;i < 12;i++) { + b[i] = ps3.getButton(i); + pc.printf("%d",b[i]); + } + /*ジョイスティック*/ + for(int i = 0;i < 4;i++){ + stick[i] = ps3.getStick(i); + pc.printf("%4d\r\n",stick[i]); + } + + for(int i = 0;i < 2;i++) { + trigger[i] = ps3.getTrigger(i); + pc.printf("%4d\r\n",trigger[i]); + } + X = stick[0] - neutral[0]; + Y = neutral[1] - stick[1]; + if( abs(X) <= 20 && abs(Y) <= 20){ + acc[0] = 0; + acc[1] = 0; + acc[2] = 0; + }else { + acc[0] = way(X,Y,PI/6); + acc[1] = way(X,Y,PI*3/2); + acc[2] = way(X,Y,PI/5/6); + } + if(b[4] == 1 && abs(X) <= 20 && abs(Y) <= 20){ + acc[0] = 1; + acc[1] = 1; + acc[2] = 1; + } + if(b[5] == 1 && abs(X) <= 20 && abs(Y) <= 20){ + acc[0] = -1; + acc[1] = -1; + acc[2] = -1; + } + if(b[7] == 1){ + acc[0] = 0; + acc[1] = 0; + acc[2] = 0; + } + if(trigger[0] == 0 && trigger[1] == 0) { + acc[3] = 0; + } + if(trigger[0] >= 1 && trigger[1] == 0) { + acc[3] = 0.4; + } + if(trigger[0] == 0 && trigger[1] >= 1) { + acc[3] = -0.4; + } + if(trigger[0] >= 1 && trigger[1] >= 1) { + acc[3] = 0; + } + ikarashi[0].setSpeed(acc[0]); + ikarashi[1].setSpeed(acc[1]); + ikarashi[2].setSpeed(acc[2]); + ikarashi[3].setSpeed(acc[3]); + for(int i = 0;i < 4;i++) { + pc.printf("%f\r\n",acc[i]); + } + + } + +}