Ryoji Sakai / Mbed 2 deprecated QSteerController

This is a QSteer (as known as Choro-Q hybrid!) control program. It can control band-A and band-B at same time. Control commands are received by a serial communication(I use a XBee).

main.cpp@0:24d1bc29e7be, 2013-01-04 (annotated)

Committer:
MRSa
Date:
Fri Jan 04 09:53:41 2013 +0000
Revision:
0:24d1bc29e7be
1st edition. (A & B band control)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MRSa 0:24d1bc29e7be 1 //
MRSa 0:24d1bc29e7be 2 //
MRSa 0:24d1bc29e7be 3 //
MRSa 0:24d1bc29e7be 4 //
MRSa 0:24d1bc29e7be 5 //
MRSa 0:24d1bc29e7be 6 //
MRSa 0:24d1bc29e7be 7 //
MRSa 0:24d1bc29e7be 8 #include "mbed.h"
MRSa 0:24d1bc29e7be 9
MRSa 0:24d1bc29e7be 10 /** magic numbers **/
MRSa 0:24d1bc29e7be 11 #define HEADER_ON_TIME 1700 // 1.7[ms]
MRSa 0:24d1bc29e7be 12
MRSa 0:24d1bc29e7be 13 /** QSTEER COMMAND INTERVALS ON EACH BANDS **/
MRSa 0:24d1bc29e7be 14 #define BAND_A_INTERVAL1 130000 // 130[ms]
MRSa 0:24d1bc29e7be 15 #define BAND_A_INTERVAL2 8000 // 8[ms]
MRSa 0:24d1bc29e7be 16 #define BAND_B_INTERVAL1 115000 // 115[ms]
MRSa 0:24d1bc29e7be 17 #define BAND_B_INTERVAL2 25000 // 25[ms]
MRSa 0:24d1bc29e7be 18
MRSa 0:24d1bc29e7be 19 #define BAND_C_INTERVAL1 90000 // 90[ms]
MRSa 0:24d1bc29e7be 20 #define BAND_C_INTERVAL2 50000 // 50[ms]
MRSa 0:24d1bc29e7be 21 #define BAND_D_INTERVAL1 70000 // 70[ms]
MRSa 0:24d1bc29e7be 22 #define BAND_D_INTERVAL2 70000 // 70[ms]
MRSa 0:24d1bc29e7be 23
MRSa 0:24d1bc29e7be 24 /** QSTEER CONTROL BANDS **/
MRSa 0:24d1bc29e7be 25 #define BAND_A 0x00
MRSa 0:24d1bc29e7be 26 #define BAND_B 0x10
MRSa 0:24d1bc29e7be 27 #define BAND_C 0x20
MRSa 0:24d1bc29e7be 28 #define BAND_D 0x30
MRSa 0:24d1bc29e7be 29
MRSa 0:24d1bc29e7be 30 /** QSTEER MOVE COMMANDS **/
MRSa 0:24d1bc29e7be 31 #define COMMAND_NONE 0x00
MRSa 0:24d1bc29e7be 32 #define COMMAND_FWD 0x01
MRSa 0:24d1bc29e7be 33 #define COMMAND_BACK 0x02
MRSa 0:24d1bc29e7be 34 #define COMMAND_LEFT 0x03
MRSa 0:24d1bc29e7be 35 #define COMMAND_RIGHT 0x04
MRSa 0:24d1bc29e7be 36 #define COMMAND_FWD_DASH 0x05
MRSa 0:24d1bc29e7be 37 #define COMMAND_FWD_LEFT 0x06
MRSa 0:24d1bc29e7be 38 #define COMMAND_FWD_RIGHT 0x07
MRSa 0:24d1bc29e7be 39 #define COMMAND_FWD_LEFT_DASH 0x08
MRSa 0:24d1bc29e7be 40 #define COMMAND_FWD_RIGHT_DASH 0x09
MRSa 0:24d1bc29e7be 41 #define COMMAND_BACK_LEFT 0x0a
MRSa 0:24d1bc29e7be 42 #define COMMAND_BACK_RIGHT 0x0b
MRSa 0:24d1bc29e7be 43 #define COMMAND_BACK_DASH 0x0c
MRSa 0:24d1bc29e7be 44 #define COMMAND_BACK_LEFT_DASH 0x0d
MRSa 0:24d1bc29e7be 45 #define COMMAND_BACK_RIGHT_DASH 0x0e
MRSa 0:24d1bc29e7be 46 #define COMMAND_STOP 0x0f
MRSa 0:24d1bc29e7be 47
MRSa 0:24d1bc29e7be 48 /** IR LED POWER RATE **/
MRSa 0:24d1bc29e7be 49 #define IR_LED_ON_POWER 0.8f
MRSa 0:24d1bc29e7be 50 #define IR_LED_OFF_POWER 0.0f
MRSa 0:24d1bc29e7be 51
MRSa 0:24d1bc29e7be 52 /** input/output pin settings **/
MRSa 0:24d1bc29e7be 53 DigitalOut keepAliveLed(LED1); // for Board LED1 (mbed running status)
MRSa 0:24d1bc29e7be 54 DigitalOut myled(LED4); // for Board LED4 (IrLED ON/OFF Status)
MRSa 0:24d1bc29e7be 55 DigitalIn tactSwitch(p8); // for Switch
MRSa 0:24d1bc29e7be 56 PwmOut irOut(p21); // for IR LED
MRSa 0:24d1bc29e7be 57 Serial xbee(p13, p14); // for XBee
MRSa 0:24d1bc29e7be 58 //Serial pc(USBTX, USBRX); // USBTX - Tranmit on USB USBRX - receive on USB
MRSa 0:24d1bc29e7be 59
MRSa 0:24d1bc29e7be 60 /** Variables **/
MRSa 0:24d1bc29e7be 61 Timer timer;
MRSa 0:24d1bc29e7be 62 unsigned int receivedChar;
MRSa 0:24d1bc29e7be 63
MRSa 0:24d1bc29e7be 64 /*
MRSa 0:24d1bc29e7be 65 * IR-LED ON/OFF
MRSa 0:24d1bc29e7be 66 */
MRSa 0:24d1bc29e7be 67 void outputSignalInterval(int iTime, bool s)
MRSa 0:24d1bc29e7be 68 {
MRSa 0:24d1bc29e7be 69 float signal = (s == false)? IR_LED_OFF_POWER : IR_LED_ON_POWER;
MRSa 0:24d1bc29e7be 70 timer.start();
MRSa 0:24d1bc29e7be 71 int offset = timer.read_us();
MRSa 0:24d1bc29e7be 72 while (iTime > (timer.read_us() - offset))
MRSa 0:24d1bc29e7be 73 {
MRSa 0:24d1bc29e7be 74 irOut.write(signal);
MRSa 0:24d1bc29e7be 75 }
MRSa 0:24d1bc29e7be 76 timer.stop();
MRSa 0:24d1bc29e7be 77 }
MRSa 0:24d1bc29e7be 78
MRSa 0:24d1bc29e7be 79 /*
MRSa 0:24d1bc29e7be 80 IR-LED OFF
MRSa 0:24d1bc29e7be 81 */
MRSa 0:24d1bc29e7be 82 void outputSignalStop()
MRSa 0:24d1bc29e7be 83 {
MRSa 0:24d1bc29e7be 84 irOut.write(IR_LED_OFF_POWER);
MRSa 0:24d1bc29e7be 85 myled = 0;
MRSa 0:24d1bc29e7be 86 }
MRSa 0:24d1bc29e7be 87
MRSa 0:24d1bc29e7be 88 /*
MRSa 0:24d1bc29e7be 89 SEND ON SIGNAL
MRSa 0:24d1bc29e7be 90 */
MRSa 0:24d1bc29e7be 91 void outputSignalOn()
MRSa 0:24d1bc29e7be 92 {
MRSa 0:24d1bc29e7be 93 outputSignalInterval(500, false);
MRSa 0:24d1bc29e7be 94 outputSignalInterval(800, true);
MRSa 0:24d1bc29e7be 95 }
MRSa 0:24d1bc29e7be 96
MRSa 0:24d1bc29e7be 97 /*
MRSa 0:24d1bc29e7be 98 SEND OFF SIGNAL
MRSa 0:24d1bc29e7be 99 */
MRSa 0:24d1bc29e7be 100 void outputSignalOff()
MRSa 0:24d1bc29e7be 101 {
MRSa 0:24d1bc29e7be 102 outputSignalInterval(500, false);
MRSa 0:24d1bc29e7be 103 outputSignalInterval(400, true);
MRSa 0:24d1bc29e7be 104 }
MRSa 0:24d1bc29e7be 105
MRSa 0:24d1bc29e7be 106 /*
MRSa 0:24d1bc29e7be 107 SEND TO IR-LED (QSTEER CONTROL)
MRSa 0:24d1bc29e7be 108 */
MRSa 0:24d1bc29e7be 109 void sendToIrLED(unsigned char band, unsigned char command)
MRSa 0:24d1bc29e7be 110 {
MRSa 0:24d1bc29e7be 111 unsigned char sendCommand = ((band & 0x30)|(command & 0x0f));
MRSa 0:24d1bc29e7be 112
MRSa 0:24d1bc29e7be 113 int interval1 = BAND_A_INTERVAL1;
MRSa 0:24d1bc29e7be 114 int interval2 = BAND_A_INTERVAL2;
MRSa 0:24d1bc29e7be 115
MRSa 0:24d1bc29e7be 116 int checkBit1 = 0x20;
MRSa 0:24d1bc29e7be 117 int checkBit2 = 0x20;
MRSa 0:24d1bc29e7be 118
MRSa 0:24d1bc29e7be 119 myled = 1;
MRSa 0:24d1bc29e7be 120
MRSa 0:24d1bc29e7be 121 // DECIDE COMMAND INTERVAL
MRSa 0:24d1bc29e7be 122 if (band == BAND_A)
MRSa 0:24d1bc29e7be 123 {
MRSa 0:24d1bc29e7be 124 interval1 = BAND_A_INTERVAL1;
MRSa 0:24d1bc29e7be 125 interval2 = BAND_A_INTERVAL2;
MRSa 0:24d1bc29e7be 126 }
MRSa 0:24d1bc29e7be 127 else if (band == BAND_B)
MRSa 0:24d1bc29e7be 128 {
MRSa 0:24d1bc29e7be 129 interval1 = BAND_B_INTERVAL1;
MRSa 0:24d1bc29e7be 130 interval2 = BAND_B_INTERVAL2;
MRSa 0:24d1bc29e7be 131 }
MRSa 0:24d1bc29e7be 132 else if (band == BAND_C)
MRSa 0:24d1bc29e7be 133 {
MRSa 0:24d1bc29e7be 134 interval1 = BAND_C_INTERVAL1;
MRSa 0:24d1bc29e7be 135 interval2 = BAND_C_INTERVAL2;
MRSa 0:24d1bc29e7be 136 }
MRSa 0:24d1bc29e7be 137 else if (band == BAND_D)
MRSa 0:24d1bc29e7be 138 {
MRSa 0:24d1bc29e7be 139 interval1 = BAND_D_INTERVAL1;
MRSa 0:24d1bc29e7be 140 interval2 = BAND_D_INTERVAL2;
MRSa 0:24d1bc29e7be 141 }
MRSa 0:24d1bc29e7be 142
MRSa 0:24d1bc29e7be 143 // SEND HEADER(1st)
MRSa 0:24d1bc29e7be 144 outputSignalInterval(interval1, false);
MRSa 0:24d1bc29e7be 145 outputSignalInterval(HEADER_ON_TIME, true);
MRSa 0:24d1bc29e7be 146
MRSa 0:24d1bc29e7be 147 // SEND COMMAND (1st)
MRSa 0:24d1bc29e7be 148 while (checkBit1 != 0)
MRSa 0:24d1bc29e7be 149 {
MRSa 0:24d1bc29e7be 150 if ((checkBit1 & sendCommand) != 0)
MRSa 0:24d1bc29e7be 151 {
MRSa 0:24d1bc29e7be 152 outputSignalOn();
MRSa 0:24d1bc29e7be 153 }
MRSa 0:24d1bc29e7be 154 else
MRSa 0:24d1bc29e7be 155 {
MRSa 0:24d1bc29e7be 156 outputSignalOff();
MRSa 0:24d1bc29e7be 157 }
MRSa 0:24d1bc29e7be 158 checkBit1 = checkBit1 >> 1;
MRSa 0:24d1bc29e7be 159 }
MRSa 0:24d1bc29e7be 160
MRSa 0:24d1bc29e7be 161 // SEND HEADER (2nd)
MRSa 0:24d1bc29e7be 162 outputSignalInterval(interval2, false);
MRSa 0:24d1bc29e7be 163 outputSignalInterval(HEADER_ON_TIME, true);
MRSa 0:24d1bc29e7be 164
MRSa 0:24d1bc29e7be 165 // SEND COMMAND (2nd)
MRSa 0:24d1bc29e7be 166 while (checkBit2 != 0)
MRSa 0:24d1bc29e7be 167 {
MRSa 0:24d1bc29e7be 168 if ((checkBit2 & sendCommand) != 0)
MRSa 0:24d1bc29e7be 169 {
MRSa 0:24d1bc29e7be 170 outputSignalOn();
MRSa 0:24d1bc29e7be 171 }
MRSa 0:24d1bc29e7be 172 else
MRSa 0:24d1bc29e7be 173 {
MRSa 0:24d1bc29e7be 174 outputSignalOff();
MRSa 0:24d1bc29e7be 175 }
MRSa 0:24d1bc29e7be 176 checkBit2 = checkBit2 >> 1;
MRSa 0:24d1bc29e7be 177 }
MRSa 0:24d1bc29e7be 178
MRSa 0:24d1bc29e7be 179 // IR-LED OFF
MRSa 0:24d1bc29e7be 180 outputSignalStop();
MRSa 0:24d1bc29e7be 181 }
MRSa 0:24d1bc29e7be 182
MRSa 0:24d1bc29e7be 183 /*
MRSa 0:24d1bc29e7be 184 *
MRSa 0:24d1bc29e7be 185 *
MRSa 0:24d1bc29e7be 186 */
MRSa 0:24d1bc29e7be 187 unsigned char decideCommand(unsigned char band, unsigned int value)
MRSa 0:24d1bc29e7be 188 {
MRSa 0:24d1bc29e7be 189 unsigned int bandA = (value & 0x000f);
MRSa 0:24d1bc29e7be 190 unsigned int bandB = ((value & 0x00f0) >> 4);
MRSa 0:24d1bc29e7be 191 unsigned int bandC = ((value & 0x0f00) >> 8);
MRSa 0:24d1bc29e7be 192 unsigned int bandD = ((value & 0xf000) >> 12);
MRSa 0:24d1bc29e7be 193 unsigned char returnValue = COMMAND_NONE;
MRSa 0:24d1bc29e7be 194
MRSa 0:24d1bc29e7be 195 if (band == BAND_A)
MRSa 0:24d1bc29e7be 196 {
MRSa 0:24d1bc29e7be 197 returnValue = (unsigned char) bandA;
MRSa 0:24d1bc29e7be 198 }
MRSa 0:24d1bc29e7be 199 else if (band == BAND_B)
MRSa 0:24d1bc29e7be 200 {
MRSa 0:24d1bc29e7be 201 returnValue = (unsigned char) bandB;
MRSa 0:24d1bc29e7be 202 }
MRSa 0:24d1bc29e7be 203 else if (band == BAND_C)
MRSa 0:24d1bc29e7be 204 {
MRSa 0:24d1bc29e7be 205 returnValue = (unsigned char) bandC;
MRSa 0:24d1bc29e7be 206 }
MRSa 0:24d1bc29e7be 207 else // if (band == BAND_D)
MRSa 0:24d1bc29e7be 208 {
MRSa 0:24d1bc29e7be 209 returnValue = (unsigned char) bandD;
MRSa 0:24d1bc29e7be 210 }
MRSa 0:24d1bc29e7be 211 return (returnValue);
MRSa 0:24d1bc29e7be 212 }
MRSa 0:24d1bc29e7be 213
MRSa 0:24d1bc29e7be 214 /*
MRSa 0:24d1bc29e7be 215 * Moving command received from HOST
MRSa 0:24d1bc29e7be 216 */
MRSa 0:24d1bc29e7be 217 void receivedSerial()
MRSa 0:24d1bc29e7be 218 {
MRSa 0:24d1bc29e7be 219 int ch = xbee.getc();
MRSa 0:24d1bc29e7be 220 receivedChar = (unsigned int) (ch & 0xff);
MRSa 0:24d1bc29e7be 221 xbee.printf("[0x%02x]", receivedChar); // for debug echoback
MRSa 0:24d1bc29e7be 222 }
MRSa 0:24d1bc29e7be 223
MRSa 0:24d1bc29e7be 224 /*
MRSa 0:24d1bc29e7be 225 *
MRSa 0:24d1bc29e7be 226 */
MRSa 0:24d1bc29e7be 227 int main()
MRSa 0:24d1bc29e7be 228 {
MRSa 0:24d1bc29e7be 229 // set serial speed
MRSa 0:24d1bc29e7be 230 xbee.baud(9600);
MRSa 0:24d1bc29e7be 231
MRSa 0:24d1bc29e7be 232 // set serial callback function.
MRSa 0:24d1bc29e7be 233 xbee.attach(&receivedSerial,Serial::RxIrq);
MRSa 0:24d1bc29e7be 234
MRSa 0:24d1bc29e7be 235 irOut.period_us(26); // 26[us] == 38[kHz]
MRSa 0:24d1bc29e7be 236
MRSa 0:24d1bc29e7be 237 int count = 0;
MRSa 0:24d1bc29e7be 238 float duration = 0.01;
MRSa 0:24d1bc29e7be 239 bool keepAlive = true;
MRSa 0:24d1bc29e7be 240 while (1)
MRSa 0:24d1bc29e7be 241 {
MRSa 0:24d1bc29e7be 242 if (tactSwitch == 0)
MRSa 0:24d1bc29e7be 243 {
MRSa 0:24d1bc29e7be 244 // Ir-LED STOP
MRSa 0:24d1bc29e7be 245 outputSignalStop();
MRSa 0:24d1bc29e7be 246 }
MRSa 0:24d1bc29e7be 247 else
MRSa 0:24d1bc29e7be 248 {
MRSa 0:24d1bc29e7be 249 unsigned int checkValue = 0x0021;
MRSa 0:24d1bc29e7be 250 sendToIrLED(BAND_A, decideCommand(BAND_A, checkValue));
MRSa 0:24d1bc29e7be 251 sendToIrLED(BAND_B, decideCommand(BAND_B, checkValue));
MRSa 0:24d1bc29e7be 252 }
MRSa 0:24d1bc29e7be 253
MRSa 0:24d1bc29e7be 254 if (receivedChar == 0)
MRSa 0:24d1bc29e7be 255 {
MRSa 0:24d1bc29e7be 256 outputSignalStop();
MRSa 0:24d1bc29e7be 257 }
MRSa 0:24d1bc29e7be 258 else
MRSa 0:24d1bc29e7be 259 {
MRSa 0:24d1bc29e7be 260 sendToIrLED(BAND_A, decideCommand(BAND_A, receivedChar));
MRSa 0:24d1bc29e7be 261 sendToIrLED(BAND_B, decideCommand(BAND_B, receivedChar));
MRSa 0:24d1bc29e7be 262 }
MRSa 0:24d1bc29e7be 263
MRSa 0:24d1bc29e7be 264 if ((count % 64) == 0)
MRSa 0:24d1bc29e7be 265 {
MRSa 0:24d1bc29e7be 266 // flashes led
MRSa 0:24d1bc29e7be 267 keepAlive = (keepAlive == true) ? false : true;
MRSa 0:24d1bc29e7be 268 }
MRSa 0:24d1bc29e7be 269 keepAliveLed = keepAlive;
MRSa 0:24d1bc29e7be 270 wait(duration);
MRSa 0:24d1bc29e7be 271 count++;
MRSa 0:24d1bc29e7be 272 }
MRSa 0:24d1bc29e7be 273 }