Suga koubou
/
Motor_TB6612
see https://mbed.org/users/okini3939/notebook/motor/
Diff: main.cpp
- Revision:
- 0:bdcbec496e71
- Child:
- 1:02b46ad7dcac
diff -r 000000000000 -r bdcbec496e71 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Feb 05 02:39:17 2013 +0000 @@ -0,0 +1,102 @@ +#include "mbed.h" + +DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4); + +DigitalOut m1a(p29), m1b(p30), m2a(p27), m2b(p28); +PwmOut m1p(p26), m2p(p25); +PwmOut m3p(p23), m4p(p24); + +void motor (int num, float speed, int brake) { + if (num == 1) { + if (brake) { + m1p = 1; + m1a = 0; + m1b = 0; + } else + if (speed > 0) { + m1p = speed; + m1a = 1; + m1b = 0; + } else + if (speed < 0) { + m1p = - speed; + m1a = 0; + m1b = 1; + } else { + m1p = 1; + m1a = 0; + m1b = 0; + } + } else + if (num == 2) { + if (brake) { + m2p = 1; + m2a = 0; + m2b = 0; + } else + if (speed > 0) { + m2p = speed; + m2a = 1; + m2b = 0; + } else + if (speed < 0) { + m2p = - speed; + m2a = 0; + m2b = 1; + } else { + m2p = 1; + m2a = 0; + m2b = 0; + } + } +} + +void servo (int num, float deg) { + if (num == 3) { + m3p.period_ms(20); + m3p.pulsewidth_us(1500 + (int)(500.0 * deg)); + } else + if (num == 4) { + m4p.period_ms(20); + m4p.pulsewidth_us(1500 + (int)(500.0 * deg)); + } +} + +int main() { + float f; + + while(1) { + led1 = 1; + for (f = 0; f < 1; f += 0.02) { + motor(1, f, 0); + motor(2, f, 0); + servo(3, f); + servo(4, f); + wait(0.1); + } + led1 = 0; + led2 = 1; + for (f = 1; f > -1; f -= 0.02) { + motor(1, f, 0); + motor(2, f, 0); + servo(3, f); + servo(4, f); + wait(0.1); + } + led2 = 0; + led3 = 1; + for (f = -1; f < 0; f += 0.02) { + motor(1, f, 0); + motor(2, f, 0); + servo(3, f); + servo(4, f); + wait(0.1); + } + led3 = 0; + led4 = 1; + motor(1, 0, 1); + motor(2, 0, 1); + wait(1); + led4 = 0; + } +}