main.cpp

00001 #include "mbed.h"
00002 #include "pins.h"
00003 #include "motor.h"
00004 #include "RGBLed.hpp"
00005 #include "USBSerial.h"
00006 #include "RFManager.h"
00007 
00008 USBSerial serial;
00009 
00010 Serial pc(USBTX, USBRX);
00011 
00012 RGBLed led1(LED1R, LED1G, LED1B);
00013 RGBLed led2(LED2R, LED2G, LED2B);
00014 
00015 DigitalIn infrared(ADC0);
00016 
00017 Timeout kicker;
00018 
00019 static const int NUMBER_OF_MOTORS = 3;
00020 
00021 Motor motor0(&pc, M0_PWM, M0_DIR1, M0_DIR2, M0_FAULT, M0_ENCA, M0_ENCB);
00022 Motor motor1(&pc, M1_PWM, M1_DIR1, M1_DIR2, M1_FAULT, M1_ENCA, M1_ENCB);
00023 Motor motor2(&pc, M2_PWM, M2_DIR1, M2_DIR2, M2_FAULT, M2_ENCA, M2_ENCB);
00024 //Motor motor3(&pc, M3_PWM, M3_DIR1, M3_DIR2, M3_FAULT, M3_ENCA, M3_ENCB);
00025 
00026 Motor * motors[NUMBER_OF_MOTORS] = {
00027   &motor0, &motor1, &motor2/*, &motor3*/
00028 };
00029 
00030 PwmOut m0(M0_PWM);
00031 PwmOut m1(M1_PWM);
00032 PwmOut m2(M2_PWM);
00033 PwmOut m3(M3_PWM);
00034 PwmOut pwm0(PWM0);
00035 PwmOut pwm1(PWM1);
00036 
00037 RFManager rfModule(COMTX, COMRX);
00038 
00039 void serialInterrupt();
00040 void parseCommand(char *command);
00041 
00042 Ticker pidTicker;
00043 int pidTickerCount = 0;
00044 static const float PID_FREQ = 60;
00045 
00046 char buf[32];
00047 int serialCount = 0;
00048 bool serialData = false;
00049 
00050 bool failSafeEnabled = true;
00051 int ticksSinceCommand = 0;
00052 
00053 void pidTick() {
00054   for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00055     motors[i]->pidTick();
00056   }
00057 
00058   if (pidTickerCount++ % 25 == 0) {
00059     led1.setBlue(!led1.getBlue());
00060   }
00061 
00062   // Fail-safe
00063   if (failSafeEnabled) {
00064     ticksSinceCommand++;
00065   }
00066 
00067   if (ticksSinceCommand == 60) {
00068     for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
00069       motors[i]->setSpeed(0);
00070     }
00071 
00072     m3.pulsewidth_us(100);
00073   }
00074 }
00075 
00076 int main() {
00077   pidTicker.attach(pidTick, 1/PID_FREQ);
00078   //serial.attach(&serialInterrupt);
00079 
00080   // Ball detector status
00081   int infraredStatus = -1;
00082 
00083   // Dribbler motor
00084   m3.period_us(20000);
00085   m3.pulsewidth_us(100);
00086 
00087   while (1) {
00088     if (rfModule.readable()) {
00089         serial.printf("<ref:%s>\n", rfModule.read());
00090     }
00091 
00092     rfModule.update();
00093 
00094     if (serial.readable()) {
00095       buf[serialCount] = serial.getc();
00096       //serial.putc(buf[serialCount]);
00097 
00098       if (buf[serialCount] == '\n') {
00099         parseCommand(buf);
00100         serialCount = 0;
00101         memset(buf, 0, 32);
00102       } else {
00103         serialCount++;
00104       }
00105     }
00106 
00107     /// INFRARED DETECTOR
00108     int newInfraredStatus = infrared.read();
00109 
00110     if (newInfraredStatus != infraredStatus) {
00111       infraredStatus = newInfraredStatus;
00112       serial.printf("<ball:%d>\n", newInfraredStatus);
00113       led2.setGreen(infraredStatus);
00114     }
00115   }
00116 }
00117 
00118 void parseCommand(char *buffer) {
00119   ticksSinceCommand = 0;
00120 
00121   char *cmd = strtok(buffer, ":");
00122 
00123   // buffer == "sd:14:16:10:30"
00124   if (strncmp(cmd, "sd", 2) == 0) {
00125     for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
00126       motors[i]->setSpeed((int16_t) atoi(strtok(NULL, ":")));
00127     }
00128 
00129     /*serial.printf("<gs:%d:%d:%d:%d>\n",
00130       motors[0]->getSpeed(),
00131       motors[1]->getSpeed(),
00132       motors[2]->getSpeed(),
00133       motors[3]->getSpeed()
00134     );*/
00135     
00136     serial.printf("<gs:%d:%d:%d>\n",
00137       motors[0]->getSpeed(),
00138       motors[1]->getSpeed(),
00139       motors[2]->getSpeed()
00140     );
00141   }
00142 
00143   if (strncmp(cmd, "d", 1) == 0) {
00144     /*
00145     if (command[1] == '0') {
00146       pwm1.pulsewidth_us(100);
00147     } else if (command[1] == '1') {
00148       pwm1.pulsewidth_us(268);
00149     } else*/ {
00150       m3.pulsewidth_us(atoi(buffer + 2));
00151     }
00152     //pwm1.pulsewidth_us((int) atoi(command+1));
00153     //serial.printf("sending %d\n", (int) atoi(command+1));
00154   }
00155 
00156   if (strncmp(cmd, "gs", 2) == 0) {
00157     /*serial.printf("<gs:%d:%d:%d:%d>\n",
00158       motors[0]->getSpeed(),
00159       motors[1]->getSpeed(),
00160       motors[2]->getSpeed(),
00161       motors[3]->getSpeed()
00162     );*/
00163     
00164     serial.printf("<gs:%d:%d:%d>\n",
00165       motors[0]->getSpeed(),
00166       motors[1]->getSpeed(),
00167       motors[2]->getSpeed()
00168     );
00169   }
00170 
00171   if (strncmp(cmd, "rf", 2) == 0) {
00172        rfModule.send(buffer + 3);
00173   }
00174 
00175   if (strncmp(cmd, "r", 1) == 0) {
00176     led1.setRed(!led1.getRed());
00177   }
00178 
00179   if (strncmp(cmd, "g", 1) == 0) {
00180     led1.setGreen(!led1.getGreen());
00181   }
00182 
00183   if (strncmp(cmd, "b", 1) == 0) {
00184     led1.setBlue(!led1.getBlue());
00185   }
00186 
00187   if (strncmp(cmd, "gb", 2) == 0) {
00188     serial.printf("<ball:%d>\n", infrared.read());
00189   }
00190 
00191   if (strncmp(cmd, "fs", 1) == 0) {
00192     failSafeEnabled = buffer[3] != '0';
00193   }
00194 }