Suga koubou
/
Motor_TB6612
see https://mbed.org/users/okini3939/notebook/motor/
main.cpp
- Committer:
- okini3939
- Date:
- 2013-02-05
- Revision:
- 1:02b46ad7dcac
- Parent:
- 0:bdcbec496e71
File content as of revision 1:02b46ad7dcac:
#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 = 1; m1b = 1; } 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 = 1; m2b = 1; } 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; } }