Cast tube base station

Dependencies:   EALib USBDevice mbed

Committer:
BPPearson
Date:
Wed Dec 16 19:49:47 2015 +0000
Revision:
0:e559b5160d84
Child:
1:240b94a8d002
First production version - printf's removed and code cleaned

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BPPearson 0:e559b5160d84 1 /******************************************************************************
BPPearson 0:e559b5160d84 2 * Includes
BPPearson 0:e559b5160d84 3 *****************************************************************************/
BPPearson 0:e559b5160d84 4 #include "mbed.h"
BPPearson 0:e559b5160d84 5 #include "XBee.h"
BPPearson 0:e559b5160d84 6 #include "USBHID.h"
BPPearson 0:e559b5160d84 7 #include "ByteOperations.h"
BPPearson 0:e559b5160d84 8 #include "USBHIDProtocol.h"
BPPearson 0:e559b5160d84 9
BPPearson 0:e559b5160d84 10 /******************************************************************************
BPPearson 0:e559b5160d84 11 * Typedefs and defines
BPPearson 0:e559b5160d84 12 *****************************************************************************/
BPPearson 0:e559b5160d84 13
BPPearson 0:e559b5160d84 14 // define NODE_IS_COORDINATOR if this board should act
BPPearson 0:e559b5160d84 15 // as XBee coordinator. Make sure it is undefined if the board
BPPearson 0:e559b5160d84 16 // should act as End-Device
BPPearson 0:e559b5160d84 17 //#define NODE_IS_COORDINATOR (0)
BPPearson 0:e559b5160d84 18
BPPearson 0:e559b5160d84 19 //#define CMD_BTN_MSG (0)
BPPearson 0:e559b5160d84 20 //#define CMD_ACK_MSG (1)
BPPearson 0:e559b5160d84 21 //#define NUMBER_OF_ZONES 72
BPPearson 0:e559b5160d84 22 //#define ECHO_SERVER_PORT 7
BPPearson 0:e559b5160d84 23 #define VERSION 0x01
BPPearson 0:e559b5160d84 24 #define COMMAND 0
BPPearson 0:e559b5160d84 25 #define DATA 1
BPPearson 0:e559b5160d84 26
BPPearson 0:e559b5160d84 27 #define XBeeDisconnected 0x400
BPPearson 0:e559b5160d84 28 #define MaxTeethExceeded 0x800
BPPearson 0:e559b5160d84 29
BPPearson 0:e559b5160d84 30 // rotational stage modes
BPPearson 0:e559b5160d84 31 #define STOPPED 1
BPPearson 0:e559b5160d84 32 #define PARKING 2
BPPearson 0:e559b5160d84 33 #define GOTOMDPOSITION 3
BPPearson 0:e559b5160d84 34 #define DRIVINGTOPOSITION 4
BPPearson 0:e559b5160d84 35 #define STARTROTATING 5
BPPearson 0:e559b5160d84 36 #define ROTATING 6
BPPearson 0:e559b5160d84 37 #define MDSCAN 7
BPPearson 0:e559b5160d84 38
BPPearson 0:e559b5160d84 39 #define MAX_TEETH 384
BPPearson 0:e559b5160d84 40
BPPearson 0:e559b5160d84 41 #define LOW 0
BPPearson 0:e559b5160d84 42 #define HIGH 1
BPPearson 0:e559b5160d84 43
BPPearson 0:e559b5160d84 44
BPPearson 0:e559b5160d84 45 // input/output devices
BPPearson 0:e559b5160d84 46 static XBee xbee(P4_22, P4_23, P4_17, P4_19);
BPPearson 0:e559b5160d84 47 static DigitalIn indexInputLevel(p10);
BPPearson 0:e559b5160d84 48 static DigitalIn zeroInputLevel(p11);
BPPearson 0:e559b5160d84 49 static DigitalOut led1(LED1); // led1 + led 2 -> active low
BPPearson 0:e559b5160d84 50 static DigitalOut led2(LED2);
BPPearson 0:e559b5160d84 51 static DigitalOut led3(LED3); // led3 + led 4 -> active high
BPPearson 0:e559b5160d84 52 static DigitalOut led4(LED4);
BPPearson 0:e559b5160d84 53 static DigitalOut motorDrive(p12);
BPPearson 0:e559b5160d84 54 Serial pc(USBTX, USBRX); // tx, rx
BPPearson 0:e559b5160d84 55 Ticker lossOfSignalTimer;
BPPearson 0:e559b5160d84 56 Ticker updateLCD;
BPPearson 0:e559b5160d84 57 Ticker rotationalControl;
BPPearson 0:e559b5160d84 58 Ticker debounceDigInputs;
BPPearson 0:e559b5160d84 59 Ticker ledPulseCheck;
BPPearson 0:e559b5160d84 60 BusInOut databus(p25, p26, p27, p28, p29, p30, p31, p32);
BPPearson 0:e559b5160d84 61 DigitalOut registerSelect(p39);
BPPearson 0:e559b5160d84 62 DigitalOut readWrite(p38);
BPPearson 0:e559b5160d84 63 DigitalOut readWriteClock(p37);
BPPearson 0:e559b5160d84 64
BPPearson 0:e559b5160d84 65 USBHID *hid;
BPPearson 0:e559b5160d84 66 HID_REPORT send_report __attribute__((aligned (4))); // Aligned for fast access
BPPearson 0:e559b5160d84 67 HID_REPORT recv_report __attribute__((aligned (4))); // Aligned for fast access
BPPearson 0:e559b5160d84 68
BPPearson 0:e559b5160d84 69
BPPearson 0:e559b5160d84 70
BPPearson 0:e559b5160d84 71 // local variables
BPPearson 0:e559b5160d84 72 float rawTemperature = 0.0;
BPPearson 0:e559b5160d84 73 float rawThickness = 0.0;
BPPearson 0:e559b5160d84 74 uint32_t position = 0;
BPPearson 0:e559b5160d84 75 uint32_t seqNo = 0;
BPPearson 0:e559b5160d84 76 uint32_t prevSeqNo = 0;
BPPearson 0:e559b5160d84 77 bool xbeeIsUp = false;
BPPearson 0:e559b5160d84 78 bool saveProfileToDatabase = false;
BPPearson 0:e559b5160d84 79 char inBuff[256];
BPPearson 0:e559b5160d84 80 char outBuff[256];
BPPearson 0:e559b5160d84 81 uint8_t xbeeSigStrength = 0;
BPPearson 0:e559b5160d84 82 uint32_t errorFlags = 0;
BPPearson 0:e559b5160d84 83 int rotationalMode = STOPPED;
BPPearson 0:e559b5160d84 84 int mdPosition = 0;
BPPearson 0:e559b5160d84 85 int toothLowCount = 0;
BPPearson 0:e559b5160d84 86 int zeroLowCount = 0;
BPPearson 0:e559b5160d84 87 int zeroHighCount = 0;
BPPearson 0:e559b5160d84 88 int led1Duration = 0;
BPPearson 0:e559b5160d84 89 int led2Duration = 0;
BPPearson 0:e559b5160d84 90 int led3Duration = 0;
BPPearson 0:e559b5160d84 91 int led4Duration = 0;
BPPearson 0:e559b5160d84 92 int parkPosition = 0;
BPPearson 0:e559b5160d84 93 int indexInputState = HIGH;
BPPearson 0:e559b5160d84 94 int zeroInputState = HIGH;
BPPearson 0:e559b5160d84 95
BPPearson 0:e559b5160d84 96
BPPearson 0:e559b5160d84 97 void pulseLed(int led, int duration)
BPPearson 0:e559b5160d84 98 {
BPPearson 0:e559b5160d84 99
BPPearson 0:e559b5160d84 100 switch (led)
BPPearson 0:e559b5160d84 101 {
BPPearson 0:e559b5160d84 102 case LED1:
BPPearson 0:e559b5160d84 103 led1 = 0; // turn on led
BPPearson 0:e559b5160d84 104 led1Duration = duration; // set duration
BPPearson 0:e559b5160d84 105 break;
BPPearson 0:e559b5160d84 106 case LED2:
BPPearson 0:e559b5160d84 107 led2 = 0; // turn on led
BPPearson 0:e559b5160d84 108 led2Duration = duration; // set duration
BPPearson 0:e559b5160d84 109 break;
BPPearson 0:e559b5160d84 110 case LED3:
BPPearson 0:e559b5160d84 111 led3 = 1; // turn on led
BPPearson 0:e559b5160d84 112 led3Duration = duration; // set duration
BPPearson 0:e559b5160d84 113 break;
BPPearson 0:e559b5160d84 114 case LED4:
BPPearson 0:e559b5160d84 115 led4 = 1; // turn on led
BPPearson 0:e559b5160d84 116 led4Duration = duration; // set duration
BPPearson 0:e559b5160d84 117 break;
BPPearson 0:e559b5160d84 118 default: ;
BPPearson 0:e559b5160d84 119 }
BPPearson 0:e559b5160d84 120 }
BPPearson 0:e559b5160d84 121
BPPearson 0:e559b5160d84 122
BPPearson 0:e559b5160d84 123
BPPearson 0:e559b5160d84 124 void ledPulser()
BPPearson 0:e559b5160d84 125 {
BPPearson 0:e559b5160d84 126
BPPearson 0:e559b5160d84 127 if (led1Duration > 0)
BPPearson 0:e559b5160d84 128 {
BPPearson 0:e559b5160d84 129 led1Duration--;
BPPearson 0:e559b5160d84 130
BPPearson 0:e559b5160d84 131 if (led1Duration == 0)
BPPearson 0:e559b5160d84 132 led1 = 1; // turn led off
BPPearson 0:e559b5160d84 133 }
BPPearson 0:e559b5160d84 134
BPPearson 0:e559b5160d84 135 if (led2Duration > 0)
BPPearson 0:e559b5160d84 136 {
BPPearson 0:e559b5160d84 137 led2Duration--;
BPPearson 0:e559b5160d84 138
BPPearson 0:e559b5160d84 139 if (led2Duration == 0)
BPPearson 0:e559b5160d84 140 led2 = 1; // turn led off
BPPearson 0:e559b5160d84 141 }
BPPearson 0:e559b5160d84 142
BPPearson 0:e559b5160d84 143 if (led3Duration > 0)
BPPearson 0:e559b5160d84 144 {
BPPearson 0:e559b5160d84 145 led3Duration--;
BPPearson 0:e559b5160d84 146
BPPearson 0:e559b5160d84 147 if (led3Duration == 0)
BPPearson 0:e559b5160d84 148 led3 = 0; // turn led off
BPPearson 0:e559b5160d84 149 }
BPPearson 0:e559b5160d84 150
BPPearson 0:e559b5160d84 151 if (led4Duration > 0)
BPPearson 0:e559b5160d84 152 {
BPPearson 0:e559b5160d84 153 led4Duration--;
BPPearson 0:e559b5160d84 154
BPPearson 0:e559b5160d84 155 if (led4Duration == 0)
BPPearson 0:e559b5160d84 156 led4 = 0; // turn led off
BPPearson 0:e559b5160d84 157 }
BPPearson 0:e559b5160d84 158 }
BPPearson 0:e559b5160d84 159
BPPearson 0:e559b5160d84 160
BPPearson 0:e559b5160d84 161
BPPearson 0:e559b5160d84 162
BPPearson 0:e559b5160d84 163
BPPearson 0:e559b5160d84 164
BPPearson 0:e559b5160d84 165 void debounceInputs()
BPPearson 0:e559b5160d84 166 {
BPPearson 0:e559b5160d84 167
BPPearson 0:e559b5160d84 168
BPPearson 0:e559b5160d84 169 if (indexInputLevel == 0)
BPPearson 0:e559b5160d84 170 {
BPPearson 0:e559b5160d84 171 if (toothLowCount < 5) // times 10 mS for debounce period
BPPearson 0:e559b5160d84 172 {
BPPearson 0:e559b5160d84 173 toothLowCount++;
BPPearson 0:e559b5160d84 174
BPPearson 0:e559b5160d84 175 if (toothLowCount >= 5)
BPPearson 0:e559b5160d84 176 {
BPPearson 0:e559b5160d84 177 position++;
BPPearson 0:e559b5160d84 178
BPPearson 0:e559b5160d84 179 if (position >= MAX_TEETH)
BPPearson 0:e559b5160d84 180 {
BPPearson 0:e559b5160d84 181 position = 0;
BPPearson 0:e559b5160d84 182 errorFlags |= MaxTeethExceeded;
BPPearson 0:e559b5160d84 183 }
BPPearson 0:e559b5160d84 184 }
BPPearson 0:e559b5160d84 185 }
BPPearson 0:e559b5160d84 186 }
BPPearson 0:e559b5160d84 187 else
BPPearson 0:e559b5160d84 188 toothLowCount = 0;
BPPearson 0:e559b5160d84 189
BPPearson 0:e559b5160d84 190 if (zeroInputLevel == 0)
BPPearson 0:e559b5160d84 191 {
BPPearson 0:e559b5160d84 192 if (zeroLowCount < 5) // times 10 mS debounce period
BPPearson 0:e559b5160d84 193 {
BPPearson 0:e559b5160d84 194 zeroLowCount++;
BPPearson 0:e559b5160d84 195
BPPearson 0:e559b5160d84 196 if (zeroLowCount >= 5)
BPPearson 0:e559b5160d84 197 {
BPPearson 0:e559b5160d84 198 position = 0; // zero position counter
BPPearson 0:e559b5160d84 199 }
BPPearson 0:e559b5160d84 200 }
BPPearson 0:e559b5160d84 201 }
BPPearson 0:e559b5160d84 202 else
BPPearson 0:e559b5160d84 203 {
BPPearson 0:e559b5160d84 204 zeroLowCount = 0;
BPPearson 0:e559b5160d84 205 }
BPPearson 0:e559b5160d84 206 }
BPPearson 0:e559b5160d84 207
BPPearson 0:e559b5160d84 208
BPPearson 0:e559b5160d84 209
BPPearson 0:e559b5160d84 210
BPPearson 0:e559b5160d84 211 static void xbeeDeviceUp(void)
BPPearson 0:e559b5160d84 212 {
BPPearson 0:e559b5160d84 213 xbeeIsUp = true;
BPPearson 0:e559b5160d84 214 }
BPPearson 0:e559b5160d84 215
BPPearson 0:e559b5160d84 216
BPPearson 0:e559b5160d84 217
BPPearson 0:e559b5160d84 218 static void xbeeDeviceDown(void)
BPPearson 0:e559b5160d84 219 {
BPPearson 0:e559b5160d84 220 xbeeIsUp = false;
BPPearson 0:e559b5160d84 221 }
BPPearson 0:e559b5160d84 222
BPPearson 0:e559b5160d84 223
BPPearson 0:e559b5160d84 224
BPPearson 0:e559b5160d84 225 static void xbeeNodeFound(void)
BPPearson 0:e559b5160d84 226 {
BPPearson 0:e559b5160d84 227 uint32_t addrHi = 0;
BPPearson 0:e559b5160d84 228 uint32_t addrLo = 0;
BPPearson 0:e559b5160d84 229 uint8_t rssi = 0;
BPPearson 0:e559b5160d84 230
BPPearson 0:e559b5160d84 231 xbee.getRemoteAddress(&addrHi, &addrLo);
BPPearson 0:e559b5160d84 232 xbee.getRssi(&rssi);
BPPearson 0:e559b5160d84 233 }
BPPearson 0:e559b5160d84 234
BPPearson 0:e559b5160d84 235
BPPearson 0:e559b5160d84 236 static void xbeeTxStat(void)
BPPearson 0:e559b5160d84 237 {
BPPearson 0:e559b5160d84 238
BPPearson 0:e559b5160d84 239 uint8_t frameId = 0;
BPPearson 0:e559b5160d84 240 XBee::XBeeTxStatus status = XBee::TxStatusOk;
BPPearson 0:e559b5160d84 241
BPPearson 0:e559b5160d84 242 xbee.getTxStatus(&frameId, &status);
BPPearson 0:e559b5160d84 243 }
BPPearson 0:e559b5160d84 244
BPPearson 0:e559b5160d84 245
BPPearson 0:e559b5160d84 246
BPPearson 0:e559b5160d84 247 static void xbeeDataAvailable(void)
BPPearson 0:e559b5160d84 248 {
BPPearson 0:e559b5160d84 249 char* data = NULL;
BPPearson 0:e559b5160d84 250 uint8_t len = 0;
BPPearson 0:e559b5160d84 251 uint32_t addrHi = 0;
BPPearson 0:e559b5160d84 252 uint32_t addrLo = 0;
BPPearson 0:e559b5160d84 253
BPPearson 0:e559b5160d84 254 xbee.getRemoteAddress(&addrHi, &addrLo);
BPPearson 0:e559b5160d84 255 xbee.getData(&data, &len);
BPPearson 0:e559b5160d84 256 //xbee.getRssi(&rssi);
BPPearson 0:e559b5160d84 257
BPPearson 0:e559b5160d84 258 if (len > 0) {
BPPearson 0:e559b5160d84 259
BPPearson 0:e559b5160d84 260 switch(data[0])
BPPearson 0:e559b5160d84 261 {
BPPearson 0:e559b5160d84 262 case '1':
BPPearson 0:e559b5160d84 263 if (len > 1)
BPPearson 0:e559b5160d84 264 {
BPPearson 0:e559b5160d84 265 pulseLed( LED4, 5);
BPPearson 0:e559b5160d84 266
BPPearson 0:e559b5160d84 267 // get values from data packet
BPPearson 0:e559b5160d84 268 // packet format - sprintf(data, "1%5.3f, %5.3f, %5.3f, %5.3f, %4x\n", rawTemperature, rawThickness, filteredTemperature, filteredThickness, errorFlags);
BPPearson 0:e559b5160d84 269 sscanf(&data[1], "%f,%f, %x\n", &rawTemperature, &rawThickness, &errorFlags);
BPPearson 0:e559b5160d84 270 }
BPPearson 0:e559b5160d84 271 break;
BPPearson 0:e559b5160d84 272
BPPearson 0:e559b5160d84 273 default:
BPPearson 0:e559b5160d84 274 pc.printf("Unknown packet format %d\n", data[0]);
BPPearson 0:e559b5160d84 275 }
BPPearson 0:e559b5160d84 276 }
BPPearson 0:e559b5160d84 277 }
BPPearson 0:e559b5160d84 278
BPPearson 0:e559b5160d84 279
BPPearson 0:e559b5160d84 280
BPPearson 0:e559b5160d84 281 static bool xbeeInit()
BPPearson 0:e559b5160d84 282 {
BPPearson 0:e559b5160d84 283 xbee.registerCallback(xbeeDeviceUp, XBee::CbDeviceUp);
BPPearson 0:e559b5160d84 284
BPPearson 0:e559b5160d84 285 xbee.registerCallback(xbeeDeviceDown, XBee::CbDeviceDown);
BPPearson 0:e559b5160d84 286
BPPearson 0:e559b5160d84 287 xbee.registerCallback(xbeeNodeFound, XBee::CbNodeFound);
BPPearson 0:e559b5160d84 288
BPPearson 0:e559b5160d84 289 xbee.registerCallback(xbeeTxStat, XBee::CbTxStat);
BPPearson 0:e559b5160d84 290
BPPearson 0:e559b5160d84 291 xbee.registerCallback(xbeeDataAvailable, XBee::CbDataAvailable);
BPPearson 0:e559b5160d84 292
BPPearson 0:e559b5160d84 293 XBee::XBeeError err = xbee.init(XBee::EndDevice, "EAEA");
BPPearson 0:e559b5160d84 294
BPPearson 0:e559b5160d84 295 if (err != XBee::Ok)
BPPearson 0:e559b5160d84 296 {
BPPearson 0:e559b5160d84 297 return false;
BPPearson 0:e559b5160d84 298 }
BPPearson 0:e559b5160d84 299
BPPearson 0:e559b5160d84 300 return true;
BPPearson 0:e559b5160d84 301 }
BPPearson 0:e559b5160d84 302
BPPearson 0:e559b5160d84 303
BPPearson 0:e559b5160d84 304 static void reportInitFailed()
BPPearson 0:e559b5160d84 305 {
BPPearson 0:e559b5160d84 306 while (true) {
BPPearson 0:e559b5160d84 307 wait_ms(200);
BPPearson 0:e559b5160d84 308 }
BPPearson 0:e559b5160d84 309 }
BPPearson 0:e559b5160d84 310
BPPearson 0:e559b5160d84 311
BPPearson 0:e559b5160d84 312 static void ledInit()
BPPearson 0:e559b5160d84 313 {
BPPearson 0:e559b5160d84 314 led1 = 1; // turn off
BPPearson 0:e559b5160d84 315 led2 = 1; // turn off
BPPearson 0:e559b5160d84 316 led3 = 0; // turn off
BPPearson 0:e559b5160d84 317 led4 = 0; // turn off
BPPearson 0:e559b5160d84 318 }
BPPearson 0:e559b5160d84 319
BPPearson 0:e559b5160d84 320
BPPearson 0:e559b5160d84 321 void empty_report(HID_REPORT *data){
BPPearson 0:e559b5160d84 322 register uint32_t *p = (uint32_t *)data->data;
BPPearson 0:e559b5160d84 323 for( register int i=0; i<((sizeof(HID_REPORT)-1)/4); i++ ){
BPPearson 0:e559b5160d84 324 *p = 0xFFFFFFFF;
BPPearson 0:e559b5160d84 325 p++;
BPPearson 0:e559b5160d84 326 }
BPPearson 0:e559b5160d84 327 }
BPPearson 0:e559b5160d84 328
BPPearson 0:e559b5160d84 329
BPPearson 0:e559b5160d84 330
BPPearson 0:e559b5160d84 331 void processUSB()
BPPearson 0:e559b5160d84 332 {
BPPearson 0:e559b5160d84 333 uint8_t hiPosition;
BPPearson 0:e559b5160d84 334 uint8_t loPosition;
BPPearson 0:e559b5160d84 335
BPPearson 0:e559b5160d84 336 if(hid->readNB(&recv_report)) //try to read a msg
BPPearson 0:e559b5160d84 337 {
BPPearson 0:e559b5160d84 338 pulseLed(LED3, 5); // pulse led3 to show USB activity
BPPearson 0:e559b5160d84 339
BPPearson 0:e559b5160d84 340 // Data packet received, start parsing
BPPearson 0:e559b5160d84 341 int irx=0;
BPPearson 0:e559b5160d84 342 int itx=0;
BPPearson 0:e559b5160d84 343
BPPearson 0:e559b5160d84 344 send_report.data[itx++] = recv_report.data[0];
BPPearson 0:e559b5160d84 345
BPPearson 0:e559b5160d84 346 switch ( recv_report.data[irx++] )
BPPearson 0:e559b5160d84 347 {
BPPearson 0:e559b5160d84 348 case CMD_SYS_CHECK:
BPPearson 0:e559b5160d84 349 send_report.data[itx++] = VERSION;
BPPearson 0:e559b5160d84 350 break;
BPPearson 0:e559b5160d84 351
BPPearson 0:e559b5160d84 352 case CMD_SYS_RESET:
BPPearson 0:e559b5160d84 353 empty_report(&recv_report); // Soft reset
BPPearson 0:e559b5160d84 354 break;
BPPearson 0:e559b5160d84 355
BPPearson 0:e559b5160d84 356 case CMD_CURRENT_VALUES:
BPPearson 0:e559b5160d84 357 write_32_to_8(&itx, send_report.data, (uint32_t)(rawTemperature * 1000));
BPPearson 0:e559b5160d84 358 write_32_to_8(&itx, send_report.data, (uint32_t)(rawThickness * 1000));
BPPearson 0:e559b5160d84 359 write_32_to_8(&itx, send_report.data, position);
BPPearson 0:e559b5160d84 360 write_32_to_8(&itx, send_report.data, xbeeSigStrength);
BPPearson 0:e559b5160d84 361 write_32_to_8(&itx, send_report.data, errorFlags);
BPPearson 0:e559b5160d84 362 write_32_to_8(&itx, send_report.data, rotationalMode);
BPPearson 0:e559b5160d84 363 break;
BPPearson 0:e559b5160d84 364
BPPearson 0:e559b5160d84 365 case PARK_AT_ZERO:
BPPearson 0:e559b5160d84 366 parkPosition = MAX_TEETH - 2; // to allow it to slow donw and hit zero
BPPearson 0:e559b5160d84 367 rotationalMode = PARKING;
BPPearson 0:e559b5160d84 368 break;
BPPearson 0:e559b5160d84 369
BPPearson 0:e559b5160d84 370 case PARK_AT_POSITION:
BPPearson 0:e559b5160d84 371 hiPosition = recv_report.data[irx++];
BPPearson 0:e559b5160d84 372 loPosition = recv_report.data[irx++];
BPPearson 0:e559b5160d84 373 parkPosition = (hiPosition << 8) + loPosition;
BPPearson 0:e559b5160d84 374
BPPearson 0:e559b5160d84 375 parkPosition -= 2; // to allow it to slow down and hit the required position
BPPearson 0:e559b5160d84 376 if (parkPosition < 0)
BPPearson 0:e559b5160d84 377 parkPosition += MAX_TEETH;
BPPearson 0:e559b5160d84 378 rotationalMode = PARKING;
BPPearson 0:e559b5160d84 379 break;
BPPearson 0:e559b5160d84 380
BPPearson 0:e559b5160d84 381 case MD_SCAN:
BPPearson 0:e559b5160d84 382 hiPosition = recv_report.data[irx++];
BPPearson 0:e559b5160d84 383 loPosition = recv_report.data[irx++];
BPPearson 0:e559b5160d84 384 mdPosition = (hiPosition << 8) + loPosition;
BPPearson 0:e559b5160d84 385 rotationalMode = GOTOMDPOSITION;
BPPearson 0:e559b5160d84 386 break;
BPPearson 0:e559b5160d84 387
BPPearson 0:e559b5160d84 388 case ROTATIONAL_SCAN:
BPPearson 0:e559b5160d84 389 rotationalMode = STARTROTATING;
BPPearson 0:e559b5160d84 390 break;
BPPearson 0:e559b5160d84 391
BPPearson 0:e559b5160d84 392 case 0xEE:
BPPearson 0:e559b5160d84 393 hid->sendNB(&send_report);
BPPearson 0:e559b5160d84 394 break;
BPPearson 0:e559b5160d84 395
BPPearson 0:e559b5160d84 396 default:
BPPearson 0:e559b5160d84 397 send_report.data[0] = 0xFF; //Failure
BPPearson 0:e559b5160d84 398 break;
BPPearson 0:e559b5160d84 399 }
BPPearson 0:e559b5160d84 400
BPPearson 0:e559b5160d84 401 hid->send(&send_report); // Return command + optional new args
BPPearson 0:e559b5160d84 402
BPPearson 0:e559b5160d84 403 empty_report(&recv_report);
BPPearson 0:e559b5160d84 404 empty_report(&send_report);
BPPearson 0:e559b5160d84 405 }
BPPearson 0:e559b5160d84 406 }
BPPearson 0:e559b5160d84 407
BPPearson 0:e559b5160d84 408
BPPearson 0:e559b5160d84 409 void checkForLossOfSignal()
BPPearson 0:e559b5160d84 410 {
BPPearson 0:e559b5160d84 411
BPPearson 0:e559b5160d84 412 xbee.getRssi(&xbeeSigStrength);
BPPearson 0:e559b5160d84 413 }
BPPearson 0:e559b5160d84 414
BPPearson 0:e559b5160d84 415
BPPearson 0:e559b5160d84 416
BPPearson 0:e559b5160d84 417 void writeToLCD(bool rs, char data)
BPPearson 0:e559b5160d84 418 {
BPPearson 0:e559b5160d84 419
BPPearson 0:e559b5160d84 420 registerSelect = rs; // set register select pin
BPPearson 0:e559b5160d84 421
BPPearson 0:e559b5160d84 422 readWrite = 0; // set read/write pin to write
BPPearson 0:e559b5160d84 423
BPPearson 0:e559b5160d84 424 databus.output(); // set bus as output
BPPearson 0:e559b5160d84 425
BPPearson 0:e559b5160d84 426 databus = data; // put data onto bus
BPPearson 0:e559b5160d84 427
BPPearson 0:e559b5160d84 428 readWriteClock = 1; // pulse read/write clock
BPPearson 0:e559b5160d84 429
BPPearson 0:e559b5160d84 430 wait_us(1);
BPPearson 0:e559b5160d84 431
BPPearson 0:e559b5160d84 432 readWriteClock = 0;
BPPearson 0:e559b5160d84 433
BPPearson 0:e559b5160d84 434 wait_us(1);
BPPearson 0:e559b5160d84 435
BPPearson 0:e559b5160d84 436 databus = 0; // clear data bus
BPPearson 0:e559b5160d84 437 }
BPPearson 0:e559b5160d84 438
BPPearson 0:e559b5160d84 439
BPPearson 0:e559b5160d84 440
BPPearson 0:e559b5160d84 441 char readFromLCD(bool rs)
BPPearson 0:e559b5160d84 442 {
BPPearson 0:e559b5160d84 443
BPPearson 0:e559b5160d84 444 char data;
BPPearson 0:e559b5160d84 445
BPPearson 0:e559b5160d84 446 registerSelect = rs; // set register select pin
BPPearson 0:e559b5160d84 447
BPPearson 0:e559b5160d84 448 readWrite = 1; // set read/write pin to read
BPPearson 0:e559b5160d84 449
BPPearson 0:e559b5160d84 450 databus.input(); // set bus as input
BPPearson 0:e559b5160d84 451
BPPearson 0:e559b5160d84 452 data = databus; // read data from bus
BPPearson 0:e559b5160d84 453
BPPearson 0:e559b5160d84 454 readWriteClock = 1; // pulse read/write clock
BPPearson 0:e559b5160d84 455
BPPearson 0:e559b5160d84 456 wait_us(10);
BPPearson 0:e559b5160d84 457
BPPearson 0:e559b5160d84 458 readWriteClock = 0;
BPPearson 0:e559b5160d84 459
BPPearson 0:e559b5160d84 460 return data;
BPPearson 0:e559b5160d84 461 }
BPPearson 0:e559b5160d84 462
BPPearson 0:e559b5160d84 463
BPPearson 0:e559b5160d84 464 void resetLCD()
BPPearson 0:e559b5160d84 465 {
BPPearson 0:e559b5160d84 466 }
BPPearson 0:e559b5160d84 467
BPPearson 0:e559b5160d84 468
BPPearson 0:e559b5160d84 469 void initLCD(){
BPPearson 0:e559b5160d84 470
BPPearson 0:e559b5160d84 471 readWrite = 0; // set output so we always write to LCD
BPPearson 0:e559b5160d84 472
BPPearson 0:e559b5160d84 473 wait_ms(15); // wait 15 ms to allow LCD to initialise
BPPearson 0:e559b5160d84 474
BPPearson 0:e559b5160d84 475 writeToLCD(COMMAND, 0x30); // set interface for 8 bit mode
BPPearson 0:e559b5160d84 476
BPPearson 0:e559b5160d84 477 wait_ms(5); // give it time
BPPearson 0:e559b5160d84 478
BPPearson 0:e559b5160d84 479 writeToLCD(COMMAND, 0x30); // set interface for 8 bit mode again
BPPearson 0:e559b5160d84 480
BPPearson 0:e559b5160d84 481 wait_us(100); // give it time
BPPearson 0:e559b5160d84 482
BPPearson 0:e559b5160d84 483 writeToLCD(COMMAND, 0x30); // set interface for 8 bit mode again, last one before we can configure the display
BPPearson 0:e559b5160d84 484
BPPearson 0:e559b5160d84 485 wait_us(500); // give it time
BPPearson 0:e559b5160d84 486
BPPearson 0:e559b5160d84 487 writeToLCD(COMMAND, 0x38); // set interface for 8 bit mode, 2 display lines and 5 x 8 character font
BPPearson 0:e559b5160d84 488
BPPearson 0:e559b5160d84 489 wait_us(100); // give it time
BPPearson 0:e559b5160d84 490
BPPearson 0:e559b5160d84 491 writeToLCD(COMMAND, 0x08); // display off
BPPearson 0:e559b5160d84 492
BPPearson 0:e559b5160d84 493 wait_us(100); // give it time
BPPearson 0:e559b5160d84 494
BPPearson 0:e559b5160d84 495 writeToLCD(COMMAND, 0x01); // clear the screen
BPPearson 0:e559b5160d84 496
BPPearson 0:e559b5160d84 497 wait_ms(2); // give it time to finish
BPPearson 0:e559b5160d84 498
BPPearson 0:e559b5160d84 499 writeToLCD(COMMAND, 0x03); // set entry mode to increment cursor position cursor on write
BPPearson 0:e559b5160d84 500
BPPearson 0:e559b5160d84 501 wait_us(100); // give it time to finish
BPPearson 0:e559b5160d84 502
BPPearson 0:e559b5160d84 503 writeToLCD(COMMAND, 0x02); // position cursor at home
BPPearson 0:e559b5160d84 504
BPPearson 0:e559b5160d84 505 wait_ms(2); // give it time to finish
BPPearson 0:e559b5160d84 506
BPPearson 0:e559b5160d84 507 writeToLCD(COMMAND, 0x0C); // display on
BPPearson 0:e559b5160d84 508 }
BPPearson 0:e559b5160d84 509
BPPearson 0:e559b5160d84 510
BPPearson 0:e559b5160d84 511
BPPearson 0:e559b5160d84 512
BPPearson 0:e559b5160d84 513 void positionCursor(uint8_t x, uint8_t y)
BPPearson 0:e559b5160d84 514 {
BPPearson 0:e559b5160d84 515
BPPearson 0:e559b5160d84 516 switch (y)
BPPearson 0:e559b5160d84 517 {
BPPearson 0:e559b5160d84 518 case 0:
BPPearson 0:e559b5160d84 519 writeToLCD(COMMAND, 0x80 + 0x00 + x);
BPPearson 0:e559b5160d84 520 break;
BPPearson 0:e559b5160d84 521
BPPearson 0:e559b5160d84 522 case 1:
BPPearson 0:e559b5160d84 523 writeToLCD(COMMAND, 0x80 + 0x40 + x);
BPPearson 0:e559b5160d84 524 break;
BPPearson 0:e559b5160d84 525
BPPearson 0:e559b5160d84 526 case 2:
BPPearson 0:e559b5160d84 527 writeToLCD(COMMAND, 0x80 + 0x14 + x);
BPPearson 0:e559b5160d84 528 break;
BPPearson 0:e559b5160d84 529
BPPearson 0:e559b5160d84 530 case 3:
BPPearson 0:e559b5160d84 531 writeToLCD(COMMAND, 0x80 + 0x54 + x);
BPPearson 0:e559b5160d84 532 break;
BPPearson 0:e559b5160d84 533
BPPearson 0:e559b5160d84 534 default:
BPPearson 0:e559b5160d84 535 writeToLCD(COMMAND, 0x80 + 0x00 + x);
BPPearson 0:e559b5160d84 536 }
BPPearson 0:e559b5160d84 537
BPPearson 0:e559b5160d84 538 wait_us(50);
BPPearson 0:e559b5160d84 539 }
BPPearson 0:e559b5160d84 540
BPPearson 0:e559b5160d84 541
BPPearson 0:e559b5160d84 542
BPPearson 0:e559b5160d84 543 void clearLcdScreen()
BPPearson 0:e559b5160d84 544 {
BPPearson 0:e559b5160d84 545
BPPearson 0:e559b5160d84 546 writeToLCD(COMMAND, 0x01);
BPPearson 0:e559b5160d84 547
BPPearson 0:e559b5160d84 548 wait_ms(2);
BPPearson 0:e559b5160d84 549 }
BPPearson 0:e559b5160d84 550
BPPearson 0:e559b5160d84 551
BPPearson 0:e559b5160d84 552
BPPearson 0:e559b5160d84 553 void displayString(int x, int y, char *str)
BPPearson 0:e559b5160d84 554 {
BPPearson 0:e559b5160d84 555
BPPearson 0:e559b5160d84 556 positionCursor(x, y); // position cursor
BPPearson 0:e559b5160d84 557
BPPearson 0:e559b5160d84 558 for (int i=0; i<strlen(str); i++) // write string to screen
BPPearson 0:e559b5160d84 559 {
BPPearson 0:e559b5160d84 560 writeToLCD(DATA, str[i]);
BPPearson 0:e559b5160d84 561
BPPearson 0:e559b5160d84 562 wait_us(50);
BPPearson 0:e559b5160d84 563 }
BPPearson 0:e559b5160d84 564 }
BPPearson 0:e559b5160d84 565
BPPearson 0:e559b5160d84 566
BPPearson 0:e559b5160d84 567
BPPearson 0:e559b5160d84 568 void updateDisplay()
BPPearson 0:e559b5160d84 569 {
BPPearson 0:e559b5160d84 570 char lineStr[20];
BPPearson 0:e559b5160d84 571
BPPearson 0:e559b5160d84 572 clearLcdScreen();
BPPearson 0:e559b5160d84 573
BPPearson 0:e559b5160d84 574 sprintf( lineStr, "Thickness %4.2f mm", rawThickness);
BPPearson 0:e559b5160d84 575
BPPearson 0:e559b5160d84 576 displayString(0, 0, lineStr);
BPPearson 0:e559b5160d84 577
BPPearson 0:e559b5160d84 578 sprintf( lineStr, "Temperature %4.1f C ", rawTemperature);
BPPearson 0:e559b5160d84 579
BPPearson 0:e559b5160d84 580 displayString(0, 1, lineStr);
BPPearson 0:e559b5160d84 581
BPPearson 0:e559b5160d84 582 sprintf( lineStr, "Sig strength %2d", xbeeSigStrength);
BPPearson 0:e559b5160d84 583
BPPearson 0:e559b5160d84 584 displayString(0, 2, lineStr);
BPPearson 0:e559b5160d84 585
BPPearson 0:e559b5160d84 586 switch (rotationalMode)
BPPearson 0:e559b5160d84 587 {
BPPearson 0:e559b5160d84 588 case STOPPED:
BPPearson 0:e559b5160d84 589 displayString(0, 3, "Parked");
BPPearson 0:e559b5160d84 590 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 591 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 592 break;
BPPearson 0:e559b5160d84 593
BPPearson 0:e559b5160d84 594 case PARKING:
BPPearson 0:e559b5160d84 595 displayString(0, 3, "Parking");
BPPearson 0:e559b5160d84 596 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 597 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 598 break;
BPPearson 0:e559b5160d84 599
BPPearson 0:e559b5160d84 600 case GOTOMDPOSITION:
BPPearson 0:e559b5160d84 601 displayString(0, 3, "Go to MD pos");
BPPearson 0:e559b5160d84 602 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 603 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 604 break;
BPPearson 0:e559b5160d84 605
BPPearson 0:e559b5160d84 606 case DRIVINGTOPOSITION:
BPPearson 0:e559b5160d84 607 displayString(0, 3, "Go to MD pos");
BPPearson 0:e559b5160d84 608 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 609 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 610 break;
BPPearson 0:e559b5160d84 611
BPPearson 0:e559b5160d84 612 case STARTROTATING:
BPPearson 0:e559b5160d84 613 displayString(0, 3, "Start scan");
BPPearson 0:e559b5160d84 614 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 615 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 616 break;
BPPearson 0:e559b5160d84 617
BPPearson 0:e559b5160d84 618 case ROTATING:
BPPearson 0:e559b5160d84 619 displayString(0, 3, "Scanning");
BPPearson 0:e559b5160d84 620 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 621 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 622 break;
BPPearson 0:e559b5160d84 623
BPPearson 0:e559b5160d84 624 case MDSCAN:
BPPearson 0:e559b5160d84 625 displayString(0, 3, "MD scanning");
BPPearson 0:e559b5160d84 626 sprintf( lineStr, "%3d", position);
BPPearson 0:e559b5160d84 627 displayString(16, 3, lineStr);
BPPearson 0:e559b5160d84 628 break;
BPPearson 0:e559b5160d84 629
BPPearson 0:e559b5160d84 630 default:
BPPearson 0:e559b5160d84 631 break;
BPPearson 0:e559b5160d84 632 }
BPPearson 0:e559b5160d84 633
BPPearson 0:e559b5160d84 634 }
BPPearson 0:e559b5160d84 635
BPPearson 0:e559b5160d84 636
BPPearson 0:e559b5160d84 637 void displayDebugMsg(char *str)
BPPearson 0:e559b5160d84 638 {
BPPearson 0:e559b5160d84 639
BPPearson 0:e559b5160d84 640 clearLcdScreen();
BPPearson 0:e559b5160d84 641
BPPearson 0:e559b5160d84 642 displayString(0, 0, str);
BPPearson 0:e559b5160d84 643 }
BPPearson 0:e559b5160d84 644
BPPearson 0:e559b5160d84 645
BPPearson 0:e559b5160d84 646
BPPearson 0:e559b5160d84 647 void checkRotationMode()
BPPearson 0:e559b5160d84 648 {
BPPearson 0:e559b5160d84 649
BPPearson 0:e559b5160d84 650 switch (rotationalMode)
BPPearson 0:e559b5160d84 651 {
BPPearson 0:e559b5160d84 652 case STOPPED:
BPPearson 0:e559b5160d84 653 break;
BPPearson 0:e559b5160d84 654
BPPearson 0:e559b5160d84 655 case PARKING:
BPPearson 0:e559b5160d84 656 if (motorDrive == 0)
BPPearson 0:e559b5160d84 657 motorDrive = 1;
BPPearson 0:e559b5160d84 658
BPPearson 0:e559b5160d84 659 if (position == parkPosition)
BPPearson 0:e559b5160d84 660 {
BPPearson 0:e559b5160d84 661 motorDrive = 0;
BPPearson 0:e559b5160d84 662 rotationalMode = STOPPED;
BPPearson 0:e559b5160d84 663 }
BPPearson 0:e559b5160d84 664 break;
BPPearson 0:e559b5160d84 665
BPPearson 0:e559b5160d84 666 case GOTOMDPOSITION:
BPPearson 0:e559b5160d84 667 motorDrive = 1;
BPPearson 0:e559b5160d84 668 rotationalMode = DRIVINGTOPOSITION;
BPPearson 0:e559b5160d84 669 break;
BPPearson 0:e559b5160d84 670
BPPearson 0:e559b5160d84 671 case DRIVINGTOPOSITION:
BPPearson 0:e559b5160d84 672 if (position == mdPosition)
BPPearson 0:e559b5160d84 673 {
BPPearson 0:e559b5160d84 674 motorDrive = 0;
BPPearson 0:e559b5160d84 675 rotationalMode = MDSCAN;
BPPearson 0:e559b5160d84 676 }
BPPearson 0:e559b5160d84 677 break;
BPPearson 0:e559b5160d84 678
BPPearson 0:e559b5160d84 679 case STARTROTATING:
BPPearson 0:e559b5160d84 680 motorDrive = 1;
BPPearson 0:e559b5160d84 681 rotationalMode = ROTATING;
BPPearson 0:e559b5160d84 682 break;
BPPearson 0:e559b5160d84 683
BPPearson 0:e559b5160d84 684 case ROTATING:
BPPearson 0:e559b5160d84 685 break;
BPPearson 0:e559b5160d84 686
BPPearson 0:e559b5160d84 687 default:
BPPearson 0:e559b5160d84 688 break;
BPPearson 0:e559b5160d84 689 }
BPPearson 0:e559b5160d84 690 }
BPPearson 0:e559b5160d84 691
BPPearson 0:e559b5160d84 692
BPPearson 0:e559b5160d84 693
BPPearson 0:e559b5160d84 694
BPPearson 0:e559b5160d84 695 int main()
BPPearson 0:e559b5160d84 696 {
BPPearson 0:e559b5160d84 697
BPPearson 0:e559b5160d84 698 ledInit(); // initialise leds
BPPearson 0:e559b5160d84 699
BPPearson 0:e559b5160d84 700 motorDrive = 0; // set motor drive output to off
BPPearson 0:e559b5160d84 701
BPPearson 0:e559b5160d84 702 initLCD(); // initialize the LCD
BPPearson 0:e559b5160d84 703
BPPearson 0:e559b5160d84 704 displayDebugMsg("Initializing XBee");
BPPearson 0:e559b5160d84 705
BPPearson 0:e559b5160d84 706 if (!xbeeInit()) {
BPPearson 0:e559b5160d84 707 reportInitFailed();
BPPearson 0:e559b5160d84 708 }
BPPearson 0:e559b5160d84 709
BPPearson 0:e559b5160d84 710 // Wait until XBee node is reported to be up.
BPPearson 0:e559b5160d84 711 // - For End-device this means that the node is associated with a
BPPearson 0:e559b5160d84 712 // coordinator
BPPearson 0:e559b5160d84 713 // - For a coordinator this means that the node is initialized and ready
BPPearson 0:e559b5160d84 714 displayDebugMsg("Connecting to XBee");
BPPearson 0:e559b5160d84 715
BPPearson 0:e559b5160d84 716 while(!xbeeIsUp)
BPPearson 0:e559b5160d84 717 {
BPPearson 0:e559b5160d84 718 xbee.process();
BPPearson 0:e559b5160d84 719 }
BPPearson 0:e559b5160d84 720
BPPearson 0:e559b5160d84 721 displayDebugMsg(" "); // clear display
BPPearson 0:e559b5160d84 722
BPPearson 0:e559b5160d84 723 displayDebugMsg("Initialising USB");
BPPearson 0:e559b5160d84 724
BPPearson 0:e559b5160d84 725 static USBHID hid_object(64, 64); // USB Initialize
BPPearson 0:e559b5160d84 726 hid = &hid_object;
BPPearson 0:e559b5160d84 727 send_report.length = 64;
BPPearson 0:e559b5160d84 728
BPPearson 0:e559b5160d84 729 lossOfSignalTimer.attach(&checkForLossOfSignal, 2.0); // check for loss of signal every 2 seconds
BPPearson 0:e559b5160d84 730
BPPearson 0:e559b5160d84 731 updateLCD.attach(&updateDisplay, 1.0);
BPPearson 0:e559b5160d84 732
BPPearson 0:e559b5160d84 733 rotationalControl.attach(checkRotationMode, 0.05);
BPPearson 0:e559b5160d84 734
BPPearson 0:e559b5160d84 735 debounceDigInputs.attach(debounceInputs, 0.01);
BPPearson 0:e559b5160d84 736
BPPearson 0:e559b5160d84 737 ledPulseCheck.attach(ledPulser, 0.01);
BPPearson 0:e559b5160d84 738
BPPearson 0:e559b5160d84 739 ledInit(); // turn off all leds as initialisation complete
BPPearson 0:e559b5160d84 740
BPPearson 0:e559b5160d84 741 while (1)
BPPearson 0:e559b5160d84 742 {
BPPearson 0:e559b5160d84 743 if (xbeeIsUp)
BPPearson 0:e559b5160d84 744 {
BPPearson 0:e559b5160d84 745 xbee.process();
BPPearson 0:e559b5160d84 746
BPPearson 0:e559b5160d84 747 processUSB();
BPPearson 0:e559b5160d84 748
BPPearson 0:e559b5160d84 749 wait_ms(2);
BPPearson 0:e559b5160d84 750 }
BPPearson 0:e559b5160d84 751 else
BPPearson 0:e559b5160d84 752 {
BPPearson 0:e559b5160d84 753 errorFlags = XBeeDisconnected; // set error flag for no connection to XBee
BPPearson 0:e559b5160d84 754
BPPearson 0:e559b5160d84 755 rawTemperature = 0.0; // clear display values
BPPearson 0:e559b5160d84 756 rawThickness = 0.0;
BPPearson 0:e559b5160d84 757 }
BPPearson 0:e559b5160d84 758 }
BPPearson 0:e559b5160d84 759 }