Revision 8:9f4c10787775, committed 2016-06-18

Comitter:

adam_z

Date:

Sat Jun 18 09:49:17 2016 +0000

Parent:

7:f33a935eb77a

Child:

9:a91135551be1

Commit message:

photon to stm has some problem; s

Changed in this revision

--- a/LSM9DS0.lib	Wed Jun 01 12:26:41 2016 +0000
+++ b/LSM9DS0.lib	Sat Jun 18 09:49:17 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#bc0db184f092
+http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#90f024c6b406

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MEDIAN_FILTER.lib	Sat Jun 18 09:49:17 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/MEDIAN_FILTER/#a46f56a22432

--- a/main.cpp	Wed Jun 01 12:26:41 2016 +0000
+++ b/main.cpp	Sat Jun 18 09:49:17 2016 +0000
@@ -4,6 +4,8 @@
 #include "QEI.h"
 #include "CURRENT_CONTROL.h"
 #include "SENSOR_FUSION.h"
+#include "MEDIAN_FILTER.h"
+#include <algorithm>
 
 #define Ts 0.001
 #define pi 3.14159
@@ -17,15 +19,21 @@
 DigitalOut debugLed_l(A4);
 DigitalOut debugLed_r(A5);
 
+InterruptIn SCEnable(USER_BUTTON);
+
+
 Ticker WIPVTimer;
 void WIPVTimerInterrupt();
 float saturation(float input, float limit_H, float limit_L);
-void SensorAcquisition(float * data, float samplingTime);
+//void SensorAcquisition(float * data, float samplingTime);
 
 void bluetoothRx();
 void RXIrqLeft();
 void RXIrqRight();
 
+void SCEnableFunc();
+bool SCEnalbeIndicator;
+
 //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);
 CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
@@ -40,6 +48,9 @@
 LPF sensorFilter3(Ts);
 LPF sensorFilter4(Ts);
 
+MedianFilter slipA_L_MF(9);
+MedianFilter slipA_R_MF(9);
+
 
 
 
@@ -51,13 +62,26 @@
 float curCmd_L =0.0, curCmd_R =0.0;
 
 
-
+//*************real plant states and matrices*****************************
 float state[4] = {0.0, 0.0, 0.0, 0.0};
 float ref[4] = {0.0, 0.0, 0.0, 0.0};
 
 float torque_L = 0.0, torque_R = 0.0;
-float KL[4] = {-0.7057 ,  -0.0733 ,  -0.0085 ,  -0.0300};
-float KR[4] = {-0.7057 ,  -0.0733 ,  -0.0300 ,  -0.0085};
+float KL[4] = {-0.9159 ,  -0.1093  , -0.0146  , -0.0369};//{-0.7057 ,  -0.0733 ,  -0.0085 ,  -0.0300};
+float KR[4] = {-0.9159 ,  -0.1093  , -0.0369  , -0.0146};//{-0.7057 ,  -0.0733 ,  -0.0300 ,  -0.0085};
+
+//********simulated states and multiplying matrices***********************
+float z1_prime[4] = {0.0, 0.0, 0.0, 0.0};
+float z1_prime_dot[4] = {0.0, 0.0, 0.0, 0.0};
+float v1[2] = {0.0,0.0};
+float A1_prime0[4] = {0,1,0,0},A1_prime1[4] = {176.4899,0,0,0},A1_prime2[4] = {-83.1468,0, 0, 0},A1_prime3[4] = {-83.1468, 0, 0,0};
+float B11_prime0[2] = {0,0}, B11_prime1[2] = {-537.6123, -537.6123}, B11_prime2[2] = { 679.2587,  104.3936}, B11_prime3[2] = { 104.3936,  679.2587};
+float B31_0[2] = {0,0},B31_1[2] = {0.7840,0.7840},B31_2[2] = {12.1836, 0.3086},B31_3[2] = {0.3086, 12.1836};
+
+float Ks0[4] = {-1.4973,   -0.1057 ,   0.0042  , -0.0096}, Ks1[4] = {-1.4973  , -0.1057  , -0.0096   , 0.0042};
+float yc[2] = {0.0,0.0};
+
+
 
 float Km_L = 1.050*0.163;
 float Km_R = 1.187*0.137;
@@ -71,15 +95,24 @@
 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;
+float slipAcceleration_L = 0.0, slipAcceleration_R = 0.0, slipAcceleration_L_f, slipAcceleration_R_f, slipAcceleration_L_m, slipAcceleration_R_m;
 int i_l, i_r;
 
