5 sensors on a board with a moving platform.
Fork of Motor_XYZ_UI_SPI_I2C_5mag by
ui.cpp@23:eede1fe4494b, 2018-08-15 (annotated)
- Committer:
- hober
- Date:
- Wed Aug 15 02:42:43 2018 +0000
- Branch:
- envelope
- Revision:
- 23:eede1fe4494b
- Parent:
- 20:ba6de7e5240c
2018/08/15 remove altimu lib test.
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 "envelopetracker.h" |
hober | 6:ce02d396c961 | 4 | |
hober | 20:ba6de7e5240c | 5 | #define I2C_FREQUENCY 1000000 |
hober | 20:ba6de7e5240c | 6 | #define SPI_FREQUENCY 1e7 |
hober | 20:ba6de7e5240c | 7 | //#define DELAY_COMPENSATE 0.000537 |
hober | 20:ba6de7e5240c | 8 | #define BIG_CHAR_MASK 0x1F |
hober | 20:ba6de7e5240c | 9 | #define BAUD 921600 |
hober | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 18 | DigitalOut mag_test(PC_8); |
hober | 12:2e1b1b1726fa | 19 | InterruptIn button(USER_BUTTON); |
hober | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 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:5dd07840023c | 42 | bool isReset = false; |
hober | 20:ba6de7e5240c | 43 | bool isTimeToRecord = false; |
hober | 20:ba6de7e5240c | 44 | bool isMagReset = false; |
hober | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 59 | uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow = true); |
hober | 9:c4f7257dee47 | 60 | void Rx_interrupt(); |
hober | 20:ba6de7e5240c | 61 | void recordTimeup() |
hober | 20:ba6de7e5240c | 62 | { |
hober | 20:ba6de7e5240c | 63 | isTimeToRecord = true; |
hober | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 75 | // char msg[] = "start!!"; |
hober | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 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 | 20:ba6de7e5240c | 88 | platform.setSensorSpiFrequency(SPI_FREQUENCY); |
hober | 7:ee0569d49c52 | 89 | platform.setSensorI2cFrequency(I2C_FREQUENCY); |
hober | 9:c4f7257dee47 | 90 | // Setup a serial interrupt function to receive data |
hober | 9:c4f7257dee47 | 91 | pc.attach(&Rx_interrupt, Serial::RxIrq); |
hober | 20:ba6de7e5240c | 92 | // echo('B',0,0,0); |
hober | 6:ce02d396c961 | 93 | while(1) { |
hober | 15:5dd07840023c | 94 | if(platform.isMoving()) continue; |
hober | 12:2e1b1b1726fa | 95 | if(isReset){ |
hober | 12:2e1b1b1726fa | 96 | platform.set_speed(1); |
hober | 12:2e1b1b1726fa | 97 | platform.reset(); |
hober | 12:2e1b1b1726fa | 98 | isReset = false; |
hober | 12:2e1b1b1726fa | 99 | platform.set_speed(2.5); |
hober | 12:2e1b1b1726fa | 100 | } |
hober | 12:2e1b1b1726fa | 101 | if(pattern.state != NONE) |
hober | 12:2e1b1b1726fa | 102 | { |
hober | 13:6850d2b41b2c | 103 | if(pattern.state == INIT) |
hober | 13:6850d2b41b2c | 104 | { |
hober | 13:6850d2b41b2c | 105 | pattern.x = pattern.xStart; |
hober | 13:6850d2b41b2c | 106 | pattern.y = pattern.yStart; |
hober | 13:6850d2b41b2c | 107 | pattern.z = pattern.zStart; |
hober | 13:6850d2b41b2c | 108 | pattern.count = pattern.num; |
hober | 13:6850d2b41b2c | 109 | pattern.state = MAGNET; |
hober | 15:5dd07840023c | 110 | //isEcho = true; |
hober | 15:5dd07840023c | 111 | platform.to(pattern.x,pattern.y,pattern.z); |
hober | 13:6850d2b41b2c | 112 | isEcho = true; |
hober | 15:5dd07840023c | 113 | continue; |
hober | 13:6850d2b41b2c | 114 | } |
hober | 12:2e1b1b1726fa | 115 | if(pattern.count == 0) pattern.state = TO; |
hober | 12:2e1b1b1726fa | 116 | if(pattern.state == TO) |
hober | 12:2e1b1b1726fa | 117 | { |
hober | 13:6850d2b41b2c | 118 | if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep)) |
hober | 13:6850d2b41b2c | 119 | { |
hober | 13:6850d2b41b2c | 120 | pattern.x += pattern.xStep; |
hober | 13:6850d2b41b2c | 121 | platform.set_speed(1.5); |
hober | 13:6850d2b41b2c | 122 | } |
hober | 12:2e1b1b1726fa | 123 | else if(abs(pattern.y-pattern.yEnd)>abs(pattern.yStep)){ |
hober | 12:2e1b1b1726fa | 124 | pattern.x = pattern.xStart; |
hober | 12:2e1b1b1726fa | 125 | pattern.y += pattern.yStep; |
hober | 12:2e1b1b1726fa | 126 | } |
hober | 12:2e1b1b1726fa | 127 | else if(abs(pattern.z-pattern.zEnd)>abs(pattern.zStep)){ |
hober | 12:2e1b1b1726fa | 128 | pattern.x = pattern.xStart; |
hober | 12:2e1b1b1726fa | 129 | pattern.y = pattern.yStart; |
hober | 12:2e1b1b1726fa | 130 | pattern.z += pattern.zStep; |
hober | 12:2e1b1b1726fa | 131 | } |
hober | 12:2e1b1b1726fa | 132 | else{ |
hober | 12:2e1b1b1726fa | 133 | pattern.state = NONE; |
hober | 13:6850d2b41b2c | 134 | echo('S',0,0,0); |
hober | 12:2e1b1b1726fa | 135 | continue; |
hober | 12:2e1b1b1726fa | 136 | } |
hober | 12:2e1b1b1726fa | 137 | platform.to(pattern.x,pattern.y,pattern.z); |
hober | 12:2e1b1b1726fa | 138 | isEcho = true; |
hober | 12:2e1b1b1726fa | 139 | pattern.count = pattern.num; |
hober | 12:2e1b1b1726fa | 140 | pattern.state = MAGNET; |
hober | 13:6850d2b41b2c | 141 | platform.set_speed(2.5); |
hober | 15:5dd07840023c | 142 | continue; |
hober | 12:2e1b1b1726fa | 143 | } |
hober | 12:2e1b1b1726fa | 144 | if(pattern.state == MAGNET) |
hober | 12:2e1b1b1726fa | 145 | { |
hober | 12:2e1b1b1726fa | 146 | if(pattern.count-- <= 0) pattern.state = TO; |
hober | 12:2e1b1b1726fa | 147 | getMag++; |
hober | 20:ba6de7e5240c | 148 | nextRecord.attach(&recordTimeup, 1.0f/(float)Fs); |
hober | 20:ba6de7e5240c | 149 | // wait(waitTime); |
hober | 12:2e1b1b1726fa | 150 | } |
hober | 12:2e1b1b1726fa | 151 | } |
hober | 12:2e1b1b1726fa | 152 | if(isEcho) |
hober | 12:2e1b1b1726fa | 153 | { |
hober | 12:2e1b1b1726fa | 154 | platform.position(pos); |
hober | 12:2e1b1b1726fa | 155 | echo('O',pos[0],pos[1],pos[2]); |
hober | 12:2e1b1b1726fa | 156 | led3 = !led3; |
hober | 12:2e1b1b1726fa | 157 | isEcho = false; |
hober | 20:ba6de7e5240c | 158 | // printf(" %f ",1.0f/(float)Fs); |
hober | 12:2e1b1b1726fa | 159 | } |
hober | 20:ba6de7e5240c | 160 | if(isRecording && n < recordTime && isTimeToRecord) { |
hober | 20:ba6de7e5240c | 161 | nextRecord.attach(&recordTimeup, 1.0f/(float)Fs); |
hober | 20:ba6de7e5240c | 162 | isTimeToRecord = false; |
hober | 20:ba6de7e5240c | 163 | char curMag = 1; |
hober | 20:ba6de7e5240c | 164 | int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0}; |
hober | 20:ba6de7e5240c | 165 | mag_test=!mag_test; |
hober | 20:ba6de7e5240c | 166 | for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++) |
hober | 20:ba6de7e5240c | 167 | { |
hober | 20:ba6de7e5240c | 168 | if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0])); |
hober | 20:ba6de7e5240c | 169 | curMag <<= 1; |
hober | 7:ee0569d49c52 | 170 | } |
hober | 20:ba6de7e5240c | 171 | curMag = 1; |
hober | 20:ba6de7e5240c | 172 | int line = 0; |
hober | 20:ba6de7e5240c | 173 | for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++) |
hober | 20:ba6de7e5240c | 174 | { |
hober | 20:ba6de7e5240c | 175 | if((curMag & magSel) != 0) |
hober | 20:ba6de7e5240c | 176 | { |
hober | 20:ba6de7e5240c | 177 | memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10); |
hober | 20:ba6de7e5240c | 178 | } |
hober | 20:ba6de7e5240c | 179 | curMag <<= 1; |
hober | 20:ba6de7e5240c | 180 | } |
hober | 20:ba6de7e5240c | 181 | if(pc.writeable()) { |
hober | 20:ba6de7e5240c | 182 | for(int i = 0; i < line*10; i++) pc.putc(magData[i]); |
hober | 20:ba6de7e5240c | 183 | //pc.putc('\0'); |
hober | 20:ba6de7e5240c | 184 | }/* |
hober | 20:ba6de7e5240c | 185 | if(pc.writeable()) { |
hober | 20:ba6de7e5240c | 186 | for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++) |
hober | 20:ba6de7e5240c | 187 | { |
hober | 20:ba6de7e5240c | 188 | if((curMag & magSel) != 0) echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0]); |
hober | 20:ba6de7e5240c | 189 | curMag <<= 1; |
hober | 20:ba6de7e5240c | 190 | } |
hober | 20:ba6de7e5240c | 191 | // wait(waitTime); |
hober | 20:ba6de7e5240c | 192 | }*/ |
hober | 7:ee0569d49c52 | 193 | n++; |
hober | 20:ba6de7e5240c | 194 | if(isRecording && n == recordTime) { |
hober | 20:ba6de7e5240c | 195 | nextRecord.detach(); |
hober | 20:ba6de7e5240c | 196 | n = 0; |
hober | 20:ba6de7e5240c | 197 | isRecording = false; |
hober | 20:ba6de7e5240c | 198 | echo('S',0,0,0); |
hober | 20:ba6de7e5240c | 199 | } |
hober | 8:33d34a775873 | 200 | }// end recording if |
hober | 20:ba6de7e5240c | 201 | if(getMag>0 && isTimeToRecord) { |
hober | 20:ba6de7e5240c | 202 | isTimeToRecord = false; |
hober | 20:ba6de7e5240c | 203 | char curMag = 1; |
hober | 20:ba6de7e5240c | 204 | int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0}; |
hober | 20:ba6de7e5240c | 205 | for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++) |
hober | 20:ba6de7e5240c | 206 | { |
hober | 20:ba6de7e5240c | 207 | if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0])); |
hober | 20:ba6de7e5240c | 208 | curMag <<= 1; |
hober | 20:ba6de7e5240c | 209 | } |
hober | 20:ba6de7e5240c | 210 | curMag = 1; |
hober | 20:ba6de7e5240c | 211 | int line = 0; |
hober | 20:ba6de7e5240c | 212 | for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++) |
hober | 20:ba6de7e5240c | 213 | { |
hober | 20:ba6de7e5240c | 214 | if((curMag & magSel) != 0) |
hober | 20:ba6de7e5240c | 215 | { |
hober | 20:ba6de7e5240c | 216 | memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10); |
hober | 20:ba6de7e5240c | 217 | } |
hober | 20:ba6de7e5240c | 218 | curMag <<= 1; |
hober | 20:ba6de7e5240c | 219 | } |
hober | 20:ba6de7e5240c | 220 | if(pc.writeable()) { |
hober | 20:ba6de7e5240c | 221 | for(int i = 0; i < line*10; i++) pc.putc(magData[i]); |
hober | 20:ba6de7e5240c | 222 | //pc.putc('\0'); |
hober | 20:ba6de7e5240c | 223 | } |
hober | 9:c4f7257dee47 | 224 | getMag--; |
hober | 9:c4f7257dee47 | 225 | } |
hober | 9:c4f7257dee47 | 226 | if(commandToDo) { |
hober | 9:c4f7257dee47 | 227 | platform.to(x,y,z); |
hober | 15:5dd07840023c | 228 | isEcho = true; |
hober | 9:c4f7257dee47 | 229 | commandToDo = false; |
hober | 15:5dd07840023c | 230 | continue; |
hober | 9:c4f7257dee47 | 231 | } |
hober | 9:c4f7257dee47 | 232 | if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) { |
hober | 9:c4f7257dee47 | 233 | if(leftCount > 0) { |
hober | 9:c4f7257dee47 | 234 | platform.go_left(); |
hober | 9:c4f7257dee47 | 235 | leftCount--; |
hober | 9:c4f7257dee47 | 236 | } |
hober | 9:c4f7257dee47 | 237 | if(rightCount > 0) { |
hober | 9:c4f7257dee47 | 238 | platform.go_right(); |
hober | 9:c4f7257dee47 | 239 | rightCount--; |
hober | 9:c4f7257dee47 | 240 | } |
hober | 9:c4f7257dee47 | 241 | if(upCount > 0) { |
hober | 9:c4f7257dee47 | 242 | platform.go_up(); |
hober | 9:c4f7257dee47 | 243 | upCount--; |
hober | 9:c4f7257dee47 | 244 | } |
hober | 9:c4f7257dee47 | 245 | if(downCount > 0) { |
hober | 9:c4f7257dee47 | 246 | platform.go_down(); |
hober | 9:c4f7257dee47 | 247 | downCount--; |
hober | 9:c4f7257dee47 | 248 | } |
hober | 9:c4f7257dee47 | 249 | if(forwardCount > 0) { |
hober | 9:c4f7257dee47 | 250 | platform.go_forward(); |
hober | 9:c4f7257dee47 | 251 | forwardCount--; |
hober | 9:c4f7257dee47 | 252 | } |
hober | 9:c4f7257dee47 | 253 | if(backwardCount > 0) { |
hober | 9:c4f7257dee47 | 254 | platform.go_backward(); |
hober | 9:c4f7257dee47 | 255 | backwardCount--; |
hober | 9:c4f7257dee47 | 256 | } |
hober | 15:5dd07840023c | 257 | isEcho = true; |
hober | 9:c4f7257dee47 | 258 | } |
hober | 20:ba6de7e5240c | 259 | if(isMagReset) |
hober | 20:ba6de7e5240c | 260 | { |
hober | 20:ba6de7e5240c | 261 | isMagReset = false; |
hober | 20:ba6de7e5240c | 262 | char curMag = 1; |
hober | 20:ba6de7e5240c | 263 | for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++) |
hober | 20:ba6de7e5240c | 264 | { |
hober | 20:ba6de7e5240c | 265 | if((curMag & magSel) != 0) platform.resetMagnetometer(i); |
hober | 20:ba6de7e5240c | 266 | curMag <<= 1; |
hober | 20:ba6de7e5240c | 267 | } |
hober | 20:ba6de7e5240c | 268 | } |
hober | 7:ee0569d49c52 | 269 | } // end while |
hober | 6:ce02d396c961 | 270 | |
hober | 6:ce02d396c961 | 271 | } |
hober | 6:ce02d396c961 | 272 | |
hober | 6:ce02d396c961 | 273 | void echo(char typ,float x, float y, float z) |
hober | 6:ce02d396c961 | 274 | { |
hober | 7:ee0569d49c52 | 275 | int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)}; |
hober | 7:ee0569d49c52 | 276 | echo(typ,p_data); |
hober | 6:ce02d396c961 | 277 | } |
hober | 6:ce02d396c961 | 278 | |
hober | 20:ba6de7e5240c | 279 | uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow) |
hober | 6:ce02d396c961 | 280 | { |
hober | 8:33d34a775873 | 281 | 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 | 282 | command->setEnvelopeData(tmp,7); |
hober | 10:a49cdab3810f | 283 | dataToSend = (uint8_t*)(command->getEnvelopeArray()); |
hober | 20:ba6de7e5240c | 284 | if(pc.writeable()&&isWriteNow) |
hober | 20:ba6de7e5240c | 285 | { |
hober | 20:ba6de7e5240c | 286 | for(int i = 0; i < 10; i++) pc.putc(dataToSend[i]); |
hober | 20:ba6de7e5240c | 287 | // pc.printf("%.*s",10,dataToSend); |
hober | 20:ba6de7e5240c | 288 | //pc.fsync(); |
hober | 20:ba6de7e5240c | 289 | //pc.putc('\0'); |
hober | 20:ba6de7e5240c | 290 | } |
hober | 20:ba6de7e5240c | 291 | return dataToSend; |
hober | 20:ba6de7e5240c | 292 | /* |
hober | 8:33d34a775873 | 293 | for(int i = 0; i < command->length(); i++) { |
hober | 10:a49cdab3810f | 294 | pc.putc(dataToSend[i]); |
hober | 20:ba6de7e5240c | 295 | }*/ |
hober | 6:ce02d396c961 | 296 | } |
hober | 6:ce02d396c961 | 297 | |
hober | 9:c4f7257dee47 | 298 | void Rx_interrupt() |
hober | 9:c4f7257dee47 | 299 | { |
hober | 9:c4f7257dee47 | 300 | char c; |
hober | 9:c4f7257dee47 | 301 | while(pc.readable()) { |
hober | 9:c4f7257dee47 | 302 | c = pc.getc(); |
hober | 9:c4f7257dee47 | 303 | tracker.parse(&c,1); |
hober | 6:ce02d396c961 | 304 | |
hober | 9:c4f7257dee47 | 305 | result = tracker.getEnvelope(); |
hober | 9:c4f7257dee47 | 306 | if(result!=NULL) { |
hober | 9:c4f7257dee47 | 307 | char *dataArray = result->getPayload(); |
hober | 9:c4f7257dee47 | 308 | switch(dataArray[0]) { |
hober | 20:ba6de7e5240c | 309 | // following alphabets is used |
hober | 20:ba6de7e5240c | 310 | // I O C X Y Z M R S P N B |
hober | 12:2e1b1b1726fa | 311 | case 'I': // pattern record |
hober | 12:2e1b1b1726fa | 312 | if(dataArray[1] == 'X') |
hober | 12:2e1b1b1726fa | 313 | { |
hober | 12:2e1b1b1726fa | 314 | pattern.xStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f; |
hober | 12:2e1b1b1726fa | 315 | pattern.xEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f; |
hober | 12:2e1b1b1726fa | 316 | pattern.xStep = (float) dataArray[6]/10.0f; |
hober | 12:2e1b1b1726fa | 317 | if((pattern.xStart-pattern.xEnd)*pattern.xStep>0) pattern.xStep = -pattern.xStep; |
hober | 12:2e1b1b1726fa | 318 | } |
hober | 12:2e1b1b1726fa | 319 | else if(dataArray[1] == 'Y') |
hober | 12:2e1b1b1726fa | 320 | { |
hober | 12:2e1b1b1726fa | 321 | pattern.yStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f; |
hober | 12:2e1b1b1726fa | 322 | pattern.yEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f; |
hober | 12:2e1b1b1726fa | 323 | pattern.yStep = (float) dataArray[6]/10.0f; |
hober | 12:2e1b1b1726fa | 324 | if((pattern.yStart-pattern.yEnd)*pattern.yStep>0) pattern.yStep = -pattern.yStep; |
hober | 12:2e1b1b1726fa | 325 | } |
hober | 12:2e1b1b1726fa | 326 | else if(dataArray[1] == 'Z') |
hober | 12:2e1b1b1726fa | 327 | { |
hober | 12:2e1b1b1726fa | 328 | pattern.zStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f; |
hober | 12:2e1b1b1726fa | 329 | pattern.zEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f; |
hober | 12:2e1b1b1726fa | 330 | pattern.zStep = (float) dataArray[6]/10.0f; |
hober | 12:2e1b1b1726fa | 331 | if((pattern.zStart-pattern.zEnd)*pattern.zStep>0) pattern.zStep = -pattern.zStep; |
hober | 12:2e1b1b1726fa | 332 | } |
hober | 12:2e1b1b1726fa | 333 | else if(dataArray[1] == 'N') |
hober | 12:2e1b1b1726fa | 334 | { |
hober | 14:9672e91010a3 | 335 | pattern.num = (int)((dataArray[2]<<8)+dataArray[3]); |
hober | 20:ba6de7e5240c | 336 | isTimeToRecord = true; |
hober | 20:ba6de7e5240c | 337 | // waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10); |
hober | 12:2e1b1b1726fa | 338 | } |
hober | 12:2e1b1b1726fa | 339 | else if(dataArray[1] == 'M') |
hober | 12:2e1b1b1726fa | 340 | { |
hober | 12:2e1b1b1726fa | 341 | if(dataArray[2] == 'B'){ |
hober | 12:2e1b1b1726fa | 342 | pattern.x = (float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 12:2e1b1b1726fa | 343 | pattern.y = (float)((dataArray[5]<<8)+dataArray[6])/10.0f; |
hober | 12:2e1b1b1726fa | 344 | } |
hober | 12:2e1b1b1726fa | 345 | if(dataArray[2] == 'U'){ |
hober | 12:2e1b1b1726fa | 346 | pattern.z = (float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 12:2e1b1b1726fa | 347 | pattern.state = TO; |
hober | 12:2e1b1b1726fa | 348 | printf("%.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %d\n", |
hober | 12:2e1b1b1726fa | 349 | 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 | 350 | } |
hober | 12:2e1b1b1726fa | 351 | } |
hober | 12:2e1b1b1726fa | 352 | |
hober | 20:ba6de7e5240c | 353 | else if(dataArray[1] == 'B') // initial |
hober | 12:2e1b1b1726fa | 354 | { |
hober | 12:2e1b1b1726fa | 355 | pattern.state = INIT; |
hober | 12:2e1b1b1726fa | 356 | } |
hober | 20:ba6de7e5240c | 357 | else if(dataArray[1] == 'S') pattern.state = NONE;// stop |
hober | 12:2e1b1b1726fa | 358 | |
hober | 9:c4f7257dee47 | 359 | break; |
hober | 9:c4f7257dee47 | 360 | case 'O': // echo |
hober | 12:2e1b1b1726fa | 361 | isEcho = true; |
hober | 9:c4f7257dee47 | 362 | break; |
hober | 9:c4f7257dee47 | 363 | case 'C': // command |
hober | 12:2e1b1b1726fa | 364 | if(commandToDo) break; |
hober | 9:c4f7257dee47 | 365 | x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f; |
hober | 9:c4f7257dee47 | 366 | y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 9:c4f7257dee47 | 367 | z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f; |
hober | 9:c4f7257dee47 | 368 | commandToDo = true; |
hober | 9:c4f7257dee47 | 369 | break; |
hober | 20:ba6de7e5240c | 370 | case 'X': // move in X direction |
hober | 9:c4f7257dee47 | 371 | if(dataArray[1]&0x80) rightCount++; |
hober | 9:c4f7257dee47 | 372 | else leftCount++; |
hober | 9:c4f7257dee47 | 373 | break; |
hober | 20:ba6de7e5240c | 374 | case 'Y': // move in Y direction |
hober | 9:c4f7257dee47 | 375 | if(dataArray[1]&0x80) forwardCount++; |
hober | 10:a49cdab3810f | 376 | else backwardCount++; |
hober | 9:c4f7257dee47 | 377 | break; |
hober | 20:ba6de7e5240c | 378 | case 'Z': // move in Z direction |
hober | 9:c4f7257dee47 | 379 | if(dataArray[1]&0x80) upCount++; |
hober | 9:c4f7257dee47 | 380 | else downCount++; |
hober | 9:c4f7257dee47 | 381 | break; |
hober | 9:c4f7257dee47 | 382 | case 'M': // magnet |
hober | 9:c4f7257dee47 | 383 | getMag++; |
hober | 20:ba6de7e5240c | 384 | magSel = dataArray[1]; |
hober | 20:ba6de7e5240c | 385 | isTimeToRecord = true; |
hober | 20:ba6de7e5240c | 386 | pc.putc('M'); |
hober | 9:c4f7257dee47 | 387 | break; |
hober | 9:c4f7257dee47 | 388 | case 'R': // record |
hober | 9:c4f7257dee47 | 389 | recordTime = dataArray[1]; |
hober | 9:c4f7257dee47 | 390 | recordTime *= Fs; |
hober | 20:ba6de7e5240c | 391 | magSel = dataArray[2]; |
hober | 20:ba6de7e5240c | 392 | // mag_test=!mag_test; |
hober | 20:ba6de7e5240c | 393 | /* |
hober | 15:5dd07840023c | 394 | waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10); |
hober | 10:a49cdab3810f | 395 | n = 0; |
hober | 20:ba6de7e5240c | 396 | if(waitTime < 0) waitTime = 0;*/ |
hober | 20:ba6de7e5240c | 397 | isTimeToRecord = true; |
hober | 9:c4f7257dee47 | 398 | isRecording = true; |
hober | 9:c4f7257dee47 | 399 | break; |
hober | 9:c4f7257dee47 | 400 | case 'S': // stop |
hober | 9:c4f7257dee47 | 401 | isRecording = false; |
hober | 20:ba6de7e5240c | 402 | nextRecord.detach(); |
hober | 20:ba6de7e5240c | 403 | isTimeToRecord = false; |
hober | 9:c4f7257dee47 | 404 | break; |
hober | 12:2e1b1b1726fa | 405 | case 'P': // set position |
hober | 12:2e1b1b1726fa | 406 | x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f; |
hober | 12:2e1b1b1726fa | 407 | y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f; |
hober | 12:2e1b1b1726fa | 408 | z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f; |
hober | 12:2e1b1b1726fa | 409 | platform.setPosition(x,y,z); |
hober | 12:2e1b1b1726fa | 410 | break; |
hober | 12:2e1b1b1726fa | 411 | case 'N': // new set up |
hober | 12:2e1b1b1726fa | 412 | isReset = true; |
hober | 12:2e1b1b1726fa | 413 | break; |
hober | 20:ba6de7e5240c | 414 | case 'B': |
hober | 20:ba6de7e5240c | 415 | magSel = dataArray[1]; |
hober | 20:ba6de7e5240c | 416 | isMagReset = true; |
hober | 20:ba6de7e5240c | 417 | break; |
hober | 9:c4f7257dee47 | 418 | default: |
hober | 9:c4f7257dee47 | 419 | break; |
hober | 9:c4f7257dee47 | 420 | } // end switch |
hober | 9:c4f7257dee47 | 421 | result = NULL; |
hober | 9:c4f7257dee47 | 422 | dataArray = NULL; |
hober | 9:c4f7257dee47 | 423 | } // end result if |
hober | 9:c4f7257dee47 | 424 | } // end parsing if |
hober | 9:c4f7257dee47 | 425 | } |
hober | 9:c4f7257dee47 | 426 |