![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Drive wheeled robot platform
Fork of mbed_blinky by
main.cpp@9:ada1a8a81354, 2017-09-25 (annotated)
- Committer:
- RichardAmes
- Date:
- Mon Sep 25 22:50:24 2017 +0000
- Revision:
- 9:ada1a8a81354
- Parent:
- 8:840b3b80cfa2
initial check in
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dan | 0:7dec7e9ac085 | 1 | #include "mbed.h" |
dan | 0:7dec7e9ac085 | 2 | |
RichardAmes | 8:840b3b80cfa2 | 3 | DigitalOut drive_led(LED2); |
RichardAmes | 8:840b3b80cfa2 | 4 | PwmOut motorA(PTD0); |
RichardAmes | 8:840b3b80cfa2 | 5 | PwmOut motorB(PTD5); |
RichardAmes | 8:840b3b80cfa2 | 6 | DigitalOut dirB1(PTA13); |
RichardAmes | 8:840b3b80cfa2 | 7 | DigitalOut dirB2(PTC9); |
RichardAmes | 8:840b3b80cfa2 | 8 | DigitalOut dirA2(PTC8); |
RichardAmes | 8:840b3b80cfa2 | 9 | DigitalOut dirA1(PTA5); |
RichardAmes | 8:840b3b80cfa2 | 10 | |
RichardAmes | 8:840b3b80cfa2 | 11 | /* set pulse width for motor drive signals */ |
RichardAmes | 8:840b3b80cfa2 | 12 | void drive_init(void) |
RichardAmes | 8:840b3b80cfa2 | 13 | { |
RichardAmes | 8:840b3b80cfa2 | 14 | motorA.period(0.01f); |
RichardAmes | 8:840b3b80cfa2 | 15 | motorB.period(0.01f); |
RichardAmes | 8:840b3b80cfa2 | 16 | } |
dan | 0:7dec7e9ac085 | 17 | |
RichardAmes | 8:840b3b80cfa2 | 18 | /* set signals to move motors */ |
RichardAmes | 8:840b3b80cfa2 | 19 | /* bias min c. 25, max 100 */ |
RichardAmes | 8:840b3b80cfa2 | 20 | void move(int bias_a, int bias_b, int milliseconds) |
RichardAmes | 8:840b3b80cfa2 | 21 | { |
RichardAmes | 8:840b3b80cfa2 | 22 | if ((bias_a == 0) && (bias_b == 0)) |
RichardAmes | 8:840b3b80cfa2 | 23 | drive_led = 1; |
RichardAmes | 8:840b3b80cfa2 | 24 | else |
RichardAmes | 8:840b3b80cfa2 | 25 | drive_led = 0; |
RichardAmes | 8:840b3b80cfa2 | 26 | if (bias_a < 0) |
RichardAmes | 8:840b3b80cfa2 | 27 | { |
RichardAmes | 8:840b3b80cfa2 | 28 | dirA1 = 0; |
RichardAmes | 8:840b3b80cfa2 | 29 | dirA2 = 1; |
RichardAmes | 8:840b3b80cfa2 | 30 | } |
RichardAmes | 8:840b3b80cfa2 | 31 | else if (bias_a > 0) |
RichardAmes | 8:840b3b80cfa2 | 32 | { |
RichardAmes | 8:840b3b80cfa2 | 33 | dirA1 = 1; |
RichardAmes | 8:840b3b80cfa2 | 34 | dirA2 = 0; |
RichardAmes | 8:840b3b80cfa2 | 35 | } |
RichardAmes | 8:840b3b80cfa2 | 36 | else |
RichardAmes | 8:840b3b80cfa2 | 37 | { |
RichardAmes | 8:840b3b80cfa2 | 38 | dirA1 = 1; |
RichardAmes | 8:840b3b80cfa2 | 39 | dirA2 = 1; |
RichardAmes | 8:840b3b80cfa2 | 40 | } |
RichardAmes | 8:840b3b80cfa2 | 41 | if (bias_b < 0) |
RichardAmes | 8:840b3b80cfa2 | 42 | { |
RichardAmes | 8:840b3b80cfa2 | 43 | dirB1 = 1; |
RichardAmes | 8:840b3b80cfa2 | 44 | dirB2 = 0; |
RichardAmes | 8:840b3b80cfa2 | 45 | } |
RichardAmes | 8:840b3b80cfa2 | 46 | else if (bias_b > 0) |
RichardAmes | 8:840b3b80cfa2 | 47 | { |
RichardAmes | 8:840b3b80cfa2 | 48 | dirB1 = 0; |
RichardAmes | 8:840b3b80cfa2 | 49 | dirB2 = 1; |
RichardAmes | 8:840b3b80cfa2 | 50 | } |
RichardAmes | 8:840b3b80cfa2 | 51 | else |
RichardAmes | 8:840b3b80cfa2 | 52 | { |
RichardAmes | 8:840b3b80cfa2 | 53 | dirB1 = 1; |
RichardAmes | 8:840b3b80cfa2 | 54 | dirB2 = 1; |
RichardAmes | 8:840b3b80cfa2 | 55 | } |
RichardAmes | 8:840b3b80cfa2 | 56 | motorA.write((float)abs(bias_a) / 100); |
RichardAmes | 8:840b3b80cfa2 | 57 | motorB.write((float)abs(bias_b) / 100); |
RichardAmes | 8:840b3b80cfa2 | 58 | wait_ms(milliseconds); |
RichardAmes | 8:840b3b80cfa2 | 59 | } |
RichardAmes | 8:840b3b80cfa2 | 60 | |
RichardAmes | 8:840b3b80cfa2 | 61 | int main() |
RichardAmes | 9:ada1a8a81354 | 62 | { |
RichardAmes | 9:ada1a8a81354 | 63 | int i; |
RichardAmes | 9:ada1a8a81354 | 64 | |
RichardAmes | 8:840b3b80cfa2 | 65 | drive_init(); |
RichardAmes | 8:840b3b80cfa2 | 66 | |
RichardAmes | 9:ada1a8a81354 | 67 | for (i = 0; i < 4; i++) |
RichardAmes | 9:ada1a8a81354 | 68 | { |
RichardAmes | 9:ada1a8a81354 | 69 | move(0, 0, 500); |
RichardAmes | 9:ada1a8a81354 | 70 | move(-50, 50, 1000); |
RichardAmes | 9:ada1a8a81354 | 71 | move(0, 0, 500); |
RichardAmes | 9:ada1a8a81354 | 72 | move(50, -50, 1000); |
RichardAmes | 9:ada1a8a81354 | 73 | } |
RichardAmes | 8:840b3b80cfa2 | 74 | while(1) |
RichardAmes | 8:840b3b80cfa2 | 75 | { |
RichardAmes | 8:840b3b80cfa2 | 76 | move(0, 0, 500); |
stevep | 4:81cea7a352b0 | 77 | } |
dan | 0:7dec7e9ac085 | 78 | } |