kuya ishikawa / Mbed 2 deprecated kourobo

a

Dependencies:   mbed ikarashiMDC PS3

main.cpp@0:193dcda90d39, 2019-03-07 (annotated)

Committer:
aiueo700A
Date:
Thu Mar 07 07:40:22 2019 +0000
Revision:
0:193dcda90d39
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aiueo700A 0:193dcda90d39 1 #include"mbed.h"
aiueo700A 0:193dcda90d39 2 #include"ikarashiMDC.h"
aiueo700A 0:193dcda90d39 3 #include"PS3.h"
aiueo700A 0:193dcda90d39 4 #define PI 3.14159265
aiueo700A 0:193dcda90d39 5 Serial serial(PC_6,PC_7);
aiueo700A 0:193dcda90d39 6 DigitalOut serialcontrol(D2);
aiueo700A 0:193dcda90d39 7
aiueo700A 0:193dcda90d39 8 ikarashiMDC ikarashi[] {
aiueo700A 0:193dcda90d39 9 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
aiueo700A 0:193dcda90d39 10 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
aiueo700A 0:193dcda90d39 11 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
aiueo700A 0:193dcda90d39 12 ikarashiMDC(&serialcontrol,2,3,SM,&serial),
aiueo700A 0:193dcda90d39 13 ikarashiMDC(&serialcontrol,1,0,SM,&serial)
aiueo700A 0:193dcda90d39 14 };
aiueo700A 0:193dcda90d39 15
aiueo700A 0:193dcda90d39 16 PS3 ps3(PC_10,PC_11);
aiueo700A 0:193dcda90d39 17 Serial pc(USBTX,USBRX,115200);
aiueo700A 0:193dcda90d39 18
aiueo700A 0:193dcda90d39 19 double way(double x,double y,double direction)
aiueo700A 0:193dcda90d39 20 {
aiueo700A 0:193dcda90d39 21 double atanval;
aiueo700A 0:193dcda90d39 22 atanval = atan2(y,x);
aiueo700A 0:193dcda90d39 23 return sin(atanval - direction);
aiueo700A 0:193dcda90d39 24 }
aiueo700A 0:193dcda90d39 25
aiueo700A 0:193dcda90d39 26 int main()
aiueo700A 0:193dcda90d39 27 {
aiueo700A 0:193dcda90d39 28 serial.baud(115200);
aiueo700A 0:193dcda90d39 29 ikarashi[0].braking = true;
aiueo700A 0:193dcda90d39 30 int b[12],stick[4],trigger[2],neutral[4],count = 0;
aiueo700A 0:193dcda90d39 31 double X,Y,acc[5];
aiueo700A 0:193dcda90d39 32 /*ジョイスティック　ニュートラル設定*/
aiueo700A 0:193dcda90d39 33 do{
aiueo700A 0:193dcda90d39 34 for(int i = 0;i < 4;i++) {
aiueo700A 0:193dcda90d39 35 neutral[i] = ps3.getStick(i);
aiueo700A 0:193dcda90d39 36 pc.printf("%4d",neutral[i]);
aiueo700A 0:193dcda90d39 37 }
aiueo700A 0:193dcda90d39 38 count++;
aiueo700A 0:193dcda90d39 39 }while(count <= 115200);
aiueo700A 0:193dcda90d39 40 for(int i = 0;i < 4;i++) {
aiueo700A 0:193dcda90d39 41 neutral[i] = ps3.getStick(i);
aiueo700A 0:193dcda90d39 42 pc.printf("%4d",neutral[i]);
aiueo700A 0:193dcda90d39 43 }
aiueo700A 0:193dcda90d39 44 while(1){
aiueo700A 0:193dcda90d39 45 /*ボタン０＝上　１＝下　２＝左　３＝右　４＝L１　５＝R１　６＝△　７＝✖
aiueo700A 0:193dcda90d39 46 ８＝🔲　９＝◯　１０＝L３　１１＝R３*/
aiueo700A 0:193dcda90d39 47 for(int i=0;i < 12;i++) {
aiueo700A 0:193dcda90d39 48 b[i] = ps3.getButton(i);
aiueo700A 0:193dcda90d39 49 pc.printf("%d",b[i]);
aiueo700A 0:193dcda90d39 50 }
aiueo700A 0:193dcda90d39 51 /*ジョイスティック*/
aiueo700A 0:193dcda90d39 52 for(int i = 0;i < 4;i++){
aiueo700A 0:193dcda90d39 53 stick[i] = ps3.getStick(i);
aiueo700A 0:193dcda90d39 54 pc.printf("%4d\r\n",stick[i]);
aiueo700A 0:193dcda90d39 55 }
aiueo700A 0:193dcda90d39 56
aiueo700A 0:193dcda90d39 57 for(int i = 0;i < 2;i++) {
aiueo700A 0:193dcda90d39 58 trigger[i] = ps3.getTrigger(i);
aiueo700A 0:193dcda90d39 59 pc.printf("%4d\r\n",trigger[i]);
aiueo700A 0:193dcda90d39 60 }
aiueo700A 0:193dcda90d39 61 X = stick[0] - neutral[0];
aiueo700A 0:193dcda90d39 62 Y = neutral[1] - stick[1];
aiueo700A 0:193dcda90d39 63 if( abs(X) <= 20 && abs(Y) <= 20){
aiueo700A 0:193dcda90d39 64 acc[0] = 0;
aiueo700A 0:193dcda90d39 65 acc[1] = 0;
aiueo700A 0:193dcda90d39 66 acc[2] = 0;
aiueo700A 0:193dcda90d39 67 }else {
aiueo700A 0:193dcda90d39 68 acc[0] = way(X,Y,PI/6);
aiueo700A 0:193dcda90d39 69 acc[1] = way(X,Y,PI*3/2);
aiueo700A 0:193dcda90d39 70 acc[2] = way(X,Y,PI/5/6);
aiueo700A 0:193dcda90d39 71 }
aiueo700A 0:193dcda90d39 72 if(b[4] == 1 && abs(X) <= 20 && abs(Y) <= 20){
aiueo700A 0:193dcda90d39 73 acc[0] = 1;
aiueo700A 0:193dcda90d39 74 acc[1] = 1;
aiueo700A 0:193dcda90d39 75 acc[2] = 1;
aiueo700A 0:193dcda90d39 76 }
aiueo700A 0:193dcda90d39 77 if(b[5] == 1 && abs(X) <= 20 && abs(Y) <= 20){
aiueo700A 0:193dcda90d39 78 acc[0] = -1;
aiueo700A 0:193dcda90d39 79 acc[1] = -1;
aiueo700A 0:193dcda90d39 80 acc[2] = -1;
aiueo700A 0:193dcda90d39 81 }
aiueo700A 0:193dcda90d39 82 if(b[7] == 1){
aiueo700A 0:193dcda90d39 83 acc[0] = 0;
aiueo700A 0:193dcda90d39 84 acc[1] = 0;
aiueo700A 0:193dcda90d39 85 acc[2] = 0;
aiueo700A 0:193dcda90d39 86 }
aiueo700A 0:193dcda90d39 87 if(trigger[0] == 0 && trigger[1] == 0) {
aiueo700A 0:193dcda90d39 88 acc[3] = 0;
aiueo700A 0:193dcda90d39 89 }
aiueo700A 0:193dcda90d39 90 if(trigger[0] >= 1 && trigger[1] == 0) {
aiueo700A 0:193dcda90d39 91 acc[3] = 0.4;
aiueo700A 0:193dcda90d39 92 }
aiueo700A 0:193dcda90d39 93 if(trigger[0] == 0 && trigger[1] >= 1) {
aiueo700A 0:193dcda90d39 94 acc[3] = -0.4;
aiueo700A 0:193dcda90d39 95 }
aiueo700A 0:193dcda90d39 96 if(trigger[0] >= 1 && trigger[1] >= 1) {
aiueo700A 0:193dcda90d39 97 acc[3] = 0;
aiueo700A 0:193dcda90d39 98 }
aiueo700A 0:193dcda90d39 99 ikarashi[0].setSpeed(acc[0]);
aiueo700A 0:193dcda90d39 100 ikarashi[1].setSpeed(acc[1]);
aiueo700A 0:193dcda90d39 101 ikarashi[2].setSpeed(acc[2]);
aiueo700A 0:193dcda90d39 102 ikarashi[3].setSpeed(acc[3]);
aiueo700A 0:193dcda90d39 103 for(int i = 0;i < 4;i++) {
aiueo700A 0:193dcda90d39 104 pc.printf("%f\r\n",acc[i]);
aiueo700A 0:193dcda90d39 105 }
aiueo700A 0:193dcda90d39 106
aiueo700A 0:193dcda90d39 107 }
aiueo700A 0:193dcda90d39 108
aiueo700A 0:193dcda90d39 109 }