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: XYZ_sensor_Platform_SPI mbed
Diff: ui.cpp
- Revision:
- 6:ce02d396c961
- Child:
- 7:ee0569d49c52
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ui.cpp Fri Nov 10 02:39:32 2017 +0000 @@ -0,0 +1,131 @@ +#include "motor.h" +#include "xyz_sensor_platform.h" +#include "ParseArray.h" + + +Serial pc(SERIAL_TX, SERIAL_RX); +DigitalOut led(LED2); +XYZSensorPlatform platform; +ParseArray *commandParse; +static const int bufferArrayLength = 100; +static int currentBufferIndex; +static byte* bufferArray; +static byte* dataArray; +byte Xor; +byte commandToSend[10]= {'H','O',0,0,0,0,0,0,'E',0}; +static const int dataLength = 7; +void echo(char typ, float x, float y, float z); +void echo(char typ, int16_t *p_data); +int main() +{ + float x, y, z; + float pos[3]; + led=1; + bufferArray = new byte[bufferArrayLength]; + dataArray = new byte[dataLength]; + currentBufferIndex = 0; + commandParse = new ParseArray; + commandParse->bufferLength = bufferArrayLength; + commandParse->bufferArray = new byte[bufferArrayLength]; + commandParse->enableHeader(0,1,"48");// 48 H + commandParse->enableFooter(8,1,"45");// 45 E + commandParse->enableCheckXOR(9); + pc.baud(115200); + platform.set_speed(2.5); + platform.reset(); + while(1) { + if(pc.readable()) { +// commandParse->bufferArray[currentBufferIndex]=pc.getc(); + pc.read((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL); +// byte m[]={"read "}; +// while(pc.writeable()!=true); + pc.write((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL); +//echo('O',0,0,0); +//pc.printf("..."); + if(commandParse->parse(currentBufferIndex)) { + pc.write(commandToSend,2,NULL); + for(int i = commandParse->currentHeaderIndex + 1, j = 0; i < commandParse->currentFooterIndex; i++, j++) + dataArray[j] = commandParse->bufferArray[i % commandParse->bufferLength]; + switch(dataArray[0]) { + case 'O': // echo + platform.position(pos); + echo('O',pos[0],pos[1],pos[2]); + break; + case 'C': // command + x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f; + y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f; + z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f; + platform.to(x,y,z); + platform.position(pos); + echo('O',pos[0],pos[1],pos[2]); + break; + case 'X': + if(dataArray[1]&0x80)platform.go_right(); + else platform.go_left(); + platform.position(pos); + echo('O',pos[0],pos[1],pos[2]); + break; + case 'Y': + if(dataArray[1]&0x80)platform.go_forward(); + else platform.go_backward(); + platform.position(pos); + echo('O',pos[0],pos[1],pos[2]); + break; + case 'Z': + if(dataArray[1]&0x80)platform.go_up(); + else platform.go_down(); + platform.position(pos); + echo('O',pos[0],pos[1],pos[2]); + break; + case 'M': + int16_t mag[3]; + if(platform.get_mag_raw(mag)==0) echo('M',mag); + break; + default: + break; + } + + } + currentBufferIndex++; + currentBufferIndex%=bufferArrayLength; + } + led=!led; + } + +} + +void echo(char typ,float x, float y, float z) +{ + byte x_b[2] = {((int)(x*10)>>8)&0xFF, (int)(x*10)&0xFF}; + byte y_b[2] = {((int)(y*10)>>8)&0xFF, (int)(y*10)&0xFF}; + byte z_b[2] = {((int)(z*10)>>8)&0xFF, (int)(z*10)&0xFF}; + + commandToSend[0] = 'H'; + commandToSend[1] = typ; + commandToSend[2] = x_b[0]; + commandToSend[3] = x_b[1]; + commandToSend[4] = y_b[0]; + commandToSend[5] = y_b[1]; + commandToSend[6] = z_b[0]; + commandToSend[7] = z_b[1]; + commandToSend[8] = 'E'; + + Xor = commandParse->computeXOR(commandToSend,10); + commandToSend[9]=Xor; + pc.write(commandToSend,10,NULL); +// pc.flush(); +} + +void echo(char typ, int16_t *p_data) +{ + byte tmp[]= {'H', typ, \ + p_data[0]>>8, p_data[0], \ + p_data[1]>>8 ,p_data[1], \ + p_data[2]>>8, p_data[2], \ + 'E',0 + }; + tmp[9]=commandParse->computeXOR(tmp,10); + pc.printf("%10s",tmp); +} + +