main.cpp

00001 //
00002 //
00003 //
00004 //
00005 //
00006 //
00007 //
00008 #include "mbed.h"
00009 
00010 /** magic numbers **/
00011 #define HEADER_ON_TIME 1700     // 1.7[ms]
00012 
00013 /** QSTEER COMMAND INTERVALS ON EACH BANDS **/
00014 #define BAND_A_INTERVAL1 130000 // 130[ms]
00015 #define BAND_A_INTERVAL2 8000   //   8[ms]
00016 #define BAND_B_INTERVAL1 115000 // 115[ms]
00017 #define BAND_B_INTERVAL2 25000  //  25[ms]
00018 
00019 #define BAND_C_INTERVAL1 90000  //  90[ms]
00020 #define BAND_C_INTERVAL2 50000  //  50[ms]
00021 #define BAND_D_INTERVAL1 70000  //  70[ms]
00022 #define BAND_D_INTERVAL2 70000  //  70[ms]
00023 
00024 /** QSTEER CONTROL BANDS **/
00025 #define BAND_A  0x00
00026 #define BAND_B  0x10
00027 #define BAND_C  0x20
00028 #define BAND_D  0x30
00029 
00030 /** QSTEER MOVE COMMANDS **/
00031 #define COMMAND_NONE 0x00
00032 #define COMMAND_FWD  0x01
00033 #define COMMAND_BACK 0x02
00034 #define COMMAND_LEFT 0x03
00035 #define COMMAND_RIGHT 0x04
00036 #define COMMAND_FWD_DASH 0x05
00037 #define COMMAND_FWD_LEFT 0x06
00038 #define COMMAND_FWD_RIGHT 0x07
00039 #define COMMAND_FWD_LEFT_DASH 0x08
00040 #define COMMAND_FWD_RIGHT_DASH 0x09
00041 #define COMMAND_BACK_LEFT 0x0a
00042 #define COMMAND_BACK_RIGHT 0x0b
00043 #define COMMAND_BACK_DASH 0x0c
00044 #define COMMAND_BACK_LEFT_DASH 0x0d
00045 #define COMMAND_BACK_RIGHT_DASH 0x0e
00046 #define COMMAND_STOP 0x0f
00047 
00048 /** IR LED POWER RATE **/
00049 #define IR_LED_ON_POWER 0.8f
00050 #define IR_LED_OFF_POWER 0.0f
00051 
00052 /** input/output pin settings **/
00053 DigitalOut keepAliveLed(LED1); // for Board LED1 (mbed running status)
00054 DigitalOut myled(LED4);        // for Board LED4 (IrLED ON/OFF Status)
00055 DigitalIn  tactSwitch(p8);     // for Switch
00056 PwmOut irOut(p21);             // for IR LED
00057 Serial xbee(p13, p14);         // for XBee
00058 //Serial pc(USBTX, USBRX);     // USBTX - Tranmit on USB  USBRX - receive on USB
00059 
00060 /** Variables **/
00061 Timer timer;
00062 unsigned int receivedChar;
00063 
00064 /*
00065  *    IR-LED ON/OFF
00066  */
00067 void outputSignalInterval(int iTime, bool s)
00068 {
00069     float signal = (s == false)? IR_LED_OFF_POWER : IR_LED_ON_POWER;
00070     timer.start();
00071     int offset = timer.read_us();
00072     while (iTime > (timer.read_us() - offset))
00073     {
00074         irOut.write(signal);
00075     }
00076     timer.stop();                        
00077 }
00078 
00079 /*
00080     IR-LED OFF
00081  */
00082 void outputSignalStop()
00083 {
00084     irOut.write(IR_LED_OFF_POWER);
00085     myled = 0;
00086 }
00087 
00088 /*
00089     SEND ON SIGNAL
00090 */
00091 void outputSignalOn()
00092 {
00093     outputSignalInterval(500, false);
00094     outputSignalInterval(800, true);
00095 }
00096 
00097 /*
00098     SEND OFF SIGNAL
00099 */
00100 void outputSignalOff()
00101 {
00102     outputSignalInterval(500, false);
00103     outputSignalInterval(400, true);
00104 }
00105 
00106 /*
00107     SEND TO IR-LED (QSTEER CONTROL)
00108 */
00109 void sendToIrLED(unsigned char band, unsigned char command)
00110 {
00111     unsigned char sendCommand = ((band & 0x30)|(command & 0x0f));
00112 
00113     int interval1 = BAND_A_INTERVAL1;
00114     int interval2 = BAND_A_INTERVAL2;
00115 
00116     int checkBit1 = 0x20;
00117     int checkBit2 = 0x20;
00118 
00119     myled = 1;
00120 
00121     // DECIDE COMMAND INTERVAL
00122     if (band == BAND_A)
00123     {
00124         interval1 = BAND_A_INTERVAL1;
00125         interval2 = BAND_A_INTERVAL2;
00126     }
00127     else if (band == BAND_B)
00128     {
00129         interval1 = BAND_B_INTERVAL1;
00130         interval2 = BAND_B_INTERVAL2;
00131     }
00132     else if (band == BAND_C)
00133     {
00134         interval1 = BAND_C_INTERVAL1;
00135         interval2 = BAND_C_INTERVAL2;
00136     }
00137     else if (band == BAND_D)
00138     {
00139         interval1 = BAND_D_INTERVAL1;
00140         interval2 = BAND_D_INTERVAL2;
00141     }
00142 
00143     // SEND HEADER(1st)
00144     outputSignalInterval(interval1, false);
00145     outputSignalInterval(HEADER_ON_TIME, true);
00146 
00147     // SEND COMMAND (1st)
00148     while (checkBit1 != 0)
00149     {
00150         if ((checkBit1 & sendCommand) != 0)
00151         {
00152             outputSignalOn();
00153         }
00154         else
00155         {
00156             outputSignalOff();
00157         }
00158         checkBit1 = checkBit1 >> 1;
00159     }
00160 
00161     // SEND HEADER (2nd)
00162     outputSignalInterval(interval2, false);
00163     outputSignalInterval(HEADER_ON_TIME, true);
00164 
00165     // SEND COMMAND (2nd)
00166     while (checkBit2 != 0)
00167     {
00168         if ((checkBit2 & sendCommand) != 0)
00169         {
00170             outputSignalOn();
00171         }
00172         else
00173         {
00174             outputSignalOff();
00175         }
00176         checkBit2 = checkBit2 >> 1;
00177     }
00178 
00179     // IR-LED OFF
00180     outputSignalStop();
00181 }
00182 
00183 /*
00184  *  
00185  *
00186  */
00187 unsigned char decideCommand(unsigned char band, unsigned int value)
00188 {
00189     unsigned int bandA =  (value & 0x000f);
00190     unsigned int bandB = ((value & 0x00f0) >> 4);
00191     unsigned int bandC = ((value & 0x0f00) >> 8);
00192     unsigned int bandD = ((value & 0xf000) >> 12);  
00193     unsigned char returnValue = COMMAND_NONE;
00194 
00195     if (band == BAND_A)
00196     {
00197         returnValue = (unsigned char) bandA;
00198     }
00199     else if (band == BAND_B)
00200     {
00201         returnValue = (unsigned char) bandB;
00202     }
00203     else if (band == BAND_C)
00204     {
00205         returnValue = (unsigned char) bandC;
00206     }
00207     else // if (band == BAND_D)
00208     {
00209         returnValue = (unsigned char) bandD;
00210     }
00211     return (returnValue);
00212 }
00213 
00214 /*
00215  *  Moving command received from HOST
00216  */
00217 void receivedSerial()
00218 {
00219     int ch = xbee.getc();
00220     receivedChar = (unsigned int) (ch & 0xff);
00221     xbee.printf("[0x%02x]", receivedChar);  // for debug echoback
00222 }
00223 
00224 /*
00225  *
00226  */
00227 int main()
00228 {
00229     // set serial speed
00230     xbee.baud(9600);
00231     
00232     // set serial callback function.
00233     xbee.attach(&receivedSerial,Serial::RxIrq);
00234 
00235     irOut.period_us(26);  // 26[us] == 38[kHz]
00236 
00237     int count = 0;
00238     float duration = 0.01;
00239     bool keepAlive = true;
00240     while (1)
00241     {
00242         if (tactSwitch == 0)
00243         {
00244             // Ir-LED STOP
00245             outputSignalStop();
00246         }
00247         else
00248         {
00249             unsigned int checkValue = 0x0021;
00250             sendToIrLED(BAND_A, decideCommand(BAND_A, checkValue));
00251             sendToIrLED(BAND_B, decideCommand(BAND_B, checkValue));
00252         }
00253 
00254         if (receivedChar == 0)
00255         {
00256             outputSignalStop();
00257         }
00258         else
00259         {
00260             sendToIrLED(BAND_A, decideCommand(BAND_A, receivedChar));
00261             sendToIrLED(BAND_B, decideCommand(BAND_B, receivedChar));
00262         }
00263 
00264         if ((count % 64) == 0)
00265         {
00266             // flashes led 
00267             keepAlive = (keepAlive == true) ? false : true;
00268         }
00269         keepAliveLed = keepAlive;
00270         wait(duration);
00271         count++;
00272     }
00273 }