Shih-Ho Hsieh / Mbed 2 deprecated pololu5mag_with_platform

5 sensors on a board with a moving platform.

Dependencies:   mbed

Fork of Motor_XYZ_UI_SPI_I2C_5mag by Shih-Ho Hsieh

ui.cpp@7:ee0569d49c52, 2017-12-08 (annotated)

Committer:
hober
Date:
Fri Dec 08 06:30:58 2017 +0000
Revision:
7:ee0569d49c52
Parent:
6:ce02d396c961
Child:
8:33d34a775873
20171208 stable version before envelope tracker

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 6:ce02d396c961 3 #include "ParseArray.h"
hober 6:ce02d396c961 4
hober 7:ee0569d49c52 5 #define I2C_FREQUENCY 400000
hober 6:ce02d396c961 6
hober 7:ee0569d49c52 7 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
hober 7:ee0569d49c52 8 DigitalOut led(LED2),led3(LED3);
hober 7:ee0569d49c52 9 DigitalOut mag_test(D11);
hober 6:ce02d396c961 10 XYZSensorPlatform platform;
hober 6:ce02d396c961 11 ParseArray *commandParse;
hober 6:ce02d396c961 12 static const int bufferArrayLength = 100;
hober 6:ce02d396c961 13 static int currentBufferIndex;
hober 6:ce02d396c961 14 static byte* bufferArray;
hober 6:ce02d396c961 15 static byte* dataArray;
hober 6:ce02d396c961 16 byte Xor;
hober 7:ee0569d49c52 17 byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
hober 6:ce02d396c961 18 static const int dataLength = 7;
hober 7:ee0569d49c52 19 bool isRecording = false;
hober 6:ce02d396c961 20 void echo(char typ, float x, float y, float z);
hober 6:ce02d396c961 21 void echo(char typ, int16_t *p_data);
hober 6:ce02d396c961 22 int main()
hober 6:ce02d396c961 23 {
hober 6:ce02d396c961 24 float x, y, z;
hober 6:ce02d396c961 25 float pos[3];
hober 7:ee0569d49c52 26 int n = 0;
hober 6:ce02d396c961 27 led=1;
hober 7:ee0569d49c52 28 mag_test=1;
hober 6:ce02d396c961 29 bufferArray = new byte[bufferArrayLength];
hober 6:ce02d396c961 30 dataArray = new byte[dataLength];
hober 6:ce02d396c961 31 currentBufferIndex = 0;
hober 6:ce02d396c961 32 commandParse = new ParseArray;
hober 6:ce02d396c961 33 commandParse->bufferLength = bufferArrayLength;
hober 6:ce02d396c961 34 commandParse->bufferArray = new byte[bufferArrayLength];
hober 6:ce02d396c961 35 commandParse->enableHeader(0,1,"48");// 48 H
hober 6:ce02d396c961 36 commandParse->enableFooter(8,1,"45");// 45 E
hober 6:ce02d396c961 37 commandParse->enableCheckXOR(9);
hober 7:ee0569d49c52 38 commandParse->length=10;
hober 7:ee0569d49c52 39 pc.format(8,SerialBase::None,1);
hober 6:ce02d396c961 40 platform.set_speed(2.5);
hober 7:ee0569d49c52 41 platform.reset(); // need to be modified here
hober 7:ee0569d49c52 42 platform.setSensorI2cFrequency(I2C_FREQUENCY);
hober 6:ce02d396c961 43 while(1) {
hober 6:ce02d396c961 44 if(pc.readable()) {
hober 6:ce02d396c961 45 pc.read((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
hober 6:ce02d396c961 46 if(commandParse->parse(currentBufferIndex)) {
hober 6:ce02d396c961 47 for(int i = commandParse->currentHeaderIndex + 1, j = 0; i < commandParse->currentFooterIndex; i++, j++)
hober 6:ce02d396c961 48 dataArray[j] = commandParse->bufferArray[i % commandParse->bufferLength];
hober 6:ce02d396c961 49 switch(dataArray[0]) {
hober 7:ee0569d49c52 50 case 'I':
hober 7:ee0569d49c52 51 break;
hober 6:ce02d396c961 52 case 'O': // echo
hober 6:ce02d396c961 53 platform.position(pos);
hober 6:ce02d396c961 54 echo('O',pos[0],pos[1],pos[2]);
hober 7:ee0569d49c52 55 led3 = !led3;
hober 6:ce02d396c961 56 break;
hober 6:ce02d396c961 57 case 'C': // command
hober 6:ce02d396c961 58 x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
hober 6:ce02d396c961 59 y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 6:ce02d396c961 60 z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 6:ce02d396c961 61 platform.to(x,y,z);
hober 6:ce02d396c961 62 platform.position(pos);
hober 6:ce02d396c961 63 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 64 break;
hober 6:ce02d396c961 65 case 'X':
hober 6:ce02d396c961 66 if(dataArray[1]&0x80)platform.go_right();
hober 6:ce02d396c961 67 else platform.go_left();
hober 6:ce02d396c961 68 platform.position(pos);
hober 6:ce02d396c961 69 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 70 break;
hober 6:ce02d396c961 71 case 'Y':
hober 6:ce02d396c961 72 if(dataArray[1]&0x80)platform.go_forward();
hober 6:ce02d396c961 73 else platform.go_backward();
hober 6:ce02d396c961 74 platform.position(pos);
hober 6:ce02d396c961 75 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 76 break;
hober 6:ce02d396c961 77 case 'Z':
hober 6:ce02d396c961 78 if(dataArray[1]&0x80)platform.go_up();
hober 6:ce02d396c961 79 else platform.go_down();
hober 6:ce02d396c961 80 platform.position(pos);
hober 6:ce02d396c961 81 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 82 break;
hober 7:ee0569d49c52 83 case 'M': // magnet
hober 6:ce02d396c961 84 int16_t mag[3];
hober 6:ce02d396c961 85 if(platform.get_mag_raw(mag)==0) echo('M',mag);
hober 7:ee0569d49c52 86 mag_test=!mag_test;
hober 7:ee0569d49c52 87 break;
hober 7:ee0569d49c52 88 case 'R': // record
hober 7:ee0569d49c52 89 isRecording = true;
hober 7:ee0569d49c52 90 break;
hober 7:ee0569d49c52 91 case 'S': // stop
hober 7:ee0569d49c52 92 isRecording = false;
hober 6:ce02d396c961 93 break;
hober 6:ce02d396c961 94 default:
hober 6:ce02d396c961 95 break;
hober 6:ce02d396c961 96 }
hober 6:ce02d396c961 97
hober 6:ce02d396c961 98 }
hober 6:ce02d396c961 99 currentBufferIndex++;
hober 6:ce02d396c961 100 currentBufferIndex%=bufferArrayLength;
hober 7:ee0569d49c52 101 } // end parsing if
hober 7:ee0569d49c52 102 if(isRecording && n < 10000) {
hober 7:ee0569d49c52 103 int16_t mag[3];
hober 7:ee0569d49c52 104 if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
hober 7:ee0569d49c52 105 echo('M',mag);
hober 7:ee0569d49c52 106 mag_test=!mag_test;
hober 7:ee0569d49c52 107 wait(0.0001);
hober 7:ee0569d49c52 108 }
hober 7:ee0569d49c52 109 n++;
hober 7:ee0569d49c52 110 } // end recording if
hober 7:ee0569d49c52 111 } // end while
hober 6:ce02d396c961 112
hober 6:ce02d396c961 113 }
hober 6:ce02d396c961 114
hober 6:ce02d396c961 115 void echo(char typ,float x, float y, float z)
hober 6:ce02d396c961 116 {
hober 7:ee0569d49c52 117 int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)};
hober 7:ee0569d49c52 118 echo(typ,p_data);
hober 6:ce02d396c961 119 }
hober 6:ce02d396c961 120
hober 6:ce02d396c961 121 void echo(char typ, int16_t *p_data)
hober 6:ce02d396c961 122 {
hober 7:ee0569d49c52 123 byte tmp[10]= {'H', typ, p_data[0]>>8, p_data[0], p_data[1]>>8 ,p_data[1], p_data[2]>>8, p_data[2], 45,'\0' };
hober 7:ee0569d49c52 124 tmp[8]='E';
hober 7:ee0569d49c52 125 tmp[9]=commandParse->computeXOR(tmp,9);
hober 7:ee0569d49c52 126 pc.write(tmp,10,NULL);
hober 6:ce02d396c961 127 }
hober 6:ce02d396c961 128
hober 6:ce02d396c961 129