![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
yamanouti_purogram_zyouhannsinn
Dependencies: mbed MotorDrivers
main.cpp@1:2e084048b5f9, 2020-10-11 (annotated)
- Committer:
- shibazakiwataru
- Date:
- Sun Oct 11 08:41:49 2020 +0000
- Revision:
- 1:2e084048b5f9
- Parent:
- 0:a6f29afbd285
yamanouti_purogram_zyouhannsinn
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sink | 0:a6f29afbd285 | 1 | #include "mbed.h" |
sink | 0:a6f29afbd285 | 2 | #include "string" |
sink | 0:a6f29afbd285 | 3 | #include "Sabertooth_Serial.h" |
sink | 0:a6f29afbd285 | 4 | #define INT_TIME 0.02 |
shibazakiwataru | 1:2e084048b5f9 | 5 | #define ADRS 133 |
sink | 0:a6f29afbd285 | 6 | #define UP 0b00000001 |
sink | 0:a6f29afbd285 | 7 | #define DOWN 0b00000010 |
sink | 0:a6f29afbd285 | 8 | #define HANDR 0b00000100 |
sink | 0:a6f29afbd285 | 9 | #define HANDL 0b00001000 |
sink | 0:a6f29afbd285 | 10 | #define OTHER 0b00010000 |
sink | 0:a6f29afbd285 | 11 | |
sink | 0:a6f29afbd285 | 12 | |
sink | 0:a6f29afbd285 | 13 | Ticker timer; |
sink | 0:a6f29afbd285 | 14 | |
shibazakiwataru | 1:2e084048b5f9 | 15 | RawSerial pc(USBTX,USBRX,115200); |
sink | 0:a6f29afbd285 | 16 | RawSerial Master(p28, p27, 115200); |
sink | 0:a6f29afbd285 | 17 | |
sink | 0:a6f29afbd285 | 18 | SaberSerial MD(115200, p13, p14); |
sink | 0:a6f29afbd285 | 19 | |
shibazakiwataru | 1:2e084048b5f9 | 20 | DigitalOut hund_fetR(p15); |
shibazakiwataru | 1:2e084048b5f9 | 21 | DigitalOut hund_fetL(p16); |
shibazakiwataru | 1:2e084048b5f9 | 22 | |
shibazakiwataru | 1:2e084048b5f9 | 23 | DigitalIn limit(p25); |
sink | 0:a6f29afbd285 | 24 | |
sink | 0:a6f29afbd285 | 25 | string Master_recv = ""; |
sink | 0:a6f29afbd285 | 26 | char buttons = 0b00000000, pre_buttons = 0b00000000; |
sink | 0:a6f29afbd285 | 27 | |
sink | 0:a6f29afbd285 | 28 | void recv_interrupt(){ |
sink | 0:a6f29afbd285 | 29 | char recv_c = Master.getc(); |
sink | 0:a6f29afbd285 | 30 | |
sink | 0:a6f29afbd285 | 31 | if (recv_c == '\n'){ |
sink | 0:a6f29afbd285 | 32 | buttons = Master_recv[0]; |
sink | 0:a6f29afbd285 | 33 | Master_recv = ""; |
sink | 0:a6f29afbd285 | 34 | } |
sink | 0:a6f29afbd285 | 35 | else{ |
sink | 0:a6f29afbd285 | 36 | Master_recv += recv_c; |
sink | 0:a6f29afbd285 | 37 | if(Master_recv.size() == 2) Master_recv = ""; |
sink | 0:a6f29afbd285 | 38 | } |
sink | 0:a6f29afbd285 | 39 | } |
sink | 0:a6f29afbd285 | 40 | |
sink | 0:a6f29afbd285 | 41 | void timer_interrupt(){ |
sink | 0:a6f29afbd285 | 42 | |
sink | 0:a6f29afbd285 | 43 | int saber_speed = 0, saber_def = 0; |
shibazakiwataru | 1:2e084048b5f9 | 44 | bool hand_R = buttons & HANDR; |
shibazakiwataru | 1:2e084048b5f9 | 45 | bool hand_L = buttons & HANDL; |
sink | 0:a6f29afbd285 | 46 | |
shibazakiwataru | 1:2e084048b5f9 | 47 | hund_fetR.write(hand_R); |
shibazakiwataru | 1:2e084048b5f9 | 48 | hund_fetL.write(hand_L); |
sink | 0:a6f29afbd285 | 49 | |
sink | 0:a6f29afbd285 | 50 | if(buttons & UP){ |
shibazakiwataru | 1:2e084048b5f9 | 51 | saber_speed = 10; |
shibazakiwataru | 1:2e084048b5f9 | 52 | saber_def = 4; |
shibazakiwataru | 1:2e084048b5f9 | 53 | } |
shibazakiwataru | 1:2e084048b5f9 | 54 | else if(buttons & OTHER){ |
sink | 0:a6f29afbd285 | 55 | saber_speed = 50; |
shibazakiwataru | 1:2e084048b5f9 | 56 | saber_def = 4; |
sink | 0:a6f29afbd285 | 57 | } |
sink | 0:a6f29afbd285 | 58 | else if(buttons & DOWN){ |
shibazakiwataru | 1:2e084048b5f9 | 59 | saber_speed = 10; |
shibazakiwataru | 1:2e084048b5f9 | 60 | saber_def = 5; |
sink | 0:a6f29afbd285 | 61 | } |
shibazakiwataru | 1:2e084048b5f9 | 62 | else { |
shibazakiwataru | 1:2e084048b5f9 | 63 | saber_speed = 0; |
shibazakiwataru | 1:2e084048b5f9 | 64 | saber_def = 4; |
shibazakiwataru | 1:2e084048b5f9 | 65 | } |
shibazakiwataru | 1:2e084048b5f9 | 66 | |
shibazakiwataru | 1:2e084048b5f9 | 67 | if(limit.read() && saber_def == 4) saber_speed = 0; |
sink | 0:a6f29afbd285 | 68 | |
sink | 0:a6f29afbd285 | 69 | MD.Serial(ADRS,saber_def,saber_speed); |
shibazakiwataru | 1:2e084048b5f9 | 70 | pc.printf("%d,%d\n",hand_R,hand_L); |
sink | 0:a6f29afbd285 | 71 | |
sink | 0:a6f29afbd285 | 72 | pre_buttons = buttons; |
sink | 0:a6f29afbd285 | 73 | } |
sink | 0:a6f29afbd285 | 74 | |
sink | 0:a6f29afbd285 | 75 | int main() { |
sink | 0:a6f29afbd285 | 76 | |
shibazakiwataru | 1:2e084048b5f9 | 77 | hund_fetR = 0; |
shibazakiwataru | 1:2e084048b5f9 | 78 | hund_fetL = 0; |
shibazakiwataru | 1:2e084048b5f9 | 79 | |
sink | 0:a6f29afbd285 | 80 | timer.attach(&timer_interrupt,INT_TIME); |
sink | 0:a6f29afbd285 | 81 | |
sink | 0:a6f29afbd285 | 82 | Master.attach(&recv_interrupt, RawSerial::RxIrq); |
sink | 0:a6f29afbd285 | 83 | |
sink | 0:a6f29afbd285 | 84 | while(true) {} |
sink | 0:a6f29afbd285 | 85 | } |