Shih-Ho Hsieh / Mbed 2 deprecated Motor_XYZ_UI_SPI_8mag

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   XYZ_sensor_Platform_SPI mbed

ui.cpp@15:67c0fbc0999a, 2018-08-03 (annotated)

Committer:
hober
Date:
Fri Aug 03 01:51:51 2018 +0000
Branch:
envelope
Revision:
15:67c0fbc0999a
Parent:
14:9672e91010a3 
Motor XYZ with UI through SPI interface to extend board magring which contains 8 magnetometers.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hober 6:ce02d396c961 1 #include "motor.h"
hober 6:ce02d396c961 2 #include "xyz_sensor_platform.h"
hober 8:33d34a775873 3 #include "envelopetracker.h"
hober 6:ce02d396c961 4
hober 7:ee0569d49c52 5 #define I2C_FREQUENCY 400000
hober 15:67c0fbc0999a 6 #define SPI_FREQUENCY 0.1e7
hober 15:67c0fbc0999a 7 //#define DELAY_COMPENSATE 0.000537
hober 15:67c0fbc0999a 8 #define BIG_CHAR_MASK 0x1F
hober 15:67c0fbc0999a 9 #define BAUD 921600
hober 15:67c0fbc0999a 10 #define Fs 1e3 // sampling rate -- max: 1kHz
hober 6:ce02d396c961 11
hober 8:33d34a775873 12 typedef unsigned char byte;
hober 10:a49cdab3810f 13 uint8_t* dataToSend;
hober 8:33d34a775873 14 int sendArrayIndex = 0;
hober 8:33d34a775873 15 int sendBufferMax = 10000;
hober 8:33d34a775873 16 Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
hober 7:ee0569d49c52 17 DigitalOut led(LED2),led3(LED3);
hober 15:67c0fbc0999a 18 DigitalOut mag_test(PC_8);
hober 12:2e1b1b1726fa 19 InterruptIn button(USER_BUTTON);
hober 15:67c0fbc0999a 20 Timeout nextRecord;
hober 6:ce02d396c961 21 XYZSensorPlatform platform;
hober 8:33d34a775873 22 Envelope *command;
hober 8:33d34a775873 23 EnvelopeTracker tracker;
hober 7:ee0569d49c52 24 byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
hober 7:ee0569d49c52 25 bool isRecording = false;
hober 9:c4f7257dee47 26 Envelope* result;
hober 9:c4f7257dee47 27 float x, y, z;
hober 9:c4f7257dee47 28 float pos[3];
hober 9:c4f7257dee47 29 int n = 0;
hober 9:c4f7257dee47 30 int getMag = 0;
hober 15:67c0fbc0999a 31 char magSel = 0;
hober 9:c4f7257dee47 32 int leftCount = 0;
hober 9:c4f7257dee47 33 int rightCount = 0;
hober 9:c4f7257dee47 34 int upCount = 0;
hober 9:c4f7257dee47 35 int downCount = 0;
hober 9:c4f7257dee47 36 int forwardCount = 0;
hober 9:c4f7257dee47 37 int backwardCount = 0;
hober 9:c4f7257dee47 38 bool commandToDo = false;
hober 9:c4f7257dee47 39 int recordTime;
hober 9:c4f7257dee47 40 float waitTime;
hober 12:2e1b1b1726fa 41 bool isEcho = false;
hober 15:67c0fbc0999a 42 bool isReset = false;
hober 15:67c0fbc0999a 43 bool isTimeToRecord = false;
hober 15:67c0fbc0999a 44 bool isMagReset = false;
hober 15:67c0fbc0999a 45 uint8_t magData[80] = {0}; //80 bytes
hober 12:2e1b1b1726fa 46 enum PatternRecordState{NONE,INIT,MAGNET,TO};
hober 12:2e1b1b1726fa 47 struct Pattern
hober 12:2e1b1b1726fa 48 {
hober 12:2e1b1b1726fa 49 float xStart, xEnd, xStep, yStart, yEnd, yStep, zStart, zEnd, zStep;
hober 12:2e1b1b1726fa 50 int num;
hober 12:2e1b1b1726fa 51 float x, y, z;
hober 12:2e1b1b1726fa 52 int count;
hober 12:2e1b1b1726fa 53 PatternRecordState state;
hober 12:2e1b1b1726fa 54 } pattern;
hober 12:2e1b1b1726fa 55
hober 8:33d34a775873 56
hober 8:33d34a775873 57
hober 6:ce02d396c961 58 void echo(char typ, float x, float y, float z);
hober 15:67c0fbc0999a 59 uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow = true);
hober 9:c4f7257dee47 60 void Rx_interrupt();
hober 15:67c0fbc0999a 61 void recordTimeup()
hober 15:67c0fbc0999a 62 {
hober 15:67c0fbc0999a 63 isTimeToRecord = true;
hober 15:67c0fbc0999a 64 }
hober 12:2e1b1b1726fa 65 void released()
hober 12:2e1b1b1726fa 66 {
hober 12:2e1b1b1726fa 67 led = !led;
hober 12:2e1b1b1726fa 68 pc.attach(&Rx_interrupt, Serial::RxIrq);
hober 12:2e1b1b1726fa 69 }
hober 15:67c0fbc0999a 70 void no_call(){}
hober 8:33d34a775873 71
hober 6:ce02d396c961 72 int main()
hober 6:ce02d396c961 73 {
hober 6:ce02d396c961 74 led=1;
hober 15:67c0fbc0999a 75 // char msg[] = "start!!";
hober 15:67c0fbc0999a 76 // fwrite(msg,1, 7,stdout);
hober 8:33d34a775873 77 command = new Envelope;
hober 8:33d34a775873 78 command->enableHeader(std::string("H"));// 48 H
hober 8:33d34a775873 79 command->enableFooter(std::string("E"),8);// 45 E
hober 8:33d34a775873 80 command->enableCheckXOR(9);
hober 8:33d34a775873 81 tracker.setEnvelope(*command);
hober 8:33d34a775873 82 tracker.setBufferLength(100);
hober 12:2e1b1b1726fa 83 pattern.state = NONE;
hober 7:ee0569d49c52 84 pc.format(8,SerialBase::None,1);
hober 15:67c0fbc0999a 85 // button.rise(&released);
hober 6:ce02d396c961 86 platform.set_speed(2.5);
hober 12:2e1b1b1726fa 87 // platform.reset(); // need to be modified here
hober 15:67c0fbc0999a 88 platform.setSensorSpiFrequency(SPI_FREQUENCY);
hober 9:c4f7257dee47 89 // Setup a serial interrupt function to receive data
hober 9:c4f7257dee47 90 pc.attach(&Rx_interrupt, Serial::RxIrq);
hober 15:67c0fbc0999a 91 // echo('B',0,0,0);
hober 6:ce02d396c961 92 while(1) {
hober 15:67c0fbc0999a 93 if(platform.isMoving()) continue;
hober 12:2e1b1b1726fa 94 if(isReset){
hober 12:2e1b1b1726fa 95 platform.set_speed(1);
hober 12:2e1b1b1726fa 96 platform.reset();
hober 12:2e1b1b1726fa 97 isReset = false;
hober 12:2e1b1b1726fa 98 platform.set_speed(2.5);
hober 12:2e1b1b1726fa 99 }
hober 12:2e1b1b1726fa 100 if(pattern.state != NONE)
hober 12:2e1b1b1726fa 101 {
hober 13:6850d2b41b2c 102 if(pattern.state == INIT)
hober 13:6850d2b41b2c 103 {
hober 13:6850d2b41b2c 104 pattern.x = pattern.xStart;
hober 13:6850d2b41b2c 105 pattern.y = pattern.yStart;
hober 13:6850d2b41b2c 106 pattern.z = pattern.zStart;
hober 13:6850d2b41b2c 107 pattern.count = pattern.num;
hober 13:6850d2b41b2c 108 pattern.state = MAGNET;
hober 15:67c0fbc0999a 109 //isEcho = true;
hober 15:67c0fbc0999a 110 platform.to(pattern.x,pattern.y,pattern.z);
hober 13:6850d2b41b2c 111 isEcho = true;
hober 15:67c0fbc0999a 112 continue;
hober 13:6850d2b41b2c 113 }
hober 12:2e1b1b1726fa 114 if(pattern.count == 0) pattern.state = TO;
hober 12:2e1b1b1726fa 115 if(pattern.state == TO)
hober 12:2e1b1b1726fa 116 {
hober 13:6850d2b41b2c 117 if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep))
hober 13:6850d2b41b2c 118 {
hober 13:6850d2b41b2c 119 pattern.x += pattern.xStep;
hober 13:6850d2b41b2c 120 platform.set_speed(1.5);
hober 13:6850d2b41b2c 121 }
hober 12:2e1b1b1726fa 122 else if(abs(pattern.y-pattern.yEnd)>abs(pattern.yStep)){
hober 12:2e1b1b1726fa 123 pattern.x = pattern.xStart;
hober 12:2e1b1b1726fa 124 pattern.y += pattern.yStep;
hober 12:2e1b1b1726fa 125 }
hober 12:2e1b1b1726fa 126 else if(abs(pattern.z-pattern.zEnd)>abs(pattern.zStep)){
hober 12:2e1b1b1726fa 127 pattern.x = pattern.xStart;
hober 12:2e1b1b1726fa 128 pattern.y = pattern.yStart;
hober 12:2e1b1b1726fa 129 pattern.z += pattern.zStep;
hober 12:2e1b1b1726fa 130 }
hober 12:2e1b1b1726fa 131 else{
hober 12:2e1b1b1726fa 132 pattern.state = NONE;
hober 13:6850d2b41b2c 133 echo('S',0,0,0);
hober 12:2e1b1b1726fa 134 continue;
hober 12:2e1b1b1726fa 135 }
hober 12:2e1b1b1726fa 136 platform.to(pattern.x,pattern.y,pattern.z);
hober 12:2e1b1b1726fa 137 isEcho = true;
hober 12:2e1b1b1726fa 138 pattern.count = pattern.num;
hober 12:2e1b1b1726fa 139 pattern.state = MAGNET;
hober 13:6850d2b41b2c 140 platform.set_speed(2.5);
hober 15:67c0fbc0999a 141 continue;
hober 12:2e1b1b1726fa 142 }
hober 12:2e1b1b1726fa 143 if(pattern.state == MAGNET)
hober 12:2e1b1b1726fa 144 {
hober 12:2e1b1b1726fa 145 if(pattern.count-- <= 0) pattern.state = TO;
hober 12:2e1b1b1726fa 146 getMag++;
hober 15:67c0fbc0999a 147 nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
hober 15:67c0fbc0999a 148 // wait(waitTime);
hober 12:2e1b1b1726fa 149 }
hober 12:2e1b1b1726fa 150 }
hober 12:2e1b1b1726fa 151 if(isEcho)
hober 12:2e1b1b1726fa 152 {
hober 12:2e1b1b1726fa 153 platform.position(pos);
hober 12:2e1b1b1726fa 154 echo('O',pos[0],pos[1],pos[2]);
hober 12:2e1b1b1726fa 155 led3 = !led3;
hober 12:2e1b1b1726fa 156 isEcho = false;
hober 15:67c0fbc0999a 157 // printf(" %f ",1.0f/(float)Fs);
hober 12:2e1b1b1726fa 158 }
hober 15:67c0fbc0999a 159 if(isRecording && n < recordTime && isTimeToRecord) {
hober 15:67c0fbc0999a 160 nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
hober 15:67c0fbc0999a 161 isTimeToRecord = false;
hober 15:67c0fbc0999a 162 char curMag = 1;
hober 15:67c0fbc0999a 163 int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0};
hober 15:67c0fbc0999a 164 mag_test=!mag_test;
hober 15:67c0fbc0999a 165 for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
hober 15:67c0fbc0999a 166 {
hober 15:67c0fbc0999a 167 if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0]));
hober 15:67c0fbc0999a 168 curMag <<= 1;
hober 15:67c0fbc0999a 169 }
hober 15:67c0fbc0999a 170 curMag = 1;
hober 15:67c0fbc0999a 171 int line = 0;
hober 15:67c0fbc0999a 172 for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
hober 15:67c0fbc0999a 173 {
hober 15:67c0fbc0999a 174 if((curMag & magSel) != 0)
hober 15:67c0fbc0999a 175 {
hober 15:67c0fbc0999a 176 memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10);
hober 15:67c0fbc0999a 177 }
hober 15:67c0fbc0999a 178 curMag <<= 1;
hober 7:ee0569d49c52 179 }
hober 15:67c0fbc0999a 180 if(pc.writeable()) {
hober 15:67c0fbc0999a 181 for(int i = 0; i < line*10; i++) pc.putc(magData[i]);
hober 15:67c0fbc0999a 182 //pc.putc('\0');
hober 15:67c0fbc0999a 183 }/*
hober 15:67c0fbc0999a 184 if(pc.writeable()) {
hober 15:67c0fbc0999a 185 for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
hober 15:67c0fbc0999a 186 {
hober 15:67c0fbc0999a 187 if((curMag & magSel) != 0) echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0]);
hober 15:67c0fbc0999a 188 curMag <<= 1;
hober 15:67c0fbc0999a 189 }
hober 15:67c0fbc0999a 190 // wait(waitTime);
hober 15:67c0fbc0999a 191 }*/
hober 7:ee0569d49c52 192 n++;
hober 15:67c0fbc0999a 193 if(isRecording && n == recordTime) {
hober 15:67c0fbc0999a 194 nextRecord.detach();
hober 15:67c0fbc0999a 195 n = 0;
hober 15:67c0fbc0999a 196 isRecording = false;
hober 15:67c0fbc0999a 197 echo('S',0,0,0);
hober 15:67c0fbc0999a 198 }
hober 8:33d34a775873 199 }// end recording if
hober 15:67c0fbc0999a 200 if(getMag>0 && isTimeToRecord) {
hober 15:67c0fbc0999a 201 isTimeToRecord = false;
hober 15:67c0fbc0999a 202 char curMag = 1;
hober 15:67c0fbc0999a 203 int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0};
hober 15:67c0fbc0999a 204 for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
hober 15:67c0fbc0999a 205 {
hober 15:67c0fbc0999a 206 if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0]));
hober 15:67c0fbc0999a 207 curMag <<= 1;
hober 15:67c0fbc0999a 208 }
hober 15:67c0fbc0999a 209 curMag = 1;
hober 15:67c0fbc0999a 210 int line = 0;
hober 15:67c0fbc0999a 211 for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
hober 15:67c0fbc0999a 212 {
hober 15:67c0fbc0999a 213 if((curMag & magSel) != 0)
hober 15:67c0fbc0999a 214 {
hober 15:67c0fbc0999a 215 memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10);
hober 15:67c0fbc0999a 216 }
hober 15:67c0fbc0999a 217 curMag <<= 1;
hober 15:67c0fbc0999a 218 }
hober 15:67c0fbc0999a 219 if(pc.writeable()) {
hober 15:67c0fbc0999a 220 for(int i = 0; i < line*10; i++) pc.putc(magData[i]);
hober 15:67c0fbc0999a 221 //pc.putc('\0');
hober 15:67c0fbc0999a 222 }
hober 9:c4f7257dee47 223 getMag--;
hober 9:c4f7257dee47 224 }
hober 9:c4f7257dee47 225 if(commandToDo) {
hober 9:c4f7257dee47 226 platform.to(x,y,z);
hober 15:67c0fbc0999a 227 isEcho = true;
hober 9:c4f7257dee47 228 commandToDo = false;
hober 15:67c0fbc0999a 229 continue;
hober 9:c4f7257dee47 230 }
hober 9:c4f7257dee47 231 if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
hober 9:c4f7257dee47 232 if(leftCount > 0) {
hober 9:c4f7257dee47 233 platform.go_left();
hober 9:c4f7257dee47 234 leftCount--;
hober 9:c4f7257dee47 235 }
hober 9:c4f7257dee47 236 if(rightCount > 0) {
hober 9:c4f7257dee47 237 platform.go_right();
hober 9:c4f7257dee47 238 rightCount--;
hober 9:c4f7257dee47 239 }
hober 9:c4f7257dee47 240 if(upCount > 0) {
hober 9:c4f7257dee47 241 platform.go_up();
hober 9:c4f7257dee47 242 upCount--;
hober 9:c4f7257dee47 243 }
hober 9:c4f7257dee47 244 if(downCount > 0) {
hober 9:c4f7257dee47 245 platform.go_down();
hober 9:c4f7257dee47 246 downCount--;
hober 9:c4f7257dee47 247 }
hober 9:c4f7257dee47 248 if(forwardCount > 0) {
hober 9:c4f7257dee47 249 platform.go_forward();
hober 9:c4f7257dee47 250 forwardCount--;
hober 9:c4f7257dee47 251 }
hober 9:c4f7257dee47 252 if(backwardCount > 0) {
hober 9:c4f7257dee47 253 platform.go_backward();
hober 9:c4f7257dee47 254 backwardCount--;
hober 9:c4f7257dee47 255 }
hober 15:67c0fbc0999a 256 isEcho = true;
hober 15:67c0fbc0999a 257 }
hober 15:67c0fbc0999a 258 if(isMagReset)
hober 15:67c0fbc0999a 259 {
hober 15:67c0fbc0999a 260 isMagReset = false;
hober 15:67c0fbc0999a 261 char curMag = 1;
hober 15:67c0fbc0999a 262 for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
hober 15:67c0fbc0999a 263 {
hober 15:67c0fbc0999a 264 if((curMag & magSel) != 0) platform.resetMagnetometer(i);
hober 15:67c0fbc0999a 265 curMag <<= 1;
hober 15:67c0fbc0999a 266 }
hober 9:c4f7257dee47 267 }
hober 7:ee0569d49c52 268 } // end while
hober 6:ce02d396c961 269
hober 6:ce02d396c961 270 }
hober 6:ce02d396c961 271
hober 6:ce02d396c961 272 void echo(char typ,float x, float y, float z)
hober 6:ce02d396c961 273 {
hober 7:ee0569d49c52 274 int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)};
hober 7:ee0569d49c52 275 echo(typ,p_data);
hober 6:ce02d396c961 276 }
hober 6:ce02d396c961 277
hober 15:67c0fbc0999a 278 uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow)
hober 6:ce02d396c961 279 {
hober 8:33d34a775873 280 char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
hober 8:33d34a775873 281 command->setEnvelopeData(tmp,7);
hober 10:a49cdab3810f 282 dataToSend = (uint8_t*)(command->getEnvelopeArray());
hober 15:67c0fbc0999a 283 if(pc.writeable()&&isWriteNow)
hober 15:67c0fbc0999a 284 {
hober 15:67c0fbc0999a 285 for(int i = 0; i < 10; i++) pc.putc(dataToSend[i]);
hober 15:67c0fbc0999a 286 // pc.printf("%.*s",10,dataToSend);
hober 15:67c0fbc0999a 287 //pc.fsync();
hober 15:67c0fbc0999a 288 //pc.putc('\0');
hober 15:67c0fbc0999a 289 }
hober 15:67c0fbc0999a 290 return dataToSend;
hober 15:67c0fbc0999a 291 /*
hober 8:33d34a775873 292 for(int i = 0; i < command->length(); i++) {
hober 10:a49cdab3810f 293 pc.putc(dataToSend[i]);
hober 15:67c0fbc0999a 294 }*/
hober 6:ce02d396c961 295 }
hober 6:ce02d396c961 296
hober 9:c4f7257dee47 297 void Rx_interrupt()
hober 9:c4f7257dee47 298 {
hober 9:c4f7257dee47 299 char c;
hober 9:c4f7257dee47 300 while(pc.readable()) {
hober 9:c4f7257dee47 301 c = pc.getc();
hober 9:c4f7257dee47 302 tracker.parse(&c,1);
hober 6:ce02d396c961 303
hober 9:c4f7257dee47 304 result = tracker.getEnvelope();
hober 9:c4f7257dee47 305 if(result!=NULL) {
hober 9:c4f7257dee47 306 char *dataArray = result->getPayload();
hober 9:c4f7257dee47 307 switch(dataArray[0]) {
hober 15:67c0fbc0999a 308 // following alphabets is used
hober 15:67c0fbc0999a 309 // I O C X Y Z M R S P N B
hober 12:2e1b1b1726fa 310 case 'I': // pattern record
hober 12:2e1b1b1726fa 311 if(dataArray[1] == 'X')
hober 12:2e1b1b1726fa 312 {
hober 12:2e1b1b1726fa 313 pattern.xStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
hober 12:2e1b1b1726fa 314 pattern.xEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
hober 12:2e1b1b1726fa 315 pattern.xStep = (float) dataArray[6]/10.0f;
hober 12:2e1b1b1726fa 316 if((pattern.xStart-pattern.xEnd)*pattern.xStep>0) pattern.xStep = -pattern.xStep;
hober 12:2e1b1b1726fa 317 }
hober 12:2e1b1b1726fa 318 else if(dataArray[1] == 'Y')
hober 12:2e1b1b1726fa 319 {
hober 12:2e1b1b1726fa 320 pattern.yStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
hober 12:2e1b1b1726fa 321 pattern.yEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
hober 12:2e1b1b1726fa 322 pattern.yStep = (float) dataArray[6]/10.0f;
hober 12:2e1b1b1726fa 323 if((pattern.yStart-pattern.yEnd)*pattern.yStep>0) pattern.yStep = -pattern.yStep;
hober 12:2e1b1b1726fa 324 }
hober 12:2e1b1b1726fa 325 else if(dataArray[1] == 'Z')
hober 12:2e1b1b1726fa 326 {
hober 12:2e1b1b1726fa 327 pattern.zStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
hober 12:2e1b1b1726fa 328 pattern.zEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
hober 12:2e1b1b1726fa 329 pattern.zStep = (float) dataArray[6]/10.0f;
hober 12:2e1b1b1726fa 330 if((pattern.zStart-pattern.zEnd)*pattern.zStep>0) pattern.zStep = -pattern.zStep;
hober 12:2e1b1b1726fa 331 }
hober 12:2e1b1b1726fa 332 else if(dataArray[1] == 'N')
hober 12:2e1b1b1726fa 333 {
hober 14:9672e91010a3 334 pattern.num = (int)((dataArray[2]<<8)+dataArray[3]);
hober 15:67c0fbc0999a 335 isTimeToRecord = true;
hober 15:67c0fbc0999a 336 // waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
hober 12:2e1b1b1726fa 337 }
hober 12:2e1b1b1726fa 338 else if(dataArray[1] == 'M')
hober 12:2e1b1b1726fa 339 {
hober 12:2e1b1b1726fa 340 if(dataArray[2] == 'B'){
hober 12:2e1b1b1726fa 341 pattern.x = (float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 12:2e1b1b1726fa 342 pattern.y = (float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 12:2e1b1b1726fa 343 }
hober 12:2e1b1b1726fa 344 if(dataArray[2] == 'U'){
hober 12:2e1b1b1726fa 345 pattern.z = (float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 12:2e1b1b1726fa 346 pattern.state = TO;
hober 12:2e1b1b1726fa 347 printf("%.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %d\n",
hober 12:2e1b1b1726fa 348 pattern.xStart, pattern.xEnd, pattern.xStep, pattern.yStart, pattern.yEnd, pattern.yStep, pattern.zStart, pattern.zEnd, pattern.zStep, pattern.x, pattern.y, pattern.z, pattern.num);
hober 12:2e1b1b1726fa 349 }
hober 12:2e1b1b1726fa 350 }
hober 12:2e1b1b1726fa 351
hober 15:67c0fbc0999a 352 else if(dataArray[1] == 'B') // initial
hober 12:2e1b1b1726fa 353 {
hober 12:2e1b1b1726fa 354 pattern.state = INIT;
hober 12:2e1b1b1726fa 355 }
hober 15:67c0fbc0999a 356 else if(dataArray[1] == 'S') pattern.state = NONE;// stop
hober 12:2e1b1b1726fa 357
hober 9:c4f7257dee47 358 break;
hober 9:c4f7257dee47 359 case 'O': // echo
hober 12:2e1b1b1726fa 360 isEcho = true;
hober 9:c4f7257dee47 361 break;
hober 9:c4f7257dee47 362 case 'C': // command
hober 12:2e1b1b1726fa 363 if(commandToDo) break;
hober 9:c4f7257dee47 364 x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
hober 9:c4f7257dee47 365 y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 9:c4f7257dee47 366 z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 9:c4f7257dee47 367 commandToDo = true;
hober 9:c4f7257dee47 368 break;
hober 15:67c0fbc0999a 369 case 'X': // move in X direction
hober 9:c4f7257dee47 370 if(dataArray[1]&0x80) rightCount++;
hober 9:c4f7257dee47 371 else leftCount++;
hober 9:c4f7257dee47 372 break;
hober 15:67c0fbc0999a 373 case 'Y': // move in Y direction
hober 9:c4f7257dee47 374 if(dataArray[1]&0x80) forwardCount++;
hober 10:a49cdab3810f 375 else backwardCount++;
hober 9:c4f7257dee47 376 break;
hober 15:67c0fbc0999a 377 case 'Z': // move in Z direction
hober 9:c4f7257dee47 378 if(dataArray[1]&0x80) upCount++;
hober 9:c4f7257dee47 379 else downCount++;
hober 9:c4f7257dee47 380 break;
hober 9:c4f7257dee47 381 case 'M': // magnet
hober 9:c4f7257dee47 382 getMag++;
hober 15:67c0fbc0999a 383 magSel = dataArray[1];
hober 15:67c0fbc0999a 384 isTimeToRecord = true;
hober 15:67c0fbc0999a 385 pc.putc('M');
hober 9:c4f7257dee47 386 break;
hober 9:c4f7257dee47 387 case 'R': // record
hober 9:c4f7257dee47 388 recordTime = dataArray[1];
hober 9:c4f7257dee47 389 recordTime *= Fs;
hober 15:67c0fbc0999a 390 magSel = dataArray[2];
hober 15:67c0fbc0999a 391 // mag_test=!mag_test;
hober 15:67c0fbc0999a 392 /*
hober 15:67c0fbc0999a 393 waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
hober 10:a49cdab3810f 394 n = 0;
hober 15:67c0fbc0999a 395 if(waitTime < 0) waitTime = 0;*/
hober 15:67c0fbc0999a 396 isTimeToRecord = true;
hober 9:c4f7257dee47 397 isRecording = true;
hober 9:c4f7257dee47 398 break;
hober 9:c4f7257dee47 399 case 'S': // stop
hober 9:c4f7257dee47 400 isRecording = false;
hober 15:67c0fbc0999a 401 nextRecord.detach();
hober 15:67c0fbc0999a 402 isTimeToRecord = false;
hober 9:c4f7257dee47 403 break;
hober 12:2e1b1b1726fa 404 case 'P': // set position
hober 12:2e1b1b1726fa 405 x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
hober 12:2e1b1b1726fa 406 y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 12:2e1b1b1726fa 407 z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 12:2e1b1b1726fa 408 platform.setPosition(x,y,z);
hober 12:2e1b1b1726fa 409 break;
hober 12:2e1b1b1726fa 410 case 'N': // new set up
hober 12:2e1b1b1726fa 411 isReset = true;
hober 12:2e1b1b1726fa 412 break;
hober 15:67c0fbc0999a 413 case 'B':
hober 15:67c0fbc0999a 414 magSel = dataArray[1];
hober 15:67c0fbc0999a 415 isMagReset = true;
hober 15:67c0fbc0999a 416 break;
hober 9:c4f7257dee47 417 default:
hober 9:c4f7257dee47 418 break;
hober 9:c4f7257dee47 419 } // end switch
hober 9:c4f7257dee47 420 result = NULL;
hober 9:c4f7257dee47 421 dataArray = NULL;
hober 9:c4f7257dee47 422 } // end result if
hober 9:c4f7257dee47 423 } // end parsing if
hober 9:c4f7257dee47 424 }
hober 9:c4f7257dee47 425