main.cpp

00001 #include "mbed.h"
00002 
00003 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
00004 
00005 DigitalOut m1a(p29), m1b(p30), m2a(p27), m2b(p28);
00006 PwmOut m1p(p26), m2p(p25);
00007 PwmOut m3p(p23), m4p(p24);
00008 
00009 void motor (int num, float speed, int brake) {
00010     if (num == 1) {
00011         if (brake) {
00012             m1p = 1;
00013             m1a = 0;
00014             m1b = 0;
00015         } else
00016         if (speed > 0) {
00017             m1p = speed;
00018             m1a = 1;
00019             m1b = 1;
00020         } else
00021         if (speed < 0) {
00022             m1p = - speed;
00023             m1a = 0;
00024             m1b = 1;
00025         } else {
00026             m1p = 1;
00027             m1a = 0;
00028             m1b = 0;
00029         }
00030     } else
00031     if (num == 2) {
00032         if (brake) {
00033             m2p = 1;
00034             m2a = 1;
00035             m2b = 0;
00036         } else
00037         if (speed > 0) {
00038             m2p = speed;
00039             m2a = 1;
00040             m2b = 0;
00041         } else
00042         if (speed < 0) {
00043             m2p = - speed;
00044             m2a = 0;
00045             m2b = 1;
00046         } else {
00047             m2p = 1;
00048             m2a = 0;
00049             m2b = 0;
00050         }
00051     }
00052 }
00053 
00054 void servo (int num, float deg) {
00055     if (num == 3) {
00056         m3p.period_ms(20);
00057         m3p.pulsewidth_us(1500 + (int)(500.0 * deg));
00058     } else
00059     if (num == 4) {
00060         m4p.period_ms(20);
00061         m4p.pulsewidth_us(1500 + (int)(500.0 * deg));
00062     }
00063 }
00064 
00065 int main() {
00066     float f;
00067 
00068     while(1) {
00069         led1 = 1;
00070         for (f = 0; f < 1; f += 0.02) {
00071             motor(1, f, 0);
00072             motor(2, f, 0);
00073             servo(3, f);
00074             servo(4, f);
00075             wait(0.1);
00076         }
00077         led1 = 0;
00078         led2 = 1;
00079         for (f = 1; f > -1; f -= 0.02) {
00080             motor(1, f, 0);
00081             motor(2, f, 0);
00082             servo(3, f);
00083             servo(4, f);
00084             wait(0.1);
00085         }
00086         led2 = 0;
00087         led3 = 1;
00088         for (f = -1; f < 0; f += 0.02) {
00089             motor(1, f, 0);
00090             motor(2, f, 0);
00091             servo(3, f);
00092             servo(4, f);
00093             wait(0.1);
00094         }
00095         led3 = 0;
00096         led4 = 1;
00097         motor(1, 1, 0);
00098         motor(2, 1, 0);
00099         wait(1);
00100         led4 = 0;
00101         printf("Motor is driving!\r\n");
00102     }
00103 }