Soma Takahashi / Mbed 2 deprecated burobo2019_program

This program used in the tournament of club I belong to.

Dependencies:   mbed

main.cpp@0:14ecdafa55eb, 2019-02-28 (annotated)

Committer:
ec30109b
Date:
Thu Feb 28 05:10:23 2019 +0000
Revision:
0:14ecdafa55eb
Don't look this code!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 0:14ecdafa55eb 1 #include "mbed.h"
ec30109b 0:14ecdafa55eb 2 #include"PS3.h"
ec30109b 0:14ecdafa55eb 3 #include "ikarashiMDC.h"
ec30109b 0:14ecdafa55eb 4
ec30109b 0:14ecdafa55eb 5 Serial serial(PC_6, PC_7);
ec30109b 0:14ecdafa55eb 6 DigitalOut serialcontrol(D2);
ec30109b 0:14ecdafa55eb 7 DigitalOut led1(LED1);
ec30109b 0:14ecdafa55eb 8 PS3 ps3(PC_12, PD_2);
ec30109b 0:14ecdafa55eb 9 Serial pc(USBTX, USBRX,115200);
ec30109b 0:14ecdafa55eb 10
ec30109b 0:14ecdafa55eb 11 ikarashiMDC ikarashi[] {
ec30109b 0:14ecdafa55eb 12 ikarashiMDC(&serialcontrol,2,3,SM,&serial),
ec30109b 0:14ecdafa55eb 13 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
ec30109b 0:14ecdafa55eb 14 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
ec30109b 0:14ecdafa55eb 15 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
ec30109b 0:14ecdafa55eb 16 ikarashiMDC(&serialcontrol,3,0,SM,&serial),
ec30109b 0:14ecdafa55eb 17 ikarashiMDC(&serialcontrol,3,1,SM,&serial)
ec30109b 0:14ecdafa55eb 18 };
ec30109b 0:14ecdafa55eb 19 PwmOut pwm(LED1);
ec30109b 0:14ecdafa55eb 20
ec30109b 0:14ecdafa55eb 21 int main()
ec30109b 0:14ecdafa55eb 22 {
ec30109b 0:14ecdafa55eb 23 serial.baud(115200);
ec30109b 0:14ecdafa55eb 24 float i = -1;
ec30109b 0:14ecdafa55eb 25 ikarashi[0].braking = true;
ec30109b 0:14ecdafa55eb 26 float pwm = 0;
ec30109b 0:14ecdafa55eb 27 while(1) {
ec30109b 0:14ecdafa55eb 28
ec30109b 0:14ecdafa55eb 29 int b[12], stick[4],X,Y,M;
ec30109b 0:14ecdafa55eb 30 double SPe[4];
ec30109b 0:14ecdafa55eb 31
ec30109b 0:14ecdafa55eb 32 /*ボタンスイッチ*/
ec30109b 0:14ecdafa55eb 33 for(int i = 0; i < 12; i++) {
ec30109b 0:14ecdafa55eb 34 b[i] = ps3.getButton(i);
ec30109b 0:14ecdafa55eb 35 pc.printf("%2d", b[i] );
ec30109b 0:14ecdafa55eb 36 }
ec30109b 0:14ecdafa55eb 37 /*ジョイスティック*/
ec30109b 0:14ecdafa55eb 38 for(int i = 0; i < 4; i++) {
ec30109b 0:14ecdafa55eb 39 stick[i] = ps3.getStick(i);
ec30109b 0:14ecdafa55eb 40 pc.printf("%4d", stick[i] );
ec30109b 0:14ecdafa55eb 41 }
ec30109b 0:14ecdafa55eb 42
ec30109b 0:14ecdafa55eb 43 //ベルト機構
ec30109b 0:14ecdafa55eb 44 if(b[6] == 0 || b[7] == 0){
ec30109b 0:14ecdafa55eb 45 ikarashi[5].setSpeed(0);
ec30109b 0:14ecdafa55eb 46 }
ec30109b 0:14ecdafa55eb 47 if(b[6] == 1){
ec30109b 0:14ecdafa55eb 48 ikarashi[4].setSpeed(0.08);
ec30109b 0:14ecdafa55eb 49 ikarashi[5].setSpeed(-0.45);
ec30109b 0:14ecdafa55eb 50 }
ec30109b 0:14ecdafa55eb 51 if(b[7] == 1){
ec30109b 0:14ecdafa55eb 52 ikarashi[4].setSpeed(-0.08);
ec30109b 0:14ecdafa55eb 53 ikarashi[5].setSpeed(0.45);
ec30109b 0:14ecdafa55eb 54 }
ec30109b 0:14ecdafa55eb 55
ec30109b 0:14ecdafa55eb 56
ec30109b 0:14ecdafa55eb 57 //バケット機構
ec30109b 0:14ecdafa55eb 58 if(b[0] == 0 || b[1] == 0){
ec30109b 0:14ecdafa55eb 59 ikarashi[4].setSpeed(0);
ec30109b 0:14ecdafa55eb 60 }
ec30109b 0:14ecdafa55eb 61 if(b[0] == 1){
ec30109b 0:14ecdafa55eb 62 ikarashi[4].setSpeed(0.08);
ec30109b 0:14ecdafa55eb 63 }
ec30109b 0:14ecdafa55eb 64 if(b[1] == 1){
ec30109b 0:14ecdafa55eb 65 ikarashi[4].setSpeed(-0.08);
ec30109b 0:14ecdafa55eb 66 }
ec30109b 0:14ecdafa55eb 67
ec30109b 0:14ecdafa55eb 68
ec30109b 0:14ecdafa55eb 69 //足回り　　　
ec30109b 0:14ecdafa55eb 70 X = stick[0];
ec30109b 0:14ecdafa55eb 71 Y = stick[1];
ec30109b 0:14ecdafa55eb 72 M = stick[2];
ec30109b 0:14ecdafa55eb 73
ec30109b 0:14ecdafa55eb 74 if(X > 100 && X < 155 && Y > 100 && Y < 155) { //停止
ec30109b 0:14ecdafa55eb 75 SPe[0] = 0;
ec30109b 0:14ecdafa55eb 76 SPe[1] = 0;
ec30109b 0:14ecdafa55eb 77 SPe[2] = 0;
ec30109b 0:14ecdafa55eb 78 SPe[3] = 0;
ec30109b 0:14ecdafa55eb 79
ec30109b 0:14ecdafa55eb 80 pc.printf(" 停止　　");
ec30109b 0:14ecdafa55eb 81 }
ec30109b 0:14ecdafa55eb 82 if(X > 100 && Y <= 100) { //右前方向
ec30109b 0:14ecdafa55eb 83 SPe[0] = -0.45;
ec30109b 0:14ecdafa55eb 84 SPe[1] = ((127.5 - Y) - (X - 127.5)) * 0.004;
ec30109b 0:14ecdafa55eb 85 SPe[2] = 0.45;
ec30109b 0:14ecdafa55eb 86 SPe[3] = ((X - 127.5) - (127.5 - Y)) * 0.004;
ec30109b 0:14ecdafa55eb 87
ec30109b 0:14ecdafa55eb 88 pc.printf(" 右前方向 ");
ec30109b 0:14ecdafa55eb 89 }
ec30109b 0:14ecdafa55eb 90 if(X <= 100 && Y < 155) { //左前方向
ec30109b 0:14ecdafa55eb 91 SPe[0] = ((127.5 - X) - (127.5 - Y)) * 0.004;
ec30109b 0:14ecdafa55eb 92 SPe[1] = 0.45;
ec30109b 0:14ecdafa55eb 93 SPe[2] = ((127.5 - X) - (127.5 - Y)) * -0.004;
ec30109b 0:14ecdafa55eb 94 SPe[3] = -0.45;
ec30109b 0:14ecdafa55eb 95
ec30109b 0:14ecdafa55eb 96 pc.printf(" 左前方向 ");
ec30109b 0:14ecdafa55eb 97 }
ec30109b 0:14ecdafa55eb 98 if(X >= 155 && Y > 100) { //右後ろ方向
ec30109b 0:14ecdafa55eb 99 SPe[0] = ((X - 127.5) - (Y - 127.5)) * -0.004;
ec30109b 0:14ecdafa55eb 100 SPe[1] = -0.45;
ec30109b 0:14ecdafa55eb 101 SPe[2] = ((X - 127.5) - (Y - 127.5)) * 0.004;
ec30109b 0:14ecdafa55eb 102 SPe[3] = 0.45;
ec30109b 0:14ecdafa55eb 103
ec30109b 0:14ecdafa55eb 104 pc.printf(" 右後ろ方向　　");
ec30109b 0:14ecdafa55eb 105 }
ec30109b 0:14ecdafa55eb 106 if(X < 155 && Y >= 155) { //左後ろ方向
ec30109b 0:14ecdafa55eb 107 SPe[0] = 0.45;
ec30109b 0:14ecdafa55eb 108 SPe[1] = ((X - 127.5) + (127.5 - Y)) * 0.004;
ec30109b 0:14ecdafa55eb 109 SPe[2] = -0.45;
ec30109b 0:14ecdafa55eb 110 SPe[3] = ((X - 127.5) - (127.5 - Y)) * 0.004;
ec30109b 0:14ecdafa55eb 111
ec30109b 0:14ecdafa55eb 112 pc.printf(" 左後ろ方向　　");
ec30109b 0:14ecdafa55eb 113 }
ec30109b 0:14ecdafa55eb 114 if(M < 10) { //左回転
ec30109b 0:14ecdafa55eb 115 SPe[0] = 0.4;
ec30109b 0:14ecdafa55eb 116 SPe[1] = 0.4;
ec30109b 0:14ecdafa55eb 117 SPe[2] = 0.4;
ec30109b 0:14ecdafa55eb 118 SPe[3] = 0.4;
ec30109b 0:14ecdafa55eb 119
ec30109b 0:14ecdafa55eb 120 pc.printf(" 左回転 ");
ec30109b 0:14ecdafa55eb 121 }
ec30109b 0:14ecdafa55eb 122 if(M > 245) { //右回転
ec30109b 0:14ecdafa55eb 123 SPe[0] = -0.4;
ec30109b 0:14ecdafa55eb 124 SPe[1] = -0.4;
ec30109b 0:14ecdafa55eb 125 SPe[2] = -0.4;
ec30109b 0:14ecdafa55eb 126 SPe[3] = -0.4;
ec30109b 0:14ecdafa55eb 127
ec30109b 0:14ecdafa55eb 128 pc.printf(" 右回転 ");
ec30109b 0:14ecdafa55eb 129
ec30109b 0:14ecdafa55eb 130 }
ec30109b 0:14ecdafa55eb 131
ec30109b 0:14ecdafa55eb 132
ec30109b 0:14ecdafa55eb 133 ikarashi[0].setSpeed(SPe[0]);
ec30109b 0:14ecdafa55eb 134 ikarashi[1].setSpeed(SPe[1]);
ec30109b 0:14ecdafa55eb 135 ikarashi[2].setSpeed(SPe[2]);
ec30109b 0:14ecdafa55eb 136 ikarashi[3].setSpeed(SPe[3]);
ec30109b 0:14ecdafa55eb 137
ec30109b 0:14ecdafa55eb 138 pc.printf("%.2f ",SPe[0]);
ec30109b 0:14ecdafa55eb 139 pc.printf("%.2f ",SPe[1]);
ec30109b 0:14ecdafa55eb 140 pc.printf("%.2f ",SPe[2]);
ec30109b 0:14ecdafa55eb 141 pc.printf("%.2f ",SPe[3]);
ec30109b 0:14ecdafa55eb 142
ec30109b 0:14ecdafa55eb 143 pc.printf("\r\n");
ec30109b 0:14ecdafa55eb 144 }