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:
- 20:ba6de7e5240c
- Parent:
- 15:5dd07840023c
diff -r 4706f06cc480 -r ba6de7e5240c ui.cpp
--- a/ui.cpp Fri Aug 10 00:48:13 2018 +0000
+++ b/ui.cpp Mon Aug 13 01:49:53 2018 +0000
@@ -1,20 +1,23 @@
#include "motor.h"
#include "xyz_sensor_platform.h"
-//#include "ParseArray.h"
#include "envelopetracker.h"
-#define I2C_FREQUENCY 400000
-#define DELAY_COMPENSATE 0.000537
+#define I2C_FREQUENCY 1000000
+#define SPI_FREQUENCY 1e7
+//#define DELAY_COMPENSATE 0.000537
+#define BIG_CHAR_MASK 0x1F
+#define BAUD 921600
+#define Fs 1e3 // sampling rate -- max: 1kHz
typedef unsigned char byte;
uint8_t* dataToSend;
int sendArrayIndex = 0;
int sendBufferMax = 10000;
-const int BAUD = 921600;
Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
DigitalOut led(LED2),led3(LED3);
-DigitalOut mag_test(D11);
+DigitalOut mag_test(PC_8);
InterruptIn button(USER_BUTTON);
+Timeout nextRecord;
XYZSensorPlatform platform;
Envelope *command;
EnvelopeTracker tracker;
@@ -25,6 +28,7 @@
float pos[3];
int n = 0;
int getMag = 0;
+char magSel = 0;
int leftCount = 0;
int rightCount = 0;
int upCount = 0;
@@ -36,6 +40,9 @@
float waitTime;
bool isEcho = false;
bool isReset = false;
+bool isTimeToRecord = false;
+bool isMagReset = false;
+uint8_t magData[80] = {0}; //80 bytes
enum PatternRecordState{NONE,INIT,MAGNET,TO};
struct Pattern
{
@@ -47,21 +54,26 @@
} pattern;
-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);
+uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow = true);
void Rx_interrupt();
+void recordTimeup()
+{
+ isTimeToRecord = true;
+}
void released()
{
led = !led;
pc.attach(&Rx_interrupt, Serial::RxIrq);
}
+void no_call(){}
int main()
{
led=1;
+// char msg[] = "start!!";
+// fwrite(msg,1, 7,stdout);
command = new Envelope;
command->enableHeader(std::string("H"));// 48 H
command->enableFooter(std::string("E"),8);// 45 E
@@ -70,13 +82,14 @@
tracker.setBufferLength(100);
pattern.state = NONE;
pc.format(8,SerialBase::None,1);
- button.rise(&released);
+// button.rise(&released);
platform.set_speed(2.5);
// platform.reset(); // need to be modified here
+ platform.setSensorSpiFrequency(SPI_FREQUENCY);
platform.setSensorI2cFrequency(I2C_FREQUENCY);
// Setup a serial interrupt function to receive data
pc.attach(&Rx_interrupt, Serial::RxIrq);
- echo('B',0,0,0);
+// echo('B',0,0,0);
while(1) {
if(platform.isMoving()) continue;
if(isReset){
@@ -132,7 +145,8 @@
{
if(pattern.count-- <= 0) pattern.state = TO;
getMag++;
- wait(waitTime);
+ nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
+// wait(waitTime);
}
}
if(isEcho)
@@ -141,23 +155,72 @@
echo('O',pos[0],pos[1],pos[2]);
led3 = !led3;
isEcho = false;
+ // printf(" %f ",1.0f/(float)Fs);
}
- if(isRecording && n < recordTime) {
- int16_t mag[3];
- if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
- echo('M',mag);
- mag_test=!mag_test;
- wait(waitTime);
+ if(isRecording && n < recordTime && isTimeToRecord) {
+ nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
+ isTimeToRecord = false;
+ char curMag = 1;
+ int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0};
+ mag_test=!mag_test;
+ for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
+ {
+ if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0]));
+ curMag <<= 1;
}
+ curMag = 1;
+ int line = 0;
+ for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
+ {
+ if((curMag & magSel) != 0)
+ {
+ memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10);
+ }
+ curMag <<= 1;
+ }
+ if(pc.writeable()) {
+ for(int i = 0; i < line*10; i++) pc.putc(magData[i]);
+ //pc.putc('\0');
+ }/*
+ if(pc.writeable()) {
+ for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
+ {
+ if((curMag & magSel) != 0) echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0]);
+ curMag <<= 1;
+ }
+// wait(waitTime);
+ }*/
n++;
- } else if(isRecording) {
- isRecording = false;
- echo('S',0,0,0);
+ if(isRecording && n == recordTime) {
+ nextRecord.detach();
+ n = 0;
+ isRecording = false;
+ echo('S',0,0,0);
+ }
}// end recording if
- if(getMag>0) {
- int16_t mag[3];
- if(platform.get_mag_raw(mag)==0) echo('M',mag);
- mag_test=!mag_test;
+ if(getMag>0 && isTimeToRecord) {
+ isTimeToRecord = false;
+ char curMag = 1;
+ int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0};
+ for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
+ {
+ if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0]));
+ curMag <<= 1;
+ }
+ curMag = 1;
+ int line = 0;
+ for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
+ {
+ if((curMag & magSel) != 0)
+ {
+ memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10);
+ }
+ curMag <<= 1;
+ }
+ if(pc.writeable()) {
+ for(int i = 0; i < line*10; i++) pc.putc(magData[i]);
+ //pc.putc('\0');
+ }
getMag--;
}
if(commandToDo) {
@@ -193,6 +256,16 @@
}
isEcho = true;
}
+ if(isMagReset)
+ {
+ isMagReset = false;
+ char curMag = 1;
+ for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
+ {
+ if((curMag & magSel) != 0) platform.resetMagnetometer(i);
+ curMag <<= 1;
+ }
+ }
} // end while
}
@@ -203,14 +276,23 @@
echo(typ,p_data);
}
-void echo(char typ, int16_t *p_data)
+uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow)
{
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);
dataToSend = (uint8_t*)(command->getEnvelopeArray());
+ if(pc.writeable()&&isWriteNow)
+ {
+ for(int i = 0; i < 10; i++) pc.putc(dataToSend[i]);
+ // pc.printf("%.*s",10,dataToSend);
+ //pc.fsync();
+ //pc.putc('\0');
+ }
+ return dataToSend;
+ /*
for(int i = 0; i < command->length(); i++) {
pc.putc(dataToSend[i]);
- }
+ }*/
}
void Rx_interrupt()
@@ -224,6 +306,8 @@
if(result!=NULL) {
char *dataArray = result->getPayload();
switch(dataArray[0]) {
+ // following alphabets is used
+ // I O C X Y Z M R S P N B
case 'I': // pattern record
if(dataArray[1] == 'X')
{
@@ -249,7 +333,8 @@
else if(dataArray[1] == 'N')
{
pattern.num = (int)((dataArray[2]<<8)+dataArray[3]);
- waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
+ isTimeToRecord = true;
+// waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
}
else if(dataArray[1] == 'M')
{
@@ -265,11 +350,11 @@
}
}
- else if(dataArray[1] == 'B')
+ else if(dataArray[1] == 'B') // initial
{
pattern.state = INIT;
}
- else if(dataArray[1] == 'S') pattern.state = NONE;
+ else if(dataArray[1] == 'S') pattern.state = NONE;// stop
break;
case 'O': // echo
@@ -282,32 +367,40 @@
z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
commandToDo = true;
break;
- case 'X':
+ case 'X': // move in X direction
if(dataArray[1]&0x80) rightCount++;
else leftCount++;
break;
- case 'Y':
+ case 'Y': // move in Y direction
if(dataArray[1]&0x80) forwardCount++;
else backwardCount++;
break;
- case 'Z':
+ case 'Z': // move in Z direction
if(dataArray[1]&0x80) upCount++;
else downCount++;
break;
case 'M': // magnet
getMag++;
- mag_test=!mag_test;
+ magSel = dataArray[1];
+ isTimeToRecord = true;
+ pc.putc('M');
break;
case 'R': // record
recordTime = dataArray[1];
recordTime *= Fs;
+ magSel = dataArray[2];
+// mag_test=!mag_test;
+ /*
waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
n = 0;
- if(waitTime < 0) waitTime = 0;
+ if(waitTime < 0) waitTime = 0;*/
+ isTimeToRecord = true;
isRecording = true;
break;
case 'S': // stop
isRecording = false;
+ nextRecord.detach();
+ isTimeToRecord = false;
break;
case 'P': // set position
x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
@@ -318,6 +411,10 @@
case 'N': // new set up
isReset = true;
break;
+ case 'B':
+ magSel = dataArray[1];
+ isMagReset = true;
+ break;
default:
break;
} // end switch