+
+float a_vehicle;
+float slipA_L_p, slipA_R_p;
+
+int send_i = 0, send_counter = 0;
+bool sent_flag = 1;
+uint8_t data_sent[18];
+
+
 int main()
 {
 
     pc.baud(230400);
     blueTooth.baud(230400);
-    LeftSerial.baud(230400); //uart commu with photon left
+    LeftSerial.baud(115200); //uart commu with photon left
     RightSerial.baud(230400); //uart commu with photon right
 
     if( sensor.begin() != 0 ) {
@@ -93,24 +126,52 @@
     motorCur_L.SetParams(3.3*8/0.6, Km_L, 0.04348);
     motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);
 
-    WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
+    WIPVTimer.attach_us(WIPVTimerInterrupt, Ts*1000.0*1000.0);
     blueTooth.attach(&bluetoothRx, Serial::RxIrq);
     LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
-    RightSerial.attach(&RXIrqRight, Serial::RxIrq);
+    //RightSerial.attach(&RXIrqRight, Serial::RxIrq);
+
+    SCEnable.rise(&SCEnableFunc);
+    SCEnalbeIndicator = 0;
 
     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\n", 10*sensor.pitch*3.14159/180);
+//        pc.printf("%5.3f\r\n", 10*curCmd_L);
+
+
+//        pc.printf("%5.4f\t",   slipAcceleration_L_f);
+//        pc.printf("%5.4f\r\n", slipAcceleration_R_f);
 
 
-        pc.printf("%5.4f\t",   slipAcceleration_L_f);
-        pc.printf("%5.4f\r\n", slipAcceleration_R_f);
+//        blueTooth.putc(0xFE);
+        //blueTooth.putc(48);
+//        blueTooth.putc(48);
+//        blueTooth.putc(48);
+//        blueTooth.putc(48);
+//        blueTooth.putc(48);
+//        blueTooth.putc(48);
+//        blueTooth.putc(48);
+//        blueTooth.putc(48);
 
 //        pc.putc(fromPhoton_l[0]);
 //        pc.putc(fromPhoton_l[1]);
 //        pc.putc(fromPhoton_r[0]);
 //        pc.putc(fromPhoton_r[1]);
+        
+
+        //if(sent_flag == 0) {///
+//            pc.putc(data_sent[send_i]);
+//            send_i++;
+//            if(send_i >= 18) {
+//                send_i = 0;
+//                sent_flag = 1;
+//            }
+//        }///
+
+
+
+
 
     }
 }
@@ -152,10 +213,11 @@
         float data_array[6];//Gs and deg/s
         data_array[0]  = sensor.readFloatAccelX();
         data_array[1]  = sensor.readFloatAccelY();
-        data_array[2]  = sensor.readFloatAccelZ();
-        data_array[3]  = sensor.readFloatGyroX();
+//        data_array[2]  = sensor.readFloatAccelZ();
+//        data_array[3]  = sensor.readFloatGyroX();
         data_array[4]  = sensor.readFloatGyroY();
         data_array[5]  = sensor.readFloatGyroZ();
+
         sensor.complementaryFilter(data_array,Ts);
         //SensorAcquisition(data_array, Ts);
 
@@ -166,7 +228,7 @@
         /////////////Balancing Control/////////////////////////
         //SI dimension
         state[0] = sensor.pitch*3.14159/180.0;
-        state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 10.0);
+        state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 9.0);
         state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),60.0);
         state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),60.0);
 
@@ -184,14 +246,105 @@
         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]));
+        //===============integration of z1_prime=========================
+        z1_prime[0] += z1_prime_dot[0]*Ts;
+        z1_prime[1] += z1_prime_dot[1]*Ts;
+        z1_prime[2] += z1_prime_dot[2]*Ts;
+        z1_prime[3] += z1_prime_dot[3]*Ts;
+
+
+        yc[0] = (KL[0]*(ref[0] - (state[0] - z1_prime[0])) + KL[1]*(ref[1] - (state[1] - z1_prime[1]))\
+                 + KL[2]*(ref[2] - (state[2] - z1_prime[2])) + KL[3]*(ref[3] - (state[3] - z1_prime[3])));
+
+        yc[1] = (KR[0]*(ref[0] - (state[0] - z1_prime[0])) + KR[1]*(ref[1] - (state[1] - z1_prime[1]))\
+                 + KR[2]*(ref[2] - (state[2] - z1_prime[2])) + KR[3]*(ref[3] - (state[3] - z1_prime[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;
+        //***********filtering the slip acceleration of two wheels********************
+        a_vehicle = (data_array[0]*cos(state[0]) - data_array[1]*sin(state[0]))*9.791;//compute acceleration of vehicle using lsm9ds0
+
+        slipA_L_p =  slipAcceleration_L_m;
+        slipA_R_p =  slipAcceleration_R_m;
+
+        if(SCEnalbeIndicator == 0) {
+            slipAcceleration_L_f = 0;
+            slipAcceleration_R_f = 0;
+
+        } else {
+
+            slipAcceleration_L_f = (1 - 30*2*3.141593*Ts)*slipAcceleration_L_f + 30*2*3.141593*Ts*slipA_L_p;
+            slipAcceleration_R_f = (1 - 30*2*3.141593*Ts)*slipAcceleration_R_f + 30*2*3.141593*Ts*slipA_R_p;
+
+        }
+
+        //***************simulated plant feedback**************
+        v1[0] = -(Ks0[0]*z1_prime[0] + Ks0[1]*z1_prime[1] + Ks0[2]*z1_prime[2] + Ks0[3]*z1_prime[3]);
+        v1[1] = -(Ks1[0]*z1_prime[0] + Ks1[1]*z1_prime[1] + Ks1[2]*z1_prime[2] + Ks1[3]*z1_prime[3]);
+
+        //**********simulated dynamics*****************
+        z1_prime_dot[0] =  A1_prime0[0]*z1_prime[0] + A1_prime0[1]*z1_prime[1]+A1_prime0[2]*z1_prime[2]+A1_prime0[3]*z1_prime[3]\
+                           + B11_prime0[0]*v1[0]+ B11_prime0[1]*v1[1]\
+                           + B31_0[0]*slipAcceleration_L_f+ B31_0[1]*slipAcceleration_R_f;
+
+        z1_prime_dot[1] =  A1_prime1[0]*z1_prime[0] + A1_prime1[1]*z1_prime[1]+A1_prime1[2]*z1_prime[2]+A1_prime1[3]*z1_prime[3]\
+                           + B11_prime1[0]*v1[0]+ B11_prime1[1]*v1[1]\
+                           + B31_1[0]*slipAcceleration_L_f+ B31_1[1]*slipAcceleration_R_f;
+
+        z1_prime_dot[2] =  A1_prime2[0]*z1_prime[0] + A1_prime2[1]*z1_prime[1]+A1_prime2[2]*z1_prime[2]+A1_prime2[3]*z1_prime[3]\
+                           + B11_prime2[0]*v1[0]+ B11_prime2[1]*v1[1]\
+                           + B31_2[0]*slipAcceleration_L_f+ B31_2[1]*slipAcceleration_R_f;
+
+        z1_prime_dot[3] =  A1_prime3[0]*z1_prime[0] + A1_prime3[1]*z1_prime[1]+A1_prime3[2]*z1_prime[2]+A1_prime3[3]*z1_prime[3]\
+                           + B11_prime3[0]*v1[0]+ B11_prime3[1]*v1[1]\
+                           + B31_3[0]*slipAcceleration_L_f+ B31_3[1]*slipAcceleration_R_f;
+
+        torque_L = yc[0] + v1[0];
+        torque_R = yc[1] + v1[1];
 
-        //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]);
+        //************motor torque control*****************
+//        motorCur_L.Control(saturation((-torque_L)/Km_L + 0.008*yawRate,1.3,-1.3), state[2]);
+//        motorCur_R.Control(saturation((torque_R)/Km_R + 0.008*yawRate,1.3,-1.3), -state[3]);
+
+        //**************sending data to PC******************
+        if(send_counter < 40)send_counter++;
+
+        else if( sent_flag == 1 & send_counter >= 40) {
+            send_counter = 0;
+            int multiplier = 500;
+            data_sent[0] = 0xAA;
+            data_sent[1] = 0x55;
+
+            data_sent[2] = ((int16_t)((state[0])*multiplier)) >>8;
+            data_sent[3] = ((int16_t)((state[0])*multiplier)) & 0x00FF;
+            data_sent[4] = ((int16_t)(state[1]*multiplier)) >>8;
+            data_sent[5] = ((int16_t)(state[1]*multiplier)) & 0x00FF;
+
+            data_sent[6] = ((int16_t)((v1[0])*multiplier)) >>8;
+            data_sent[7] = ((int16_t)((v1[0])*multiplier)) & 0x00FF;
+            data_sent[8] = ((int16_t)((v1[1])*multiplier)) >>8;
+            data_sent[9] = ((int16_t)((v1[1])*multiplier)) & 0x00FF;
+
+            data_sent[10] = ((int16_t)((yc[0])*multiplier)) >>8;
+            data_sent[11] = ((int16_t)((yc[0])*multiplier)) & 0x00FF;
+            data_sent[12] = ((int16_t)((yc[1])*multiplier)) >>8;
+            data_sent[13] = ((int16_t)((yc[1])*multiplier)) & 0x00FF;
+            //data_sent[10] = ((int16_t)((-a_vehicle)*multiplier)) >>8;
+//            data_sent[11] = ((int16_t)((-a_vehicle)*multiplier)) & 0x00FF;
+//            data_sent[12] = ((int16_t)((slipAcceleration_R_m)*multiplier)) >>8;
+//            data_sent[13] = ((int16_t)((slipAcceleration_R_m)*multiplier)) & 0x00FF;
+            data_sent[14] = ((int16_t)((slipAcceleration_L)*multiplier)) >>8;
+            data_sent[15] = ((int16_t)((slipAcceleration_L)*multiplier)) & 0x00FF;
+            data_sent[16] = ((int16_t)((slipAcceleration_L_f)*multiplier)) >>8;
+            data_sent[17] = ((int16_t)((slipAcceleration_L_f)*multiplier)) & 0x00FF;
+
+
+            sent_flag = 0;
+
+        }
+
+
+
 
 
         /*  //PD Balancing Control
@@ -217,8 +370,8 @@
 
 void  bluetoothRx()
 {
-    while(blueTooth.readable()) {
-        char charRx = blueTooth.getc();
+    while(pc.readable()) {
+        char charRx = pc.getc();
         switch(charRx) {
             case 'w'://forward
                 velocityCmd[0] = 2.5;
@@ -247,6 +400,7 @@
                 break;
 
             case 'q'://accelerate
+//                debugLed_l = !debugLed_l;
                 accelerateFlag = 1;
                 break;
         }
@@ -267,19 +421,23 @@
             headerCaptured_l = 0;
             i_l = 0;
             debugLed_l = 0;
-        } 
-        else {
+        } 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;
+                slipAcceleration_L = (float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1]))/1000.0;
                 
+                if((abs(slipAcceleration_L) < 15.0 ))if((abs(slipAcceleration_L - slipAcceleration_L_m)< 0.4))\
+                slipAcceleration_L_m = slipAcceleration_L;
+                
+
             }
         }
     }
     cLast_l = c;
+    pc.printf("%5.4f\r\n", (float)cLast_l);
 
 }
 
@@ -297,14 +455,16 @@
             headerCaptured_r = 0;
             i_r = 0;
             debugLed_r = 0;
-        } 
-        else {
+        } 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;
+                slipAcceleration_R = -(float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1]))/1000.0;
+                
+                if((abs(slipAcceleration_R) < 15.0 ))if((abs(slipAcceleration_R - slipAcceleration_R_m)< 0.4))\
+                slipAcceleration_R_m = slipAcceleration_R;
             }
         }
     }
@@ -324,28 +484,35 @@
 }
 
 
-void SensorAcquisition(float * data, float samplingTime)
-{
-
-    axm =  data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
-    aym =  data[1]*(-1)*9.81;
-    azm =  data[2]*(-1)*9.81;
-    u1  = data[0]*3.14159/180;      //gyroscope :deg/s to rad/s
-    u2  = data[1]*3.14159/180;
-    u3  = data[2]*3.14159/180;
+//void SensorAcquisition(float * data, float samplingTime)
+//{
+//
+//    axm =  data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
+//    aym =  data[1]*(-1)*9.81;
+//    azm =  data[2]*(-1)*9.81;
+//    u1  = data[0]*3.14159/180;      //gyroscope :deg/s to rad/s
+//    u2  = data[1]*3.14159/180;
+//    u3  = data[2]*3.14159/180;
+//
+//
+//    if(conv_init <= 3) {
+//        axm_f_old = axm;
+//        aym_f_old = aym;
+//        azm_f_old = azm;
+//
+//        conv_init++;
+//    } else {
+//        pitch_fusion(axm,  aym,azm,u3,u2,20, samplingTime);
+//        roll_fusion(axm,   aym,azm,u3,u1,20, samplingTime);
+//        x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
+//    }
+//
+//}
 
 
-    if(conv_init <= 3) {
-        axm_f_old = axm;
-        aym_f_old = aym;
-        azm_f_old = azm;
+void SCEnableFunc()
+{
+    debugLed_l = !debugLed_l;
 
-        conv_init++;
-    } else {
-        pitch_fusion(axm,  aym,azm,u3,u2,20, samplingTime);
-        roll_fusion(axm,   aym,azm,u3,u1,20, samplingTime);
-        x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
-    }
-
+    SCEnalbeIndicator = !SCEnalbeIndicator;
 }
-