Diff: main.cpp

Revision:

0:24d1bc29e7be

diff -r 000000000000 -r 24d1bc29e7be main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 04 09:53:41 2013 +0000
@@ -0,0 +1,273 @@
+//
+//
+//
+//
+//
+//
+//
+#include "mbed.h"
+
+/** magic numbers **/
+#define HEADER_ON_TIME 1700     // 1.7[ms]
+
+/** QSTEER COMMAND INTERVALS ON EACH BANDS **/
+#define BAND_A_INTERVAL1 130000 // 130[ms]
+#define BAND_A_INTERVAL2 8000   //   8[ms]
+#define BAND_B_INTERVAL1 115000 // 115[ms]
+#define BAND_B_INTERVAL2 25000  //  25[ms]
+
+#define BAND_C_INTERVAL1 90000  //  90[ms]
+#define BAND_C_INTERVAL2 50000  //  50[ms]
+#define BAND_D_INTERVAL1 70000  //  70[ms]
+#define BAND_D_INTERVAL2 70000  //  70[ms]
+
+/** QSTEER CONTROL BANDS **/
+#define BAND_A  0x00
+#define BAND_B  0x10
+#define BAND_C  0x20
+#define BAND_D  0x30
+
+/** QSTEER MOVE COMMANDS **/
+#define COMMAND_NONE 0x00
+#define COMMAND_FWD  0x01
+#define COMMAND_BACK 0x02
+#define COMMAND_LEFT 0x03
+#define COMMAND_RIGHT 0x04
+#define COMMAND_FWD_DASH 0x05
+#define COMMAND_FWD_LEFT 0x06
+#define COMMAND_FWD_RIGHT 0x07
+#define COMMAND_FWD_LEFT_DASH 0x08
+#define COMMAND_FWD_RIGHT_DASH 0x09
+#define COMMAND_BACK_LEFT 0x0a
+#define COMMAND_BACK_RIGHT 0x0b
+#define COMMAND_BACK_DASH 0x0c
+#define COMMAND_BACK_LEFT_DASH 0x0d
+#define COMMAND_BACK_RIGHT_DASH 0x0e
+#define COMMAND_STOP 0x0f
+
+/** IR LED POWER RATE **/
+#define IR_LED_ON_POWER 0.8f
+#define IR_LED_OFF_POWER 0.0f
+
+/** input/output pin settings **/
+DigitalOut keepAliveLed(LED1); // for Board LED1 (mbed running status)
+DigitalOut myled(LED4);        // for Board LED4 (IrLED ON/OFF Status)
+DigitalIn  tactSwitch(p8);     // for Switch
+PwmOut irOut(p21);             // for IR LED
+Serial xbee(p13, p14);         // for XBee
+//Serial pc(USBTX, USBRX);     // USBTX - Tranmit on USB  USBRX - receive on USB
+
+/** Variables **/
+Timer timer;
+unsigned int receivedChar;
+
+/*
+ *    IR-LED ON/OFF
+ */
+void outputSignalInterval(int iTime, bool s)
+{
+    float signal = (s == false)? IR_LED_OFF_POWER : IR_LED_ON_POWER;
+    timer.start();
+    int offset = timer.read_us();
+    while (iTime > (timer.read_us() - offset))
+    {
+        irOut.write(signal);
+    }
+    timer.stop();                        
+}
+
+/*
+    IR-LED OFF
+ */
+void outputSignalStop()
+{
+    irOut.write(IR_LED_OFF_POWER);
+    myled = 0;
+}
+
+/*
+    SEND ON SIGNAL
+*/
+void outputSignalOn()
+{
+    outputSignalInterval(500, false);
+    outputSignalInterval(800, true);
+}
+
+/*
+    SEND OFF SIGNAL
+*/
+void outputSignalOff()
+{
+    outputSignalInterval(500, false);
+    outputSignalInterval(400, true);
+}
+
+/*
+    SEND TO IR-LED (QSTEER CONTROL)
+*/
+void sendToIrLED(unsigned char band, unsigned char command)
+{
+    unsigned char sendCommand = ((band & 0x30)|(command & 0x0f));
+
+    int interval1 = BAND_A_INTERVAL1;
+    int interval2 = BAND_A_INTERVAL2;
+
+    int checkBit1 = 0x20;
+    int checkBit2 = 0x20;
+
+    myled = 1;
+
+    // DECIDE COMMAND INTERVAL
+    if (band == BAND_A)
+    {
+        interval1 = BAND_A_INTERVAL1;
+        interval2 = BAND_A_INTERVAL2;
+    }
+    else if (band == BAND_B)
+    {
+        interval1 = BAND_B_INTERVAL1;
+        interval2 = BAND_B_INTERVAL2;
+    }
+    else if (band == BAND_C)
+    {
+        interval1 = BAND_C_INTERVAL1;
+        interval2 = BAND_C_INTERVAL2;
+    }
+    else if (band == BAND_D)
+    {
+        interval1 = BAND_D_INTERVAL1;
+        interval2 = BAND_D_INTERVAL2;
+    }
+
+    // SEND HEADER(1st)
+    outputSignalInterval(interval1, false);
+    outputSignalInterval(HEADER_ON_TIME, true);
+
+    // SEND COMMAND (1st)
+    while (checkBit1 != 0)
+    {
+        if ((checkBit1 & sendCommand) != 0)
+        {
+            outputSignalOn();
+        }
+        else
+        {
+            outputSignalOff();
+        }
+        checkBit1 = checkBit1 >> 1;
+    }
+
+    // SEND HEADER (2nd)
+    outputSignalInterval(interval2, false);
+    outputSignalInterval(HEADER_ON_TIME, true);
+
+    // SEND COMMAND (2nd)
+    while (checkBit2 != 0)
+    {
+        if ((checkBit2 & sendCommand) != 0)
+        {
+            outputSignalOn();
+        }
+        else
+        {
+            outputSignalOff();
+        }
+        checkBit2 = checkBit2 >> 1;
+    }
+
+    // IR-LED OFF
+    outputSignalStop();
+}
+
+/*
+ *  
+ *
+ */
+unsigned char decideCommand(unsigned char band, unsigned int value)
+{
+    unsigned int bandA =  (value & 0x000f);
+    unsigned int bandB = ((value & 0x00f0) >> 4);
+    unsigned int bandC = ((value & 0x0f00) >> 8);
+    unsigned int bandD = ((value & 0xf000) >> 12);  
+    unsigned char returnValue = COMMAND_NONE;
+
+    if (band == BAND_A)
+    {
+        returnValue = (unsigned char) bandA;
+    }
+    else if (band == BAND_B)
+    {
+        returnValue = (unsigned char) bandB;
+    }
+    else if (band == BAND_C)
+    {
+        returnValue = (unsigned char) bandC;
+    }
+    else // if (band == BAND_D)
+    {
+        returnValue = (unsigned char) bandD;
+    }
+    return (returnValue);
+}
+
+/*
+ *  Moving command received from HOST
+ */
+void receivedSerial()
+{
+    int ch = xbee.getc();
+    receivedChar = (unsigned int) (ch & 0xff);
+    xbee.printf("[0x%02x]", receivedChar);  // for debug echoback
+}
+
+/*
+ *
+ */
+int main()
+{
+    // set serial speed
+    xbee.baud(9600);
+    
+    // set serial callback function.
+    xbee.attach(&receivedSerial,Serial::RxIrq);
+
+    irOut.period_us(26);  // 26[us] == 38[kHz]
+
+    int count = 0;
+    float duration = 0.01;
+    bool keepAlive = true;
+    while (1)
+    {
+        if (tactSwitch == 0)
+        {
+            // Ir-LED STOP
+            outputSignalStop();
+        }
+        else
+        {
+            unsigned int checkValue = 0x0021;
+            sendToIrLED(BAND_A, decideCommand(BAND_A, checkValue));
+            sendToIrLED(BAND_B, decideCommand(BAND_B, checkValue));
+        }
+
+        if (receivedChar == 0)
+        {
+            outputSignalStop();
+        }
+        else
+        {
+            sendToIrLED(BAND_A, decideCommand(BAND_A, receivedChar));
+            sendToIrLED(BAND_B, decideCommand(BAND_B, receivedChar));
+        }
+
+        if ((count % 64) == 0)
+        {
+            // flashes led 
+            keepAlive = (keepAlive == true) ? false : true;
+        }
+        keepAliveLed = keepAlive;
+        wait(duration);
+        count++;
+    }
+}