![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |