Shih-Ho Hsieh / Mbed 2 deprecated Motor_XYZ_UI_SPI_I2C_5mag

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@15:5dd07840023c, 2018-08-08 (annotated)

Committer:
hober
Date:
Wed Aug 08 07:58:49 2018 +0000
Branch:
envelope
Revision:
15:5dd07840023c
Parent:
14:9672e91010a3
Child:
20:ba6de7e5240c
2018/08/08 new branch.

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