Shih-Ho Hsieh / Mbed 2 deprecated pololu5mag_with_platform

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

Fork of Motor_XYZ_UI_SPI_I2C_5mag by Shih-Ho Hsieh

ui.cpp@6:ce02d396c961, 2017-11-10 (annotated)

Committer:
hober
Date:
Fri Nov 10 02:39:32 2017 +0000
Revision:
6:ce02d396c961
Child:
7:ee0569d49c52
20171110 error

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