Shih-Ho Hsieh
/
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.
Fork of
Motor_XYZ_UI_SPI_I2C_5mag
by 
Shih-Ho Hsieh
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