kourobo2019 teamA
Dependencies: Controller ikarashiMDC
main.cpp@4:94f943530940, 2019-03-30 (annotated)
- Committer:
- piroro4560
- Date:
- Sat Mar 30 10:34:08 2019 +0000
- Revision:
- 4:94f943530940
- Parent:
- 2:6cce570ce61b
kourobo Ateam
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 1:e208233598b5 | 1 | #include "mbed.h" |
piroro4560 | 0:93be607fd77a | 2 | #include "controller.h" |
piroro4560 | 0:93be607fd77a | 3 | #include "ikarashiMDC.h" |
piroro4560 | 0:93be607fd77a | 4 | |
piroro4560 | 4:94f943530940 | 5 | Controller pad(PA_0, PA_1, ); |
piroro4560 | 0:93be607fd77a | 6 | Serial serial(PC_10, PC_11, 115200); |
piroro4560 | 0:93be607fd77a | 7 | Serial PC(USBTX,USBRX,115200); |
piroro4560 | 0:93be607fd77a | 8 | DigitalOut serialcontrol(D2); |
piroro4560 | 0:93be607fd77a | 9 | |
piroro4560 | 0:93be607fd77a | 10 | ikarashiMDC ikarashi[] { |
piroro4560 | 0:93be607fd77a | 11 | ikarashiMDC(&serialcontrol,2,0,SM,&serial), |
piroro4560 | 0:93be607fd77a | 12 | ikarashiMDC(&serialcontrol,2,1,SM,&serial), |
piroro4560 | 0:93be607fd77a | 13 | ikarashiMDC(&serialcontrol,2,2,SM,&serial), |
piroro4560 | 0:93be607fd77a | 14 | ikarashiMDC(&serialcontrol,2,3,SM,&serial), |
piroro4560 | 0:93be607fd77a | 15 | }; |
piroro4560 | 0:93be607fd77a | 16 | PwmOut pwm(LED1); |
piroro4560 | 0:93be607fd77a | 17 | |
piroro4560 | 0:93be607fd77a | 18 | int main() { |
piroro4560 | 0:93be607fd77a | 19 | int b[11]; |
piroro4560 | 2:6cce570ce61b | 20 | double value[4], rad[2], dis[2]; |
piroro4560 | 2:6cce570ce61b | 21 | double spV = 0.3;/////////speed Vertical |
piroro4560 | 2:6cce570ce61b | 22 | double spS = 0.3;/////////speed Side |
piroro4560 | 4:94f943530940 | 23 | double spT = 0.15;////////speed Turn |
piroro4560 | 4:94f943530940 | 24 | double Fm = 1.0;//////////Front magnification |
piroro4560 | 4:94f943530940 | 25 | double Bm = 1.0;//////////Back magnification |
piroro4560 | 4:94f943530940 | 26 | double Sm = 1.0///////////Side magnification |
piroro4560 | 4:94f943530940 | 27 | double acc = 25;//////////acceleration |
piroro4560 | 4:94f943530940 | 28 | double dec = 5;///////////deceleration |
piroro4560 | 0:93be607fd77a | 29 | ikarashi[0].braking = true; |
piroro4560 | 0:93be607fd77a | 30 | while (1) { |
piroro4560 | 2:6cce570ce61b | 31 | //button stick trigger// |
piroro4560 | 2:6cce570ce61b | 32 | if(pad.receiveState()){ |
piroro4560 | 0:93be607fd77a | 33 | for(int i = 0; i < 13; i++){ |
piroro4560 | 0:93be607fd77a | 34 | b[i] = pad.getButton1(i); |
piroro4560 | 1:e208233598b5 | 35 | pc.printf("%d ", [i]); |
piroro4560 | 0:93be607fd77a | 36 | } |
piroro4560 | 1:e208233598b5 | 37 | for(int i = 0; i < 2; i++){ |
piroro4560 | 1:e208233598b5 | 38 | rad[i] = pad.getRadian(i); |
piroro4560 | 1:e208233598b5 | 39 | dis[i] = pad.getNorm(i); |
piroro4560 | 2:6cce570ce61b | 40 | PC.printf("%f %f\t", rad[i], dis[i]); |
piroro4560 | 1:e208233598b5 | 41 | } |
piroro4560 | 0:93be607fd77a | 42 | } |
piroro4560 | 2:6cce570ce61b | 43 | /*************************** |
piroro4560 | 2:6cce570ce61b | 44 | ***************************/ |
piroro4560 | 2:6cce570ce61b | 45 | ////acceleration//// |
piroro4560 | 1:e208233598b5 | 46 | if (b[2] == 0 && b[3] + b[4] + b[5] == 3 && value[0] < spS && value[1] < spS * Sm) { |
piroro4560 | 0:93be607fd77a | 47 | value[0] += spS / acc; |
piroro4560 | 0:93be607fd77a | 48 | value[1] -= spS / acc * Sm; |
piroro4560 | 0:93be607fd77a | 49 | value[2] -= spS / acc * Sm; |
piroro4560 | 0:93be607fd77a | 50 | value[3] += spS / acc; |
piroro4560 | 0:93be607fd77a | 51 | } |
piroro4560 | 1:e208233598b5 | 52 | if (b[3] == 0 && b[2] + b[4] + b[5] == 3 && value[0] < spV && value[2] < spV * Fm) { |
piroro4560 | 0:93be607fd77a | 53 | value[0] += spV / acc; |
piroro4560 | 0:93be607fd77a | 54 | value[1] += spV / acc; |
piroro4560 | 0:93be607fd77a | 55 | value[2] -= spV / acc * Fm; |
piroro4560 | 0:93be607fd77a | 56 | value[3] -= spV / acc * Fm; |
piroro4560 | 0:93be607fd77a | 57 | } |
piroro4560 | 1:e208233598b5 | 58 | if (b[4] == 0 && b[2] + b[3] + b[5] == 3 && value[1] < spS && value[0] < spS * Sm) { |
piroro4560 | 0:93be607fd77a | 59 | value[0] -= spS / acc * Sm; |
piroro4560 | 0:93be607fd77a | 60 | value[1] += spS / acc; |
piroro4560 | 0:93be607fd77a | 61 | value[2] += spS / acc; |
piroro4560 | 0:93be607fd77a | 62 | value[3] -= spS / acc * Sm; |
piroro4560 | 0:93be607fd77a | 63 | } |
piroro4560 | 1:e208233598b5 | 64 | if (b[5] == 0 && b[2] + b[3] + b[4] == 3 && value[2] < spV && value[0] < spV * Bm) { |
piroro4560 | 0:93be607fd77a | 65 | value[0] -= spV / acc * Bm; |
piroro4560 | 0:93be607fd77a | 66 | value[1] -= spV / acc * Bm; |
piroro4560 | 0:93be607fd77a | 67 | value[2] += spV / acc; |
piroro4560 | 0:93be607fd77a | 68 | value[3] += spV / acc; |
piroro4560 | 0:93be607fd77a | 69 | } |
piroro4560 | 2:6cce570ce61b | 70 | /********/ |
piroro4560 | 1:e208233598b5 | 71 | ////turn//// |
piroro4560 | 0:93be607fd77a | 72 | if (b[0] + b[1] + b[2] + b[3] == 4) { |
piroro4560 | 0:93be607fd77a | 73 | if (b[8] == 0 && b[10] == 1) { |
piroro4560 | 0:93be607fd77a | 74 | for (int i = 0;i < 4; i++) { |
piroro4560 | 2:6cce570ce61b | 75 | value[i] = spT; |
piroro4560 | 0:93be607fd77a | 76 | } |
piroro4560 | 0:93be607fd77a | 77 | } |
piroro4560 | 0:93be607fd77a | 78 | if (b[10] == 0 && b[8] == 1) { |
piroro4560 | 0:93be607fd77a | 79 | for (int i = 0; i < 4; i++) { |
piroro4560 | 2:6cce570ce61b | 80 | value[i] = -spT; |
piroro4560 | 0:93be607fd77a | 81 | } |
piroro4560 | 0:93be607fd77a | 82 | } |
piroro4560 | 0:93be607fd77a | 83 | } |
piroro4560 | 2:6cce570ce61b | 84 | /********/ |
piroro4560 | 0:93be607fd77a | 85 | if (b[2] + b[3] + b[4] + b[5] <= 2 || b[2] + b[3] + b[4] + b[5] == 4 && b[8] + b[10] == 2) { |
piroro4560 | 0:93be607fd77a | 86 | for (int i = 0; i < 4; i++) { |
piroro4560 | 1:e208233598b5 | 87 | value[i] -= value[i] / dec; |
piroro4560 | 0:93be607fd77a | 88 | } |
piroro4560 | 0:93be607fd77a | 89 | } |
piroro4560 | 1:e208233598b5 | 90 | /*************************** |
piroro4560 | 1:e208233598b5 | 91 | ***************************/ |
piroro4560 | 0:93be607fd77a | 92 | for (int i = 0; i < 4; i++) { |
piroro4560 | 0:93be607fd77a | 93 | PC.printf("%.2f,", value[i]); |
piroro4560 | 0:93be607fd77a | 94 | } |
piroro4560 | 0:93be607fd77a | 95 | PC.printf("||"); |
piroro4560 | 0:93be607fd77a | 96 | ikarashi[0].setSpeed(value[0]); |
piroro4560 | 0:93be607fd77a | 97 | ikarashi[1].setSpeed(value[1]); |
piroro4560 | 0:93be607fd77a | 98 | ikarashi[2].setSpeed(value[2]); |
piroro4560 | 0:93be607fd77a | 99 | ikarashi[3].setSpeed(value[3]); |
piroro4560 | 0:93be607fd77a | 100 | |
piroro4560 | 2:6cce570ce61b | 101 | for (int i = 0; i < 4; i++) { |
piroro4560 | 0:93be607fd77a | 102 | PC.printf("%.2f,", value[i]); |
piroro4560 | 0:93be607fd77a | 103 | } |
piroro4560 | 0:93be607fd77a | 104 | PC.printf("\n\r"); |
piroro4560 | 0:93be607fd77a | 105 | } |
piroro4560 | 0:93be607fd77a | 106 | } |