Revision 15:67c0fbc0999a, committed 2018-08-03

Comitter:

hober

Date:

Fri Aug 03 01:51:51 2018 +0000

Branch:

envelope

Parent:

14:9672e91010a3

Commit message:

Motor XYZ with UI through SPI interface to extend board magring which contains 8 magnetometers.

Changed in this revision

--- a/XYZ_sensor_Platform.lib	Tue Mar 06 08:35:40 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/hober/code/XYZ_sensor_Platform/#1aa1d5ec949b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XYZ_sensor_Platform_SPI.lib	Fri Aug 03 01:51:51 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/hober/code/XYZ_sensor_Platform_SPI/#db753c75e53f

--- a/mbed.bld	Tue Mar 06 08:35:40 2018 +0000
+++ b/mbed.bld	Fri Aug 03 01:51:51 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/aa5281ff4a02
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file

--- a/parse_array/ParseArray.cpp	Tue Mar 06 08:35:40 2018 +0000
+++ b/parse_array/ParseArray.cpp	Fri Aug 03 01:51:51 2018 +0000
@@ -27,7 +27,7 @@
     byte* bytes = new byte[length];
     long value;
     string subS;
-    char arr[2];
+//    char arr[2];
     //strtol( &hexstring[1], NULL, 16);
     for (int i = 0; i < length; i += 1) {
         subS = hex.substr(i * 3, 2);

--- a/ui.cpp	Tue Mar 06 08:35:40 2018 +0000
+++ b/ui.cpp	Fri Aug 03 01:51:51 2018 +0000
@@ -1,19 +1,23 @@
 #include "motor.h"
 #include "xyz_sensor_platform.h"
-//#include "ParseArray.h"
 #include "envelopetracker.h"
 
 #define I2C_FREQUENCY 400000
+#define SPI_FREQUENCY 0.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;
@@ -24,6 +28,7 @@
 float pos[3];
 int n = 0;
 int getMag = 0;
+char magSel = 0;
 int leftCount = 0;
 int rightCount = 0;
 int upCount = 0;
@@ -34,7 +39,10 @@
 int recordTime;
 float waitTime;
 bool isEcho = false;
-bool isReset = true;
+bool isReset = false;
+bool isTimeToRecord = false;
+bool isMagReset = false;
+uint8_t magData[80] = {0};  //80 bytes
 enum PatternRecordState{NONE,INIT,MAGNET,TO};
 struct Pattern
 {
@@ -46,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
@@ -69,14 +82,15 @@
     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.setSensorI2cFrequency(I2C_FREQUENCY);
+    platform.setSensorSpiFrequency(SPI_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){
             platform.set_speed(1);
             platform.reset();
@@ -92,8 +106,10 @@
                 pattern.z = pattern.zStart;
                 pattern.count = pattern.num;
                 pattern.state = MAGNET;
+                //isEcho = true;
+                platform.to(pattern.x,pattern.y,pattern.z);
                 isEcho = true;
-                platform.to(pattern.x,pattern.y,pattern.z);
+                continue;
             }
             if(pattern.count == 0) pattern.state = TO;
             if(pattern.state == TO) 
@@ -122,12 +138,14 @@
                 pattern.count = pattern.num;
                 pattern.state = MAGNET;
                 platform.set_speed(2.5);
+                continue;
             }
             if(pattern.state == MAGNET)
             {
                 if(pattern.count-- <= 0) pattern.state = TO;
                 getMag++;
-                wait(waitTime);
+                nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
+//                wait(waitTime);
             }
         }
         if(isEcho)
@@ -136,29 +154,79 @@
             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);
+        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) {
             platform.to(x,y,z);
-            platform.position(pos);
-            echo('O',pos[0],pos[1],pos[2]);
+            isEcho = true;
             commandToDo = false;
+            continue;
         }
         if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
             if(leftCount > 0) {
@@ -185,8 +253,17 @@
                 platform.go_backward();
                 backwardCount--;
             }
-            platform.position(pos);
-            echo('O',pos[0],pos[1],pos[2]);
+            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
 
@@ -198,14 +275,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()
@@ -219,6 +305,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')
                     {
@@ -244,7 +332,8 @@
                     else if(dataArray[1] == 'N')
                     {
                         pattern.num = (int)((dataArray[2]<<8)+dataArray[3]);
-                        waitTime = 1.0/Fs-0.00052-1/(BAUD/8/10);
+                        isTimeToRecord = true;
+//                        waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
                     }
                     else if(dataArray[1] == 'M')
                     {
@@ -260,11 +349,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
@@ -277,32 +366,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;
-                    waitTime = 1.0/Fs-0.00052-1/(BAUD/8/10);
+                    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;
@@ -313,6 +410,10 @@
                 case 'N': // new set up
                     isReset = true;
                     break;
+                case 'B':
+                    magSel = dataArray[1];
+                    isMagReset = true;
+                    break;
                 default:
                     break;
             } // end switch