Diff: main.cpp

Revision:

7:f33a935eb77a

Parent:

6:5bd08053e95c

Child:

8:9f4c10787775

--- a/main.cpp	Tue May 03 08:05:30 2016 +0000
+++ b/main.cpp	Wed Jun 01 12:26:41 2016 +0000
@@ -11,12 +11,20 @@
 LSM9DS0 sensor(SPI_MODE, D9, D6);
 Serial pc(SERIAL_TX, SERIAL_RX);
 Serial blueTooth(D10, D2);
+Serial LeftSerial(PC_12, PD_2);
+Serial RightSerial(PC_10, PC_11);
+
+DigitalOut debugLed_l(A4);
+DigitalOut debugLed_r(A5);
 
 Ticker WIPVTimer;
 void WIPVTimerInterrupt();
 float saturation(float input, float limit_H, float limit_L);
 void SensorAcquisition(float * data, float samplingTime);
-void SerialRx();
+
+void bluetoothRx();
+void RXIrqLeft();
+void RXIrqRight();
 
 //MOTOR L == MOTOR 1;  MOTOR R = MOTOR 2
 CURRENT_CONTROL motorCur_L(PC_3, D8,  A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
@@ -59,11 +67,21 @@
 float velocityCmd[2] = {0.0, 0.0};
 unsigned int accelerateFlag = 0;
 
+
+uint8_t cLast_l = 0x00, cLast_r = 0x00;
+bool headerCaptured_l = 0, headerCaptured_r = 0;
+uint8_t fromPhoton_l[2] = {0,0}, fromPhoton_r[2] = {0,0};
+float slipAcceleration_L = 0.1, slipAcceleration_R = 0.1, slipAcceleration_L_f, slipAcceleration_R_f;
+int i_l, i_r;
+
 int main()
 {
 
-    pc.baud(250000);
+    pc.baud(230400);
     blueTooth.baud(230400);
+    LeftSerial.baud(230400); //uart commu with photon left
+    RightSerial.baud(230400); //uart commu with photon right
+
     if( sensor.begin() != 0 ) {
         pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
     } else {
@@ -76,15 +94,23 @@
     motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);
 
     WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
-    blueTooth.attach(&SerialRx, Serial::RxIrq);
+    blueTooth.attach(&bluetoothRx, Serial::RxIrq);
+    LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
+    RightSerial.attach(&RXIrqRight, Serial::RxIrq);
+
     while(true) {
         //pc.printf("%5.4f\t", 10*pitch_angle);
         //pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
         //pc.printf("%5.3f\r\n", 10*curCmd_L);
 
 
-        pc.printf("%5.3f\t",   10*yawRate);
-        pc.printf("%5.3f\r\n", velocityCmd[1]);
+        pc.printf("%5.4f\t",   slipAcceleration_L_f);
+        pc.printf("%5.4f\r\n", slipAcceleration_R_f);
+
+//        pc.putc(fromPhoton_l[0]);
+//        pc.putc(fromPhoton_l[1]);
+//        pc.putc(fromPhoton_r[0]);
+//        pc.putc(fromPhoton_r[1]);
 
     }
 }
@@ -155,17 +181,19 @@
         ref[2] = velocityCmd[0];
         ref[3] = velocityCmd[1];
 
-        yawRate = sensorFilter4.filter(data_array[4],10);
+        yawRate = sensorFilter4.filter(data_array[4],20);
 
 
         torque_L =  (KL[0]*(state[0] - ref[0])+KL[1]*(state[1] - ref[1])+KL[2]*(state[2] - ref[2])+KL[3]*(state[3]-ref[3]));
         torque_R = -(KR[0]*(state[0] - ref[0])+KR[1]*(state[1] - ref[1])+KR[2]*(state[2] - ref[2])+KR[3]*(state[3]-ref[3]));
+        //=========================Anti slip control=============================//
+        slipAcceleration_L_f = (1 - 10*2*3.141593*Ts)*slipAcceleration_L_f + 10*2*3.141593*Ts*slipAcceleration_L;
+        slipAcceleration_R_f = (1 - 10*2*3.141593*Ts)*slipAcceleration_R_f + 10*2*3.141593*Ts*slipAcceleration_R;
 
-        motorCur_L.Control(saturation(torque_L/Km_L + 0.008*yawRate,1.2,-1.2), state[2]);
-        motorCur_R.Control(saturation(torque_R/Km_R + 0.008*yawRate,1.2,-1.2), -state[3]);
+        //motorCur_L.Control(saturation(torque_L/Km_L + 0.008*yawRate,1.2,-1.2), state[2]);
+//        motorCur_R.Control(saturation(torque_R/Km_R + 0.008*yawRate,1.2,-1.2), -state[3]);
 
-        //motorCur_L.SetPWMDuty(0.68);
-        //motorCur_R.SetPWMDuty(0.5 - 0.15);
+
         /*  //PD Balancing Control
                 balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
                 curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
@@ -187,7 +215,7 @@
 }
 
 
-void SerialRx()
+void  bluetoothRx()
 {
     while(blueTooth.readable()) {
         char charRx = blueTooth.getc();
@@ -217,7 +245,7 @@
                 velocityCmd[1] = 0.0;
                 accelerateFlag = 0;
                 break;
-                
+
             case 'q'://accelerate
                 accelerateFlag = 1;
                 break;
@@ -225,6 +253,65 @@
     }
 }
 
+void RXIrqLeft()
+{
+    //  slipAcceleration_L += 0.1;
+    while(!LeftSerial.readable());
+    uint8_t c = LeftSerial.getc();
+    if(!headerCaptured_l & cLast_l == 0xAA & c == 0x55) {
+        headerCaptured_l = 1;
+        i_l = 0;
+        debugLed_l = 1;
+    } else if(headerCaptured_l) {
+        if(c == 0xAA | c == 0x55 ) {
+            headerCaptured_l = 0;
+            i_l = 0;
+            debugLed_l = 0;
+        } 
+        else {
+            fromPhoton_l[i_l] = c;
+            i_l++;
+            if(i_l == 2) {
+                headerCaptured_l = 0;
+                debugLed_l = 0;
+                slipAcceleration_L = -(float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1]))/100.0;
+                
+            }
+        }
+    }
+    cLast_l = c;
+
+}
+
+void RXIrqRight()
+{
+    //  slipAcceleration_L += 0.1;
+    while(!RightSerial.readable());
+    uint8_t c = RightSerial.getc();
+    if(!headerCaptured_r & cLast_r == 0xAA & c == 0x55) {
+        headerCaptured_r = 1;
+        i_r = 0;
+        debugLed_r = 1;
+    } else if(headerCaptured_r) {
+        if(c == 0xAA | c == 0x55 ) {
+            headerCaptured_r = 0;
+            i_r = 0;
+            debugLed_r = 0;
+        } 
+        else {
+            fromPhoton_r[i_r] = c;
+            i_r++;
+            if(i_r == 2) {
+                headerCaptured_r = 0;
+                debugLed_r = 0;
+                slipAcceleration_R = (float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1]))/100.0;
+            }
+        }
+    }
+    cLast_r = c;
+
+}
+
 
 
 float saturation(float input, float limit_H, float limit_L)