Shih-Ho Hsieh / Mbed 2 deprecated Motor_XYZ_UI_SPI_8mag_encoder

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:   mbed

ui.cpp@0:9e6e3dc903c0, 2018-12-22 (annotated)

Committer:
hober
Date:
Sat Dec 22 01:07:34 2018 +0000
Revision:
0:9e6e3dc903c0
Child:
1:edc6b5cc7112
20181222 backup

Who changed what in which revision?

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