5 sensors on a board with a moving platform.
Fork of Motor_XYZ_UI_SPI_I2C_5mag by
ui.cpp@12:2e1b1b1726fa, 2018-03-02 (annotated)
- Committer:
- hober
- Date:
- Fri Mar 02 06:40:25 2018 +0000
- Branch:
- envelope
- Revision:
- 12:2e1b1b1726fa
- Parent:
- 11:bb86ffb378f4
- Child:
- 13:6850d2b41b2c
2018/02/27
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hober | 6:ce02d396c961 | 1 | #include "motor.h" |
hober | 6:ce02d396c961 | 2 | #include "xyz_sensor_platform.h" |
hober | 8:33d34a775873 | 3 | //#include "ParseArray.h" |
hober | 8:33d34a775873 | 4 | #include "envelopetracker.h" |
hober | 6:ce02d396c961 | 5 | |
hober | 7:ee0569d49c52 | 6 | #define I2C_FREQUENCY 400000 |
hober | 6:ce02d396c961 | 7 | |
hober | 8:33d34a775873 | 8 | typedef unsigned char byte; |
hober | 10:a49cdab3810f | 9 | uint8_t* dataToSend; |
hober | 8:33d34a775873 | 10 | int sendArrayIndex = 0; |
hober | 8:33d34a775873 | 11 | int sendBufferMax = 10000; |
hober | 8:33d34a775873 | 12 | const int BAUD = 921600; |
hober | 8:33d34a775873 | 13 | Serial pc(SERIAL_TX, SERIAL_RX, BAUD ); |
hober | 7:ee0569d49c52 | 14 | DigitalOut led(LED2),led3(LED3); |
hober | 7:ee0569d49c52 | 15 | DigitalOut mag_test(D11); |
hober | 12:2e1b1b1726fa | 16 | InterruptIn button(USER_BUTTON); |
hober | 6:ce02d396c961 | 17 | XYZSensorPlatform platform; |
hober | 8:33d34a775873 | 18 | Envelope *command; |
hober | 8:33d34a775873 | 19 | EnvelopeTracker tracker; |
hober | 7:ee0569d49c52 | 20 | byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0}; |
hober | 7:ee0569d49c52 | 21 | bool isRecording = false; |
hober | 9:c4f7257dee47 | 22 | Envelope* result; |
hober | 9:c4f7257dee47 | 23 | float x, y, z; |
hober | 9:c4f7257dee47 | 24 | float pos[3]; |
hober | 9:c4f7257dee47 | 25 | int n = 0; |
hober | 9:c4f7257dee47 | 26 | int getMag = 0; |
hober | 9:c4f7257dee47 | 27 | int leftCount = 0; |
hober | 9:c4f7257dee47 | 28 | int rightCount = 0; |
hober | 9:c4f7257dee47 | 29 | int upCount = 0; |
hober | 9:c4f7257dee47 | 30 | int downCount = 0; |
hober | 9:c4f7257dee47 | 31 | int forwardCount = 0; |
hober | 9:c4f7257dee47 | 32 | int backwardCount = 0; |
hober | 9:c4f7257dee47 | 33 | bool commandToDo = false; |
hober | 9:c4f7257dee47 | 34 | int recordTime; |
hober | 9:c4f7257dee47 | 35 | float waitTime; |
hober | 12:2e1b1b1726fa | 36 | bool isEcho = false; |
hober | 12:2e1b1b1726fa | 37 | bool isReset = true; |
hober | 12:2e1b1b1726fa | 38 | enum PatternRecordState{NONE,INIT,MAGNET,TO}; |
hober | 12:2e1b1b1726fa | 39 | struct Pattern |
hober | 12:2e1b1b1726fa | 40 | { |
hober | 12:2e1b1b1726fa | 41 | float xStart, xEnd, xStep, yStart, yEnd, yStep, zStart, zEnd, zStep; |
hober | 12:2e1b1b1726fa | 42 | int num; |
hober | 12:2e1b1b1726fa | 43 | float x, y, z; |
hober | 12:2e1b1b1726fa | 44 | int count; |
hober | 12:2e1b1b1726fa | 45 | PatternRecordState state; |
hober | 12:2e1b1b1726fa | 46 | } pattern; |
hober | 12:2e1b1b1726fa | 47 | |
hober | 8:33d34a775873 | 48 | |
hober | 9:c4f7257dee47 | 49 | const int Fs = 1000; // sampling rate -- max: 1kHz |
hober | 8:33d34a775873 | 50 | |
hober | 8:33d34a775873 | 51 | |
hober | 6:ce02d396c961 | 52 | void echo(char typ, float x, float y, float z); |
hober | 6:ce02d396c961 | 53 | void echo(char typ, int16_t *p_data); |
hober | 9:c4f7257dee47 | 54 | void Rx_interrupt(); |
hober | 12:2e1b1b1726fa | 55 | void released() |
hober | 12:2e1b1b1726fa | 56 | { |
hober | 12:2e1b1b1726fa | 57 | led = !led; |
hober | 12:2e1b1b1726fa | 58 | pc.attach(&Rx_interrupt, Serial::RxIrq); |
hober | 12:2e1b1b1726fa | 59 | } |
hober | 8:33d34a775873 | 60 | |
hober | 6:ce02d396c961 | 61 | int main() |
hober | 6:ce02d396c961 | 62 | { |
hober | 6:ce02d396c961 | 63 | led=1; |
hober | 8:33d34a775873 | 64 | command = new Envelope; |
hober | 8:33d34a775873 | 65 | command->enableHeader(std::string("H"));// 48 H |
hober | 8:33d34a775873 | 66 | command->enableFooter(std::string("E"),8);// 45 E |
hober | 8:33d34a775873 | 67 | command->enableCheckXOR(9); |
hober | 8:33d34a775873 | 68 | tracker.setEnvelope(*command); |
hober | 8:33d34a775873 | 69 | tracker.setBufferLength(100); |
hober | 12:2e1b1b1726fa | 70 | pattern.state = NONE; |
hober | 7:ee0569d49c52 | 71 | pc.format(8,SerialBase::None,1); |
hober | 12:2e1b1b1726fa | 72 | button.rise(&released); |
hober | 6:ce02d396c961 | 73 | platform.set_speed(2.5); |
hober | 12:2e1b1b1726fa | 74 | // platform.reset(); // need to be modified here |
hober | 7:ee0569d49c52 | 75 | platform.setSensorI2cFrequency(I2C_FREQUENCY); |
hober | 9:c4f7257dee47 | 76 | // Setup a serial interrupt function to receive data |
hober | 9:c4f7257dee47 | 77 | pc.attach(&Rx_interrupt, Serial::RxIrq); |
hober | 12:2e1b1b1726fa | 78 | echo('B',0,0,0); |
hober | 6:ce02d396c961 | 79 | while(1) { |
hober | 12:2e1b1b1726fa | 80 | if(isReset){ |
hober | 12:2e1b1b1726fa | 81 | platform.set_speed(1); |
hober | 12:2e1b1b1726fa | 82 | platform.reset(); |
hober | 12:2e1b1b1726fa | 83 | isReset = false; |
hober | 12:2e1b1b1726fa | 84 | platform.set_speed(2.5); |
hober | 12:2e1b1b1726fa | 85 | } |
hober | 12:2e1b1b1726fa | 86 | if(pattern.state != NONE) |
hober | 12:2e1b1b1726fa | 87 | { |
hober | 12:2e1b1b1726fa | 88 | if(pattern.state == INIT) platform.to(pattern.xStart,pattern.yStart,pattern.zStart); |
hober | 12:2e1b1b1726fa | 89 | if(pattern.count == 0) pattern.state = TO; |
hober | 12:2e1b1b1726fa | 90 | if(pattern.state == TO) |
hober | 12:2e1b1b1726fa | 91 | { |
hober | 12:2e1b1b1726fa | 92 | if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep)) pattern.x += pattern.xStep; |
hober | 12:2e1b1b1726fa | 93 | else if(abs(pattern.y-pattern.yEnd)>abs(pattern.yStep)){ |
hober | 12:2e1b1b1726fa | 94 | pattern.x = pattern.xStart; |
hober | 12:2e1b1b1726fa | 95 | pattern.y += pattern.yStep; |
hober | 12:2e1b1b1726fa | 96 | } |
hober | 12:2e1b1b1726fa | 97 | else if(abs(pattern.z-pattern.zEnd)>abs(pattern.zStep)){ |
hober | 12:2e1b1b1726fa | 98 | pattern.x = pattern.xStart; |
hober | 12:2e1b1b1726fa | 99 | pattern.y = pattern.yStart; |
hober | 12:2e1b1b1726fa | 100 | pattern.z += pattern.zStep; |
hober | 12:2e1b1b1726fa | 101 | } |
hober | 12:2e1b1b1726fa | 102 | else{ |
hober | 12:2e1b1b1726fa | 103 | pattern.state = NONE; |
hober | 12:2e1b1b1726fa | 104 | continue; |
hober | 12:2e1b1b1726fa | 105 | } |
hober | 12:2e1b1b1726fa | 106 | platform.to(pattern.x,pattern.y,pattern.z); |
hober | 12:2e1b1b1726fa | 107 | // wait(0.); |
hober | 12:2e1b1b1726fa | 108 | isEcho = true; |
hober | 12:2e1b1b1726fa | 109 | pattern.count = pattern.num; |
hober | 12:2e1b1b1726fa | 110 | pattern.state = MAGNET; |
hober | 12:2e1b1b1726fa | 111 | } |
hober | 12:2e1b1b1726fa | 112 | if(pattern.state == MAGNET) |
hober | 12:2e1b1b1726fa | 113 | { |
hober | 12:2e1b1b1726fa | 114 | if(pattern.count-- <= 0) pattern.state = TO; |
hober | 12:2e1b1b1726fa | 115 | getMag++; |
hober | 12:2e1b1b1726fa | 116 | } |
hober | 12:2e1b1b1726fa | 117 | } |
hober | 12:2e1b1b1726fa | 118 | if(isEcho) |
hober | 12:2e1b1b1726fa | 119 | { |
hober | 12:2e1b1b1726fa | 120 | platform.position(pos); |
hober | 12:2e1b1b1726fa | 121 | echo('O',pos[0],pos[1],pos[2]); |
hober | 12:2e1b1b1726fa | 122 | led3 = !led3; |
hober | 12:2e1b1b1726fa | 123 | isEcho = false; |
hober | 12:2e1b1b1726fa | 124 | } |
hober | 8:33d34a775873 | 125 | if(isRecording && n < recordTime) { |
hober | 7:ee0569d49c52 | 126 | int16_t mag[3]; |
hober | 7:ee0569d49c52 | 127 | if(platform.get_mag_raw(mag)==0&&pc.writeable()) { |
hober | 7:ee0569d49c52 | 128 | echo('M',mag); |
hober | 7:ee0569d49c52 | 129 | mag_test=!mag_test; |
hober | 8:33d34a775873 | 130 | wait(waitTime); |
hober | 7:ee0569d49c52 | 131 | } |
hober | 7:ee0569d49c52 | 132 | n++; |
hober | 8:33d34a775873 | 133 | } else if(isRecording) { |
hober | 8:33d34a775873 | 134 | isRecording = false; |
hober | 8:33d34a775873 | 135 | echo('S',0,0,0); |
hober | 8:33d34a775873 | 136 | }// end recording if |
hober | 9:c4f7257dee47 | 137 | if(getMag>0) { |
hober | 9:c4f7257dee47 | 138 | int16_t mag[3]; |
hober | 9:c4f7257dee47 | 139 | if(platform.get_mag_raw(mag)==0) echo('M',mag); |
hober | 9:c4f7257dee47 | 140 | getMag--; |
hober | 9:c4f7257dee47 | 141 | } |
hober | 9:c4f7257dee47 | 142 | if(commandToDo) { |
hober | 9:c4f7257dee47 | 143 | platform.to(x,y,z); |
hober | 9:c4f7257dee47 | 144 | platform.position(pos); |
hober | 9:c4f7257dee47 | 145 | echo('O',pos[0],pos[1],pos[2]); |
hober | 9:c4f7257dee47 | 146 | commandToDo = false; |
hober | 9:c4f7257dee47 | 147 | } |
hober | 9:c4f7257dee47 | 148 | if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) { |
hober | 9:c4f7257dee47 | 149 | if(leftCount > 0) { |
hober | 9:c4f7257dee47 | 150 | platform.go_left(); |
hober | 9:c4f7257dee47 | 151 | leftCount--; |
hober | 9:c4f7257dee47 | 152 | } |
hober | 9:c4f7257dee47 | 153 | if(rightCount > 0) { |
hober | 9:c4f7257dee47 | 154 | platform.go_right(); |
hober | 9:c4f7257dee47 | 155 | rightCount--; |
hober | 9:c4f7257dee47 | 156 | } |
hober | 9:c4f7257dee47 | 157 | if(upCount > 0) { |
hober | 9:c4f7257dee47 | 158 | platform.go_up(); |
hober | 9:c4f7257dee47 | 159 | upCount--; |
hober | 9:c4f7257dee47 | 160 | } |
hober | 9:c4f7257dee47 | 161 | if(downCount > 0) { |
hober | 9:c4f7257dee47 | 162 | platform.go_down(); |
hober | 9:c4f7257dee47 | 163 | downCount--; |
hober | 9:c4f7257dee47 | 164 | } |
hober | 9:c4f7257dee47 | 165 | if(forwardCount > 0) { |
hober | 9:c4f7257dee47 | 166 | platform.go_forward(); |
hober | 9:c4f7257dee47 | 167 | forwardCount--; |
hober | 9:c4f7257dee47 | 168 | } |
hober | 9:c4f7257dee47 | 169 | if(backwardCount > 0) { |
hober | 9:c4f7257dee47 | 170 | platform.go_backward(); |
hober | 9:c4f7257dee47 | 171 | backwardCount--; |
hober | 9:c4f7257dee47 | 172 | } |
hober | 9:c4f7257dee47 | 173 | platform.position(pos); |
hober | 9:c4f7257dee47 | 174 | echo('O',pos[0],pos[1],pos[2]); |
hober | 9:c4f7257dee47 | 175 | } |
hober | 7:ee0569d49c52 | 176 | } // end while |
hober | 6:ce02d396c961 | 177 | |
hober | 6:ce02d396c961 | 178 | } |
hober | 6:ce02d396c961 | 179 | |
hober | 6:ce02d396c961 | 180 | void echo(char typ,float x, float y, float z) |
hober | 6:ce02d396c961 | 181 | { |
hober | 7:ee0569d49c52 | 182 | int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)}; |
hober | 7:ee0569d49c52 | 183 | echo(typ,p_data); |
hober | 6:ce02d396c961 | 184 | } |
hober | 6:ce02d396c961 | 185 | |
hober | 6:ce02d396c961 | 186 | void echo(char typ, int16_t *p_data) |
hober | 6:ce02d396c961 | 187 | { |
hober | 8:33d34a775873 | 188 | 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 | 189 | command->setEnvelopeData(tmp,7); |
hober | 10:a49cdab3810f | 190 | dataToSend = (uint8_t*)(command->getEnvelopeArray()); |
hober | 8:33d34a775873 | 191 | for(int i = 0; i < command->length(); i++) { |
hober | 10:a49cdab3810f | 192 | pc.putc(dataToSend[i]); |
hober | 8:33d34a775873 | 193 | } |
hober | 6:ce02d396c961 | 194 | } |
hober | 6:ce02d396c961 | 195 | |
hober | 9:c4f7257dee47 | 196 | void Rx_interrupt() |
hober | 9:c4f7257dee47 | 197 | { |
hober | 9:c4f7257dee47 | 198 | char c; |
hober | 9:c4f7257dee47 | 199 | while(pc.readable()) { |
hober | 9:c4f7257dee47 | 200 | c = pc.getc(); |
hober | 9:c4f7257dee47 | 201 | tracker.parse(&c,1); |
hober | 6:ce02d396c961 | 202 | |
hober | 9:c4f7257dee47 | 203 | result = tracker.getEnvelope(); |
hober | 9:c4f7257dee47 | 204 | if(result!=NULL) { |
hober | 9:c4f7257dee47 | 205 | char *dataArray = result->getPayload(); |
hober | 9:c4f7257dee47 | 206 | switch(dataArray[0]) { |
hober | 12:2e1b1b1726fa | 207 | case 'I': // pattern record |
hober | 12:2e1b1b1726fa | 208 | if(dataArray[1] == 'X') |
hober | 12:2e1b1b1726fa | 209 | { |
hober | 12:2e1b1b1726fa | 210 | pattern.xStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f; |
hober | 12:2e1b1b1726fa | 211 | pattern.xEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f; |
hober | 12:2e1b1b1726fa | 212 | pattern.xStep = (float) dataArray[6]/10.0f; |
hober | 12:2e1b1b1726fa | 213 | if((pattern.xStart-pattern.xEnd)*pattern.xStep>0) pattern.xStep = -pattern.xStep; |
hober | 12:2e1b1b1726fa | 214 | } |
hober | 12:2e1b1b1726fa | 215 | else if(dataArray[1] == 'Y') |
hober | 12:2e1b1b1726fa | 216 | { |
hober | 12:2e1b1b1726fa | 217 | pattern.yStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f; |
hober | 12:2e1b1b1726fa | 218 | pattern.yEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f; |
hober | 12:2e1b1b1726fa | 219 | pattern.yStep = (float) dataArray[6]/10.0f; |
hober | 12:2e1b1b1726fa | 220 | if((pattern.yStart-pattern.yEnd)*pattern.yStep>0) pattern.yStep = -pattern.yStep; |
hober | 12:2e1b1b1726fa | 221 | } |
hober | 12:2e1b1b1726fa | 222 | else if(dataArray[1] == 'Z') |
hober | 12:2e1b1b1726fa | 223 | { |
hober | 12:2e1b1b1726fa | 224 | pattern.zStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f; |
hober | 12:2e1b1b1726fa | 225 | pattern.zEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f; |
hober | 12:2e1b1b1726fa | 226 | pattern.zStep = (float) dataArray[6]/10.0f; |
hober | 12:2e1b1b1726fa | 227 | if((pattern.zStart-pattern.zEnd)*pattern.zStep>0) pattern.zStep = -pattern.zStep; |
hober | 12:2e1b1b1726fa | 228 | } |
hober | 12:2e1b1b1726fa | 229 | else if(dataArray[1] == 'N') |
hober | 12:2e1b1b1726fa | 230 | { |
hober | 12:2e1b1b1726fa | 231 | pattern.num = (int)dataArray[2]; |
hober | 12:2e1b1b1726fa | 232 | } |
hober | 12:2e1b1b1726fa | 233 | else if(dataArray[1] == 'M') |
hober | 12:2e1b1b1726fa | 234 | { |
hober | 12:2e1b1b1726fa | 235 | if(dataArray[2] == 'B'){ |
hober | 12:2e1b1b1726fa | 236 | pattern.x = (float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 12:2e1b1b1726fa | 237 | pattern.y = (float)((dataArray[5]<<8)+dataArray[6])/10.0f; |
hober | 12:2e1b1b1726fa | 238 | } |
hober | 12:2e1b1b1726fa | 239 | if(dataArray[2] == 'U'){ |
hober | 12:2e1b1b1726fa | 240 | pattern.z = (float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 12:2e1b1b1726fa | 241 | pattern.state = TO; |
hober | 12:2e1b1b1726fa | 242 | printf("%.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %d\n", |
hober | 12:2e1b1b1726fa | 243 | 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 | 244 | } |
hober | 12:2e1b1b1726fa | 245 | } |
hober | 12:2e1b1b1726fa | 246 | |
hober | 12:2e1b1b1726fa | 247 | else if(dataArray[1] == 'B') |
hober | 12:2e1b1b1726fa | 248 | { |
hober | 12:2e1b1b1726fa | 249 | pattern.state = INIT; |
hober | 12:2e1b1b1726fa | 250 | pattern.count = pattern.num; |
hober | 12:2e1b1b1726fa | 251 | } |
hober | 12:2e1b1b1726fa | 252 | else if(dataArray[1] == 'S') pattern.state = NONE; |
hober | 12:2e1b1b1726fa | 253 | |
hober | 9:c4f7257dee47 | 254 | break; |
hober | 9:c4f7257dee47 | 255 | case 'O': // echo |
hober | 12:2e1b1b1726fa | 256 | isEcho = true; |
hober | 9:c4f7257dee47 | 257 | break; |
hober | 9:c4f7257dee47 | 258 | case 'C': // command |
hober | 12:2e1b1b1726fa | 259 | if(commandToDo) break; |
hober | 9:c4f7257dee47 | 260 | x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f; |
hober | 9:c4f7257dee47 | 261 | y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 9:c4f7257dee47 | 262 | z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f; |
hober | 9:c4f7257dee47 | 263 | commandToDo = true; |
hober | 9:c4f7257dee47 | 264 | break; |
hober | 9:c4f7257dee47 | 265 | case 'X': |
hober | 9:c4f7257dee47 | 266 | if(dataArray[1]&0x80) rightCount++; |
hober | 9:c4f7257dee47 | 267 | else leftCount++; |
hober | 9:c4f7257dee47 | 268 | break; |
hober | 9:c4f7257dee47 | 269 | case 'Y': |
hober | 9:c4f7257dee47 | 270 | if(dataArray[1]&0x80) forwardCount++; |
hober | 10:a49cdab3810f | 271 | else backwardCount++; |
hober | 9:c4f7257dee47 | 272 | break; |
hober | 9:c4f7257dee47 | 273 | case 'Z': |
hober | 9:c4f7257dee47 | 274 | if(dataArray[1]&0x80) upCount++; |
hober | 9:c4f7257dee47 | 275 | else downCount++; |
hober | 9:c4f7257dee47 | 276 | break; |
hober | 9:c4f7257dee47 | 277 | case 'M': // magnet |
hober | 9:c4f7257dee47 | 278 | getMag++; |
hober | 9:c4f7257dee47 | 279 | mag_test=!mag_test; |
hober | 9:c4f7257dee47 | 280 | break; |
hober | 9:c4f7257dee47 | 281 | case 'R': // record |
hober | 9:c4f7257dee47 | 282 | recordTime = dataArray[1]; |
hober | 9:c4f7257dee47 | 283 | recordTime *= Fs; |
hober | 11:bb86ffb378f4 | 284 | waitTime = 1.0/Fs-0.00052-1/(BAUD/8/10); |
hober | 10:a49cdab3810f | 285 | n = 0; |
hober | 9:c4f7257dee47 | 286 | if(waitTime < 0) waitTime = 0; |
hober | 9:c4f7257dee47 | 287 | isRecording = true; |
hober | 9:c4f7257dee47 | 288 | break; |
hober | 9:c4f7257dee47 | 289 | case 'S': // stop |
hober | 9:c4f7257dee47 | 290 | isRecording = false; |
hober | 9:c4f7257dee47 | 291 | break; |
hober | 12:2e1b1b1726fa | 292 | case 'P': // set position |
hober | 12:2e1b1b1726fa | 293 | x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f; |
hober | 12:2e1b1b1726fa | 294 | y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 12:2e1b1b1726fa | 295 | z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f; |
hober | 12:2e1b1b1726fa | 296 | platform.setPosition(x,y,z); |
hober | 12:2e1b1b1726fa | 297 | break; |
hober | 12:2e1b1b1726fa | 298 | case 'N': // new set up |
hober | 12:2e1b1b1726fa | 299 | isReset = true; |
hober | 12:2e1b1b1726fa | 300 | break; |
hober | 9:c4f7257dee47 | 301 | default: |
hober | 9:c4f7257dee47 | 302 | break; |
hober | 9:c4f7257dee47 | 303 | } // end switch |
hober | 9:c4f7257dee47 | 304 | result = NULL; |
hober | 9:c4f7257dee47 | 305 | dataArray = NULL; |
hober | 9:c4f7257dee47 | 306 | } // end result if |
hober | 9:c4f7257dee47 | 307 | } // end parsing if |
hober | 9:c4f7257dee47 | 308 | } |
hober | 9:c4f7257dee47 | 309 |