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@1:edc6b5cc7112, 2019-01-07 (annotated)

Committer:
hober
Date:
Mon Jan 07 03:32:36 2019 +0000
Revision:
1:edc6b5cc7112
Parent:
0:9e6e3dc903c0 
20190107 for motor control

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