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:
- 9:c4f7257dee47
- Parent:
- 8:33d34a775873
- Child:
- 10:a49cdab3810f
--- a/ui.cpp Tue Dec 12 08:47:56 2017 +0000
+++ b/ui.cpp Tue Dec 12 09:37:33 2017 +0000
@@ -18,25 +18,32 @@
EnvelopeTracker tracker;
byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
bool isRecording = false;
+Envelope* result;
+float x, y, z;
+float pos[3];
+int n = 0;
+int getMag = 0;
+int leftCount = 0;
+int rightCount = 0;
+int upCount = 0;
+int downCount = 0;
+int forwardCount = 0;
+int backwardCount = 0;
+bool commandToDo = false;
+int recordTime;
+float waitTime;
-const int Fs = 100; // sampling rate -- max: 1kHz
+const int Fs = 1000; // sampling rate -- max: 1kHz
void echo(char typ, float x, float y, float z);
void echo(char typ, int16_t *p_data);
+void Rx_interrupt();
int main()
{
- Envelope* result;
- float x, y, z;
- float pos[3];
- int n = 0;
- int recordTime;
- float waitTime;
dataToSend = new uint8_t*[sendBufferMax];
-// uint8_t *ch_in = new char [10];
led=1;
- mag_test=1;
command = new Envelope;
command->enableHeader(std::string("H"));// 48 H
command->enableFooter(std::string("E"),8);// 45 E
@@ -47,73 +54,9 @@
platform.set_speed(2.5);
platform.reset(); // need to be modified here
platform.setSensorI2cFrequency(I2C_FREQUENCY);
- char c;
+// Setup a serial interrupt function to receive data
+ pc.attach(&Rx_interrupt, Serial::RxIrq);
while(1) {
- if(pc.readable()) {
- c = pc.getc();
- tracker.parse(&c,1);
-
- result = tracker.getEnvelope();
- if(result!=NULL) {
- char *dataArray = result->getPayload();
- switch(dataArray[0]) {
- case 'I':
- break;
- case 'O': // echo
- platform.position(pos);
- echo('O',pos[0],pos[1],pos[2]);
- led3 = !led3;
- 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': // magnet
- int16_t mag[3];
- if(platform.get_mag_raw(mag)==0) echo('M',mag);
- mag_test=!mag_test;
- break;
- case 'R': // record
- recordTime = dataArray[1];
- recordTime *= Fs;
- waitTime = 1.0/Fs-0.0005-1/(BAUD/8/10);
- if(waitTime < 0) waitTime = 0;
- isRecording = true;
- break;
- case 'S': // stop
- isRecording = false;
- break;
- default:
- break;
- } // end switch
-// delete result;
- delete dataArray;
- result = NULL;
- dataArray = NULL;
- } // end result if
- } // end parsing if
if(isRecording && n < recordTime) {
int16_t mag[3];
if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
@@ -123,12 +66,53 @@
}
n++;
} else if(isRecording) {
- n = 0;
isRecording = false;
- for(int i = 0; i < sendBufferMax; i++)
- if(dataToSend[i]) delete dataToSend[i];
echo('S',0,0,0);
}// end recording if
+ if(!isRecording && n != 0) {
+ n = 0;
+ for(int i = 0; i < sendBufferMax; i++)
+ if(dataToSend[i]) delete dataToSend[i];
+ }
+ if(getMag>0) {
+ int16_t mag[3];
+ if(platform.get_mag_raw(mag)==0) echo('M',mag);
+ getMag--;
+ }
+ if(commandToDo) {
+ platform.to(x,y,z);
+ platform.position(pos);
+ echo('O',pos[0],pos[1],pos[2]);
+ commandToDo = false;
+ }
+ if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
+ if(leftCount > 0) {
+ platform.go_left();
+ leftCount--;
+ }
+ if(rightCount > 0) {
+ platform.go_right();
+ rightCount--;
+ }
+ if(upCount > 0) {
+ platform.go_up();
+ upCount--;
+ }
+ if(downCount > 0) {
+ platform.go_down();
+ downCount--;
+ }
+ if(forwardCount > 0) {
+ platform.go_forward();
+ forwardCount--;
+ }
+ if(backwardCount > 0) {
+ platform.go_backward();
+ backwardCount--;
+ }
+ platform.position(pos);
+ echo('O',pos[0],pos[1],pos[2]);
+ }
} // end while
}
@@ -141,26 +125,75 @@
void echo(char typ, int16_t *p_data)
{
-// char * tmp = new char [7];
-// tmp[0] = typ;
-// tmp[1] = p_data[0]>>8;
-// tmp[2] = p_data[0];
-// tmp[3] = p_data[1]>>8;
-// tmp[4] = p_data[1];
-// tmp[5] = p_data[2]>>8;
-// tmp[6] = p_data[2];
char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
command->setEnvelopeData(tmp,7);
-// if(dataToSend[sendArrayIndex]) delete dataToSend[sendArrayIndex];
dataToSend[sendArrayIndex] = (uint8_t*)(command->getEnvelopeArray());
for(int i = 0; i < command->length(); i++) {
-// pc.write(dataToSend[sendArrayIndex],command->length(),NULL);
-// wait(0.0002);
pc.putc(dataToSend[sendArrayIndex][i]);
}
sendArrayIndex = (sendArrayIndex+1)%sendBufferMax;
-// delete dataToSend;
-// dataToSend = NULL;
}
+void Rx_interrupt()
+{
+ char c;
+ while(pc.readable()) {
+ c = pc.getc();
+ tracker.parse(&c,1);
+ result = tracker.getEnvelope();
+ if(result!=NULL) {
+ char *dataArray = result->getPayload();
+ switch(dataArray[0]) {
+ case 'I':
+ break;
+ case 'O': // echo
+ platform.position(pos);
+ echo('O',pos[0],pos[1],pos[2]);
+ led3 = !led3;
+ 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;
+ commandToDo = true;
+ break;
+ case 'X':
+ if(dataArray[1]&0x80) rightCount++;
+ else leftCount++;
+ break;
+ case 'Y':
+ if(dataArray[1]&0x80) forwardCount++;
+ else forwardCount++;
+ break;
+ case 'Z':
+ if(dataArray[1]&0x80) upCount++;
+ else downCount++;
+ break;
+ case 'M': // magnet
+ getMag++;
+// pc.putc(dataArray[0]);
+// echo('O',mag);
+ mag_test=!mag_test;
+ break;
+ case 'R': // record
+ recordTime = dataArray[1];
+ recordTime *= Fs;
+ waitTime = 1.0/Fs-0.0005-1/(BAUD/8/10);
+ if(waitTime < 0) waitTime = 0;
+ isRecording = true;
+ break;
+ case 'S': // stop
+ isRecording = false;
+ break;
+ default:
+ break;
+ } // end switch
+// delete result;
+ delete dataArray;
+ result = NULL;
+ dataArray = NULL;
+ } // end result if
+ } // end parsing if
+}
+