Soma Takahashi
/
burobo2019_program
This program used in the tournament of club I belong to.
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?
User | Revision | Line number | New 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 | } |