Shih-Ho Hsieh
/
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.
Diff: ui.cpp
- Branch:
- envelope
- Revision:
- 12:2e1b1b1726fa
- Parent:
- 11:bb86ffb378f4
- Child:
- 13:6850d2b41b2c
--- a/ui.cpp Fri Jan 05 08:06:13 2018 +0000
+++ b/ui.cpp Fri Mar 02 06:40:25 2018 +0000
@@ -13,6 +13,7 @@
Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
DigitalOut led(LED2),led3(LED3);
DigitalOut mag_test(D11);
+InterruptIn button(USER_BUTTON);
XYZSensorPlatform platform;
Envelope *command;
EnvelopeTracker tracker;
@@ -32,6 +33,18 @@
bool commandToDo = false;
int recordTime;
float waitTime;
+bool isEcho = false;
+bool isReset = true;
+enum PatternRecordState{NONE,INIT,MAGNET,TO};
+struct Pattern
+{
+ float xStart, xEnd, xStep, yStart, yEnd, yStep, zStart, zEnd, zStep;
+ int num;
+ float x, y, z;
+ int count;
+ PatternRecordState state;
+} pattern;
+
const int Fs = 1000; // sampling rate -- max: 1kHz
@@ -39,6 +52,11 @@
void echo(char typ, float x, float y, float z);
void echo(char typ, int16_t *p_data);
void Rx_interrupt();
+void released()
+{
+ led = !led;
+ pc.attach(&Rx_interrupt, Serial::RxIrq);
+}
int main()
{
@@ -49,13 +67,61 @@
command->enableCheckXOR(9);
tracker.setEnvelope(*command);
tracker.setBufferLength(100);
+ pattern.state = NONE;
pc.format(8,SerialBase::None,1);
+ button.rise(&released);
platform.set_speed(2.5);
- platform.reset(); // need to be modified here
+// platform.reset(); // need to be modified here
platform.setSensorI2cFrequency(I2C_FREQUENCY);
// Setup a serial interrupt function to receive data
pc.attach(&Rx_interrupt, Serial::RxIrq);
+ echo('B',0,0,0);
while(1) {
+ if(isReset){
+ platform.set_speed(1);
+ platform.reset();
+ isReset = false;
+ platform.set_speed(2.5);
+ }
+ if(pattern.state != NONE)
+ {
+ if(pattern.state == INIT) platform.to(pattern.xStart,pattern.yStart,pattern.zStart);
+ if(pattern.count == 0) pattern.state = TO;
+ if(pattern.state == TO)
+ {
+ if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep)) pattern.x += pattern.xStep;
+ else if(abs(pattern.y-pattern.yEnd)>abs(pattern.yStep)){
+ pattern.x = pattern.xStart;
+ pattern.y += pattern.yStep;
+ }
+ else if(abs(pattern.z-pattern.zEnd)>abs(pattern.zStep)){
+ pattern.x = pattern.xStart;
+ pattern.y = pattern.yStart;
+ pattern.z += pattern.zStep;
+ }
+ else{
+ pattern.state = NONE;
+ continue;
+ }
+ platform.to(pattern.x,pattern.y,pattern.z);
+// wait(0.);
+ isEcho = true;
+ pattern.count = pattern.num;
+ pattern.state = MAGNET;
+ }
+ if(pattern.state == MAGNET)
+ {
+ if(pattern.count-- <= 0) pattern.state = TO;
+ getMag++;
+ }
+ }
+ if(isEcho)
+ {
+ platform.position(pos);
+ echo('O',pos[0],pos[1],pos[2]);
+ led3 = !led3;
+ isEcho = false;
+ }
if(isRecording && n < recordTime) {
int16_t mag[3];
if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
@@ -138,14 +204,59 @@
if(result!=NULL) {
char *dataArray = result->getPayload();
switch(dataArray[0]) {
- case 'I':
+ case 'I': // pattern record
+ if(dataArray[1] == 'X')
+ {
+ pattern.xStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
+ pattern.xEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
+ pattern.xStep = (float) dataArray[6]/10.0f;
+ if((pattern.xStart-pattern.xEnd)*pattern.xStep>0) pattern.xStep = -pattern.xStep;
+ }
+ else if(dataArray[1] == 'Y')
+ {
+ pattern.yStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
+ pattern.yEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
+ pattern.yStep = (float) dataArray[6]/10.0f;
+ if((pattern.yStart-pattern.yEnd)*pattern.yStep>0) pattern.yStep = -pattern.yStep;
+ }
+ else if(dataArray[1] == 'Z')
+ {
+ pattern.zStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
+ pattern.zEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
+ pattern.zStep = (float) dataArray[6]/10.0f;
+ if((pattern.zStart-pattern.zEnd)*pattern.zStep>0) pattern.zStep = -pattern.zStep;
+ }
+ else if(dataArray[1] == 'N')
+ {
+ pattern.num = (int)dataArray[2];
+ }
+ else if(dataArray[1] == 'M')
+ {
+ if(dataArray[2] == 'B'){
+ pattern.x = (float)((dataArray[3]<<8)+dataArray[4])/10.0f;
+ pattern.y = (float)((dataArray[5]<<8)+dataArray[6])/10.0f;
+ }
+ if(dataArray[2] == 'U'){
+ pattern.z = (float)((dataArray[3]<<8)+dataArray[4])/10.0f;
+ pattern.state = TO;
+ printf("%.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %d\n",
+ 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);
+ }
+ }
+
+ else if(dataArray[1] == 'B')
+ {
+ pattern.state = INIT;
+ pattern.count = pattern.num;
+ }
+ else if(dataArray[1] == 'S') pattern.state = NONE;
+
break;
case 'O': // echo
- platform.position(pos);
- echo('O',pos[0],pos[1],pos[2]);
- led3 = !led3;
+ isEcho = true;
break;
case 'C': // command
+ if(commandToDo) break;
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;
@@ -178,10 +289,18 @@
case 'S': // stop
isRecording = false;
break;
+ case 'P': // set position
+ 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.setPosition(x,y,z);
+ break;
+ case 'N': // new set up
+ isReset = true;
+ break;
default:
break;
} // end switch
-// delete dataArray;
result = NULL;
dataArray = NULL;
} // end result if