Suga koubou
/
Motor_TB6612
see https://mbed.org/users/okini3939/notebook/motor/
main.cpp@1:02b46ad7dcac, 2013-02-05 (annotated)
- Committer:
- okini3939
- Date:
- Tue Feb 05 04:11:53 2013 +0000
- Revision:
- 1:02b46ad7dcac
- Parent:
- 0:bdcbec496e71
fix
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
okini3939 | 0:bdcbec496e71 | 1 | #include "mbed.h" |
okini3939 | 0:bdcbec496e71 | 2 | |
okini3939 | 0:bdcbec496e71 | 3 | DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4); |
okini3939 | 0:bdcbec496e71 | 4 | |
okini3939 | 0:bdcbec496e71 | 5 | DigitalOut m1a(p29), m1b(p30), m2a(p27), m2b(p28); |
okini3939 | 0:bdcbec496e71 | 6 | PwmOut m1p(p26), m2p(p25); |
okini3939 | 0:bdcbec496e71 | 7 | PwmOut m3p(p23), m4p(p24); |
okini3939 | 0:bdcbec496e71 | 8 | |
okini3939 | 0:bdcbec496e71 | 9 | void motor (int num, float speed, int brake) { |
okini3939 | 0:bdcbec496e71 | 10 | if (num == 1) { |
okini3939 | 0:bdcbec496e71 | 11 | if (brake) { |
okini3939 | 0:bdcbec496e71 | 12 | m1p = 1; |
okini3939 | 1:02b46ad7dcac | 13 | m1a = 1; |
okini3939 | 1:02b46ad7dcac | 14 | m1b = 1; |
okini3939 | 0:bdcbec496e71 | 15 | } else |
okini3939 | 0:bdcbec496e71 | 16 | if (speed > 0) { |
okini3939 | 0:bdcbec496e71 | 17 | m1p = speed; |
okini3939 | 0:bdcbec496e71 | 18 | m1a = 1; |
okini3939 | 0:bdcbec496e71 | 19 | m1b = 0; |
okini3939 | 0:bdcbec496e71 | 20 | } else |
okini3939 | 0:bdcbec496e71 | 21 | if (speed < 0) { |
okini3939 | 0:bdcbec496e71 | 22 | m1p = - speed; |
okini3939 | 0:bdcbec496e71 | 23 | m1a = 0; |
okini3939 | 0:bdcbec496e71 | 24 | m1b = 1; |
okini3939 | 0:bdcbec496e71 | 25 | } else { |
okini3939 | 0:bdcbec496e71 | 26 | m1p = 1; |
okini3939 | 0:bdcbec496e71 | 27 | m1a = 0; |
okini3939 | 0:bdcbec496e71 | 28 | m1b = 0; |
okini3939 | 0:bdcbec496e71 | 29 | } |
okini3939 | 0:bdcbec496e71 | 30 | } else |
okini3939 | 0:bdcbec496e71 | 31 | if (num == 2) { |
okini3939 | 0:bdcbec496e71 | 32 | if (brake) { |
okini3939 | 0:bdcbec496e71 | 33 | m2p = 1; |
okini3939 | 1:02b46ad7dcac | 34 | m2a = 1; |
okini3939 | 1:02b46ad7dcac | 35 | m2b = 1; |
okini3939 | 0:bdcbec496e71 | 36 | } else |
okini3939 | 0:bdcbec496e71 | 37 | if (speed > 0) { |
okini3939 | 0:bdcbec496e71 | 38 | m2p = speed; |
okini3939 | 0:bdcbec496e71 | 39 | m2a = 1; |
okini3939 | 0:bdcbec496e71 | 40 | m2b = 0; |
okini3939 | 0:bdcbec496e71 | 41 | } else |
okini3939 | 0:bdcbec496e71 | 42 | if (speed < 0) { |
okini3939 | 0:bdcbec496e71 | 43 | m2p = - speed; |
okini3939 | 0:bdcbec496e71 | 44 | m2a = 0; |
okini3939 | 0:bdcbec496e71 | 45 | m2b = 1; |
okini3939 | 0:bdcbec496e71 | 46 | } else { |
okini3939 | 0:bdcbec496e71 | 47 | m2p = 1; |
okini3939 | 0:bdcbec496e71 | 48 | m2a = 0; |
okini3939 | 0:bdcbec496e71 | 49 | m2b = 0; |
okini3939 | 0:bdcbec496e71 | 50 | } |
okini3939 | 0:bdcbec496e71 | 51 | } |
okini3939 | 0:bdcbec496e71 | 52 | } |
okini3939 | 0:bdcbec496e71 | 53 | |
okini3939 | 0:bdcbec496e71 | 54 | void servo (int num, float deg) { |
okini3939 | 0:bdcbec496e71 | 55 | if (num == 3) { |
okini3939 | 0:bdcbec496e71 | 56 | m3p.period_ms(20); |
okini3939 | 0:bdcbec496e71 | 57 | m3p.pulsewidth_us(1500 + (int)(500.0 * deg)); |
okini3939 | 0:bdcbec496e71 | 58 | } else |
okini3939 | 0:bdcbec496e71 | 59 | if (num == 4) { |
okini3939 | 0:bdcbec496e71 | 60 | m4p.period_ms(20); |
okini3939 | 0:bdcbec496e71 | 61 | m4p.pulsewidth_us(1500 + (int)(500.0 * deg)); |
okini3939 | 0:bdcbec496e71 | 62 | } |
okini3939 | 0:bdcbec496e71 | 63 | } |
okini3939 | 0:bdcbec496e71 | 64 | |
okini3939 | 0:bdcbec496e71 | 65 | int main() { |
okini3939 | 0:bdcbec496e71 | 66 | float f; |
okini3939 | 0:bdcbec496e71 | 67 | |
okini3939 | 0:bdcbec496e71 | 68 | while(1) { |
okini3939 | 0:bdcbec496e71 | 69 | led1 = 1; |
okini3939 | 0:bdcbec496e71 | 70 | for (f = 0; f < 1; f += 0.02) { |
okini3939 | 0:bdcbec496e71 | 71 | motor(1, f, 0); |
okini3939 | 0:bdcbec496e71 | 72 | motor(2, f, 0); |
okini3939 | 0:bdcbec496e71 | 73 | servo(3, f); |
okini3939 | 0:bdcbec496e71 | 74 | servo(4, f); |
okini3939 | 0:bdcbec496e71 | 75 | wait(0.1); |
okini3939 | 0:bdcbec496e71 | 76 | } |
okini3939 | 0:bdcbec496e71 | 77 | led1 = 0; |
okini3939 | 0:bdcbec496e71 | 78 | led2 = 1; |
okini3939 | 0:bdcbec496e71 | 79 | for (f = 1; f > -1; f -= 0.02) { |
okini3939 | 0:bdcbec496e71 | 80 | motor(1, f, 0); |
okini3939 | 0:bdcbec496e71 | 81 | motor(2, f, 0); |
okini3939 | 0:bdcbec496e71 | 82 | servo(3, f); |
okini3939 | 0:bdcbec496e71 | 83 | servo(4, f); |
okini3939 | 0:bdcbec496e71 | 84 | wait(0.1); |
okini3939 | 0:bdcbec496e71 | 85 | } |
okini3939 | 0:bdcbec496e71 | 86 | led2 = 0; |
okini3939 | 0:bdcbec496e71 | 87 | led3 = 1; |
okini3939 | 0:bdcbec496e71 | 88 | for (f = -1; f < 0; f += 0.02) { |
okini3939 | 0:bdcbec496e71 | 89 | motor(1, f, 0); |
okini3939 | 0:bdcbec496e71 | 90 | motor(2, f, 0); |
okini3939 | 0:bdcbec496e71 | 91 | servo(3, f); |
okini3939 | 0:bdcbec496e71 | 92 | servo(4, f); |
okini3939 | 0:bdcbec496e71 | 93 | wait(0.1); |
okini3939 | 0:bdcbec496e71 | 94 | } |
okini3939 | 0:bdcbec496e71 | 95 | led3 = 0; |
okini3939 | 0:bdcbec496e71 | 96 | led4 = 1; |
okini3939 | 0:bdcbec496e71 | 97 | motor(1, 0, 1); |
okini3939 | 0:bdcbec496e71 | 98 | motor(2, 0, 1); |
okini3939 | 0:bdcbec496e71 | 99 | wait(1); |
okini3939 | 0:bdcbec496e71 | 100 | led4 = 0; |
okini3939 | 0:bdcbec496e71 | 101 | } |
okini3939 | 0:bdcbec496e71 | 102 | } |