wheel_test3
Dependencies: wheel_an R1370 Controller ikarashiMDC
main.cpp@0:46c70e5b719d, 2019-05-16 (annotated)
- Committer:
- piroro4560
- Date:
- Thu May 16 07:41:44 2019 +0000
- Revision:
- 0:46c70e5b719d
wheeltest3;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:46c70e5b719d | 1 | #include "mbed.h" //wheel_test3 |
piroro4560 | 0:46c70e5b719d | 2 | #include "controller.h" |
piroro4560 | 0:46c70e5b719d | 3 | #include "ikarashiMDC.h" |
piroro4560 | 0:46c70e5b719d | 4 | #include "R1370.h" |
piroro4560 | 0:46c70e5b719d | 5 | #include "wheel_an.h" |
piroro4560 | 0:46c70e5b719d | 6 | #define PI 3.141593 |
piroro4560 | 0:46c70e5b719d | 7 | |
piroro4560 | 0:46c70e5b719d | 8 | Controller pad(PC_10, PC_11, 208); |
piroro4560 | 0:46c70e5b719d | 9 | R1370 R1370(D10,D2); |
piroro4560 | 0:46c70e5b719d | 10 | Serial pc(USBTX, USBRX, 115200); |
piroro4560 | 0:46c70e5b719d | 11 | Serial serial(PC_6, PC_7, 115200); |
piroro4560 | 0:46c70e5b719d | 12 | DigitalOut serialcontrol(PD_2); |
piroro4560 | 0:46c70e5b719d | 13 | |
piroro4560 | 0:46c70e5b719d | 14 | ikarashiMDC ikarashi[] |
piroro4560 | 0:46c70e5b719d | 15 | { |
piroro4560 | 0:46c70e5b719d | 16 | ikarashiMDC(&serialcontrol,2,0,SM,&serial), |
piroro4560 | 0:46c70e5b719d | 17 | ikarashiMDC(&serialcontrol,2,1,SM,&serial), |
piroro4560 | 0:46c70e5b719d | 18 | ikarashiMDC(&serialcontrol,2,2,SM,&serial) |
piroro4560 | 0:46c70e5b719d | 19 | }; |
piroro4560 | 0:46c70e5b719d | 20 | |
piroro4560 | 0:46c70e5b719d | 21 | Wheelan wheel[] |
piroro4560 | 0:46c70e5b719d | 22 | { |
piroro4560 | 0:46c70e5b719d | 23 | Wheelan(PI * 1 / 6), |
piroro4560 | 0:46c70e5b719d | 24 | Wheelan(PI * 5 / 6), |
piroro4560 | 0:46c70e5b719d | 25 | Wheelan(PI * 9 / 6) |
piroro4560 | 0:46c70e5b719d | 26 | }; |
piroro4560 | 0:46c70e5b719d | 27 | |
piroro4560 | 0:46c70e5b719d | 28 | int main() |
piroro4560 | 0:46c70e5b719d | 29 | { |
piroro4560 | 0:46c70e5b719d | 30 | ikarashi[0].braking = true; |
piroro4560 | 0:46c70e5b719d | 31 | int b[11]; |
piroro4560 | 0:46c70e5b719d | 32 | double rad[2],dis[2]; |
piroro4560 | 0:46c70e5b719d | 33 | double value[3]; |
piroro4560 | 0:46c70e5b719d | 34 | while(1){ |
piroro4560 | 0:46c70e5b719d | 35 | if(pad.receiveState()){ |
piroro4560 | 0:46c70e5b719d | 36 | for(int i = 0; i < 13; i++){ |
piroro4560 | 0:46c70e5b719d | 37 | b[i] = pad.getButton1(i); |
piroro4560 | 0:46c70e5b719d | 38 | } |
piroro4560 | 0:46c70e5b719d | 39 | for(int i = 0; i < 2; i++){ |
piroro4560 | 0:46c70e5b719d | 40 | rad[i] = pad.getRadian(i); |
piroro4560 | 0:46c70e5b719d | 41 | dis[i] = pad.getNorm(i); |
piroro4560 | 0:46c70e5b719d | 42 | rad[i] = PI - rad[i]; |
piroro4560 | 0:46c70e5b719d | 43 | } |
piroro4560 | 0:46c70e5b719d | 44 | R1370.update(); |
piroro4560 | 0:46c70e5b719d | 45 | pc.printf("degree : %.3lf ", R1370.getAngle()); |
piroro4560 | 0:46c70e5b719d | 46 | |
piroro4560 | 0:46c70e5b719d | 47 | for (int i = 0; i < 3; ++i) |
piroro4560 | 0:46c70e5b719d | 48 | { |
piroro4560 | 0:46c70e5b719d | 49 | value[i] = wheel[i].an_uc(rad[1], dis[1]); |
piroro4560 | 0:46c70e5b719d | 50 | } |
piroro4560 | 0:46c70e5b719d | 51 | |
piroro4560 | 0:46c70e5b719d | 52 | if(b[7] == 1 && b[9] == 0) { |
piroro4560 | 0:46c70e5b719d | 53 | value[0] -= 0.3; |
piroro4560 | 0:46c70e5b719d | 54 | value[1] -= 0.3; |
piroro4560 | 0:46c70e5b719d | 55 | value[2] -= 0.3; |
piroro4560 | 0:46c70e5b719d | 56 | } else if (b[9] == 1 && b[7] == 0) { |
piroro4560 | 0:46c70e5b719d | 57 | value[0] += 0.3; |
piroro4560 | 0:46c70e5b719d | 58 | value[1] += 0.3; |
piroro4560 | 0:46c70e5b719d | 59 | value[2] += 0.3; |
piroro4560 | 0:46c70e5b719d | 60 | } |
piroro4560 | 0:46c70e5b719d | 61 | |
piroro4560 | 0:46c70e5b719d | 62 | for (int i = 0; i < 3; ++i) |
piroro4560 | 0:46c70e5b719d | 63 | { |
piroro4560 | 0:46c70e5b719d | 64 | pc.printf("%lf ", value[i]); |
piroro4560 | 0:46c70e5b719d | 65 | ikarashi[i].setSpeed(value[i]); |
piroro4560 | 0:46c70e5b719d | 66 | } |
piroro4560 | 0:46c70e5b719d | 67 | pc.printf("\n\r"); |
piroro4560 | 0:46c70e5b719d | 68 | |
piroro4560 | 0:46c70e5b719d | 69 | }else{ |
piroro4560 | 0:46c70e5b719d | 70 | pc.printf("error\n\r"); |
piroro4560 | 0:46c70e5b719d | 71 | for (int i = 0; i < 3; i++) |
piroro4560 | 0:46c70e5b719d | 72 | { |
piroro4560 | 0:46c70e5b719d | 73 | ikarashi[i].setSpeed(0); |
piroro4560 | 0:46c70e5b719d | 74 | } |
piroro4560 | 0:46c70e5b719d | 75 | } |
piroro4560 | 0:46c70e5b719d | 76 | } |
piroro4560 | 0:46c70e5b719d | 77 | } |