main.cpp

00001 #include "mbed.h"
00002 #include "pins.h"
00003 #include "motor.h"
00004 #include "USBSerial.h"
00005 #include "RFManager.h"
00006 
00007 #define BUF_SIZE 32
00008 
00009 USBSerial serial;
00010 
00011 Serial pc(USBTX, USBRX);
00012 
00013 DigitalOut led1(LED_1);
00014 DigitalOut led2(LED_2);
00015 DigitalOut led3(LED_3);
00016 
00017 static const int NUMBER_OF_MOTORS = 3;
00018 
00019 PwmOut m3(M3_PWM);
00020 
00021 Motor motor0(M0_PWM, M0_DIR1, M0_DIR2, M0_ENCA, M0_ENCB);
00022 Motor motor1(M1_PWM, M1_DIR1, M1_DIR2, M1_ENCA, M1_ENCB);
00023 Motor motor2(M2_PWM, M2_DIR1, M2_DIR2, M2_ENCA, M2_ENCB);
00024 //Motor motor3(M3_PWM, M3_DIR1, M3_DIR2, M3_ENCA, M3_ENCB);
00025 
00026 Motor * motors[NUMBER_OF_MOTORS] = {
00027     &motor0, &motor1, &motor2
00028 };
00029 
00030 DigitalOut servo(ISO_PWM5);
00031 
00032 RFManager rfModule(COM1_TX, COM1_RX);
00033 
00034 void parseCommand(char *command);
00035 
00036 Ticker pidTicker;
00037 unsigned int pidTickerCount = 0;
00038 static const float PID_FREQ = 60;
00039 
00040 char buf[BUF_SIZE];
00041 int serialCount = 0;
00042 bool serialData = false;
00043 
00044 bool failSafeEnabled = true;
00045 int failSafeCount = 0;
00046 int failSafeLimit = 60;
00047 
00048 Ticker softPWMTicker;
00049 Timeout softPWMTimeout;
00050 int currentServoPosition = 0;
00051 
00052 void pidTick() {
00053     for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00054         motors[i]->pid();
00055     }
00056 
00057     if (pidTickerCount++ % 60 == 0) {
00058         led1 = !led1;
00059     }
00060 
00061     failSafeCount++;
00062     
00063     if (failSafeCount == failSafeLimit) {
00064         failSafeCount = 0;
00065         
00066         if (failSafeEnabled) {
00067             for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
00068                 motors[i]->setSpeed(0);
00069             }
00070 
00071             m3.pulsewidth_us(100);
00072         }
00073     }
00074 }
00075 
00076 void softPWMEndPulse() {
00077     servo = 0;
00078 }
00079 
00080 void softPWMTick() {
00081     if(currentServoPosition <= 0) {
00082         servo = 0 ;
00083     } else {
00084         servo = 1;
00085         softPWMTimeout.attach_us(softPWMEndPulse, currentServoPosition);
00086     }
00087 }
00088 
00089 int main() {
00090     pc.baud(115200);
00091     
00092     pidTicker.attach(pidTick, 1 / PID_FREQ);
00093 
00094     m3.pulsewidth_us(100);
00095     //servo.pulsewidth_us(0);
00096     
00097     // TGY-180D (KC2462 controller) has problems with higher frequency PWM.
00098     // 50Hz works, but would like to use higher frequency for motors.
00099     // Software PWM seems to be good enough.    
00100     softPWMTicker.attach_us(softPWMTick, 20000);
00101 
00102     while (1) {
00103         if (rfModule.readable()) {
00104             serial.printf("<ref:%s>\n", rfModule.read());
00105         }
00106 
00107         rfModule.update();
00108 
00109         while (serial.readable()) {
00110             char c = serial.getc();
00111             
00112             buf[serialCount] = c;
00113 
00114             if (c == '\n') {
00115                 parseCommand(buf);
00116                 serialCount = 0;
00117                 memset(buf, 0, BUF_SIZE);
00118             } else {
00119                 serialCount++;
00120                 
00121                 if (serialCount == BUF_SIZE) {
00122                     serialCount = 0;
00123                 }
00124             }
00125         }
00126     }
00127 }
00128 
00129 void parseCommand(char *buffer) {
00130     failSafeCount = 0;
00131 
00132     char *cmd = strtok(buffer, ":");
00133 
00134     // buffer == "sd:14:16:10:30"
00135     if (strncmp(cmd, "sd", 2) == 0) {
00136         for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
00137             motors[i]->setSpeed(atoi(strtok(NULL, ":")));
00138         }
00139 
00140         serial.printf("<gs:%d:%d:%d>\n",
00141             motors[0]->getSpeed(),
00142             motors[1]->getSpeed(),
00143             motors[2]->getSpeed()
00144          );
00145          
00146     } else if (strncmp(cmd, "d", 1) == 0) {
00147         int pulsewidth = atoi(buffer + 2);
00148         
00149         /*if (pulsewidth < 800) {
00150             pulsewidth = 800;
00151         } else if (pulsewidth > 2100) {
00152             pulsewidth = 2100;
00153         }*/
00154         
00155         m3.pulsewidth_us(pulsewidth);
00156              
00157     } else if (strncmp(cmd, "sv", 2) == 0) {
00158         currentServoPosition = atoi(buffer + 3);
00159         
00160         if (currentServoPosition < 700) {
00161             currentServoPosition = 0;
00162         } else if (currentServoPosition > 2300) {
00163             currentServoPosition = 2300;
00164         }
00165         
00166         //servo.pulsewidth_us(currentServoPosition);
00167              
00168     } else if (strncmp(cmd, "gs", 2) == 0) {
00169         serial.printf("<gs:%d:%d:%d>\n",
00170             motors[0]->getSpeed(),
00171             motors[1]->getSpeed(),
00172             motors[2]->getSpeed()
00173         );
00174     } else if (strncmp(cmd, "rf", 2) == 0) {
00175         rfModule.send(buffer + 3);
00176         
00177     } else if (strncmp(cmd, "fs", 1) == 0) {
00178         failSafeEnabled = buffer[3] != '0';
00179     }
00180 }