Revision 9:a91135551be1, committed 2016-07-28

Comitter:

adam_z

Date:

Thu Jul 28 08:17:30 2016 +0000

Parent:

8:9f4c10787775

Child:

10:2dc43cd59ff0

Commit message:

ready to combine RF24L01 to this board;

Changed in this revision

--- a/LSM9DS0.lib	Sat Jun 18 09:49:17 2016 +0000
+++ b/LSM9DS0.lib	Thu Jul 28 08:17:30 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#90f024c6b406
+http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#56ff956c499e

--- a/main.cpp	Sat Jun 18 09:49:17 2016 +0000
+++ b/main.cpp	Thu Jul 28 08:17:30 2016 +0000
@@ -38,8 +38,8 @@
 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);
 
-QEI wheel_L(D13, D12, NC, 280, 200, Ts, QEI::X4_ENCODING);
-QEI wheel_R(A1,  A2,  NC, 280, 200, Ts, QEI::X4_ENCODING);
+QEI wheel_L(D13, D12, NC, 280, 250, Ts, QEI::X4_ENCODING);
+QEI wheel_R(A1,  A2,  NC, 280, 250, Ts, QEI::X4_ENCODING);
 
 
 PID balancingPD(20,0.00,0.0,Ts);
@@ -67,8 +67,8 @@
 float ref[4] = {0.0, 0.0, 0.0, 0.0};
 
 float torque_L = 0.0, torque_R = 0.0;
-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};
+float KL[4] = {-1.9483 ,  -0.1317 ,   0.0006*2  , -0.0112*2};//{-0.7057 ,  -0.0733 ,  -0.0085 ,  -0.0300};
+float KR[4] = {-1.9483,   -0.1317 ,  -0.0112*2 ,   0.0006*2};//{-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};
@@ -78,7 +78,7 @@
 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 Ks0[4] = {-1.9483,   -0.1317,    0.0006,   -0.0112}, Ks1[4] = {-1.9483,   -0.1317,   -0.0112,    0.0006};
 float yc[2] = {0.0,0.0};
 
 
@@ -89,30 +89,37 @@
 float yawRate = 0.0;
 
 float velocityCmd[2] = {0.0, 0.0};
-unsigned int accelerateFlag = 0;
+unsigned int accelerateFlag = 0, decelerationFlag = 0;
 
 
-uint8_t cLast_l = 0x00, cLast_r = 0x00;
+//uint8_t cLast_l = 0x00, cLast_r = 0x00;
+float cLast_l, cLast_r;
 bool headerCaptured_l = 0, headerCaptured_r = 0;
 uint8_t fromPhoton_l[2] = {0,0}, fromPhoton_r[2] = {0,0};
 float slipAcceleration_L = 0.0, slipAcceleration_R = 0.0, slipAcceleration_L_f, slipAcceleration_R_f, slipAcceleration_L_m, slipAcceleration_R_m;
+float slipAcceleration_L_f_d, slipAcceleration_R_f_d;
 int i_l, i_r;
 
 
 float a_vehicle;
-float slipA_L_p, slipA_R_p;
+float a_slip_L,a_slip_R;
 
 int send_i = 0, send_counter = 0;
 bool sent_flag = 1;
 uint8_t data_sent[18];
 
+int delay_num = 37;
+float fifo_array[37];
+float a_vehicle_fifo;
+int fifo_i = 0;
+
 
 int main()
 {
 
     pc.baud(230400);
     blueTooth.baud(230400);
-    LeftSerial.baud(115200); //uart commu with photon left
+    LeftSerial.baud(230400); //uart commu with photon left
     RightSerial.baud(230400); //uart commu with photon right
 
     if( sensor.begin() != 0 ) {
@@ -120,8 +127,8 @@
     } else {
         pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
     }
-    sensor.setGyroOffset(38,-24,-106);
-    sensor.setAccelOffset(-793,-511,300);
+    sensor.setGyroOffset(38,-20,-95);
+    sensor.setAccelOffset(-680,-460,300);
 
     motorCur_L.SetParams(3.3*8/0.6, Km_L, 0.04348);
     motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);
@@ -129,7 +136,7 @@
     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;
@@ -140,26 +147,6 @@
 //        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);
-
-
-//        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++;
@@ -168,10 +155,12 @@
 //                sent_flag = 1;
 //            }
 //        }///
-
-
-
-
+    pc.printf("%5.4f\t",a_vehicle);
+        pc.printf("%5.4f\t",a_vehicle_fifo);
+//        pc.printf("%5.4f\t", slipAcceleration_L);
+        pc.printf("%5.4f\r\n",slipAcceleration_L);
+//    pc.printf("%5.4f\t",yc[0]);
+//    pc.printf("%5.4f\r\n",yc[1]);
 
     }
 }
@@ -191,23 +180,23 @@
 
     } else {
 
-        /*
-        int16_t data_array[6];
 
-        data_array[0]  = sensor.readRawAccelX();
-        data_array[1]  = sensor.readRawAccelY();
-        data_array[2]  = sensor.readRawAccelZ();
-        data_array[3]  = sensor.readRawGyroX();
-        data_array[4]  = sensor.readRawGyroY();
-        data_array[5]  = sensor.readRawGyroZ();
+        //int16_t data_array[6];
+//
+//        data_array[0]  = sensor.readRawAccelX();
+//        data_array[1]  = sensor.readRawAccelY();
+////        data_array[2]  = sensor.readRawAccelZ();
+////        data_array[3]  = sensor.readRawGyroX();
+//        data_array[4]  = sensor.readRawGyroY();
+//        data_array[5]  = sensor.readRawGyroZ();
+//
+//        pc.printf("%d, ", data_array[0]);
+//        pc.printf("%d, ", data_array[1]);
+////        pc.printf("%d, ", data_array[2]);
+////        pc.printf("%d, ", data_array[3]);
+//        pc.printf("%d, ", data_array[4]);
+//        pc.printf("%d;\r\n ", data_array[5]);
 
-        pc.printf("%d, ", data_array[0]);
-        pc.printf("%d, ", data_array[1]);
-        pc.printf("%d, ", data_array[2]);
-        pc.printf("%d, ", data_array[3]);
-        pc.printf("%d, ", data_array[4]);
-        pc.printf("%d;\r\n ", data_array[5]);
-        */
 
 
         float data_array[6];//Gs and deg/s
@@ -219,31 +208,40 @@
         data_array[5]  = sensor.readFloatGyroZ();
 
         sensor.complementaryFilter(data_array,Ts);
-        //SensorAcquisition(data_array, Ts);
+
 
         //===============wheel speed calculation============//
         wheel_L.Calculate();
         wheel_R.Calculate();
 
+
         /////////////Balancing Control/////////////////////////
         //SI dimension
         state[0] = sensor.pitch*3.14159/180.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);
+        state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 6.0);
+        state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),80.0);
+        state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),80.0);
 
         if(accelerateFlag == 1) {
-            if(velocityCmd[0]>=7 || velocityCmd[1]>=7)accelerateFlag = 0;
-            else {
-                velocityCmd[0] += 0.004;
-                velocityCmd[1] += 0.004;
+            if(velocityCmd[0]>=20 || velocityCmd[1]>=20) {
+                accelerateFlag = 0;
+                decelerationFlag = 1;
+            } else {
+                velocityCmd[0] += 0.010;
+                velocityCmd[1] += 0.010;
             }
         }
 
+        if(decelerationFlag == 1 & velocityCmd[0] > 0.0) {
+            velocityCmd[0] -= 0.015;
+            velocityCmd[1] -= 0.015;
+            if(velocityCmd[0] <= 0.0)decelerationFlag = 0;
+        }
+
         ref[2] = velocityCmd[0];
         ref[3] = velocityCmd[1];
 
-        yawRate = sensorFilter4.filter(data_array[4],20);
+        yawRate = sensorFilter4.filter(data_array[4],9);
 
 
         //===============integration of z1_prime=========================
@@ -263,21 +261,43 @@
         //=========================Anti slip control=============================//
         //***********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
+        //fifo delay
+        a_vehicle_fifo = fifo_array[fifo_i];
+//        delay_num = sizeof(fifo_array);
+        fifo_array[fifo_i] = a_vehicle;
+        fifo_i++;
+        if(fifo_i >= delay_num)fifo_i = 0;
 
-        slipA_L_p =  slipAcceleration_L_m;
-        slipA_R_p =  slipAcceleration_R_m;
+
+
+
+        //pc.printf("%5.2f\t", a_vehicle);
+////        pc.printf("%5.2f\t",slipAcceleration_L_f);
+//        pc.printf("%5.2f\r\n",slipAcceleration_R_f);
+
+        a_slip_L = slipAcceleration_L - a_vehicle_fifo;
+        a_slip_R = slipAcceleration_R - a_vehicle_fifo;
+
+        float alpha = 40.0;//HZ
+        slipAcceleration_L_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_L_f + alpha*2*3.141593*Ts*a_slip_L;
+        slipAcceleration_R_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_R_f + alpha*2*3.141593*Ts*a_slip_R;
 
         if(SCEnalbeIndicator == 0) {
-            slipAcceleration_L_f = 0;
-            slipAcceleration_R_f = 0;
+            slipAcceleration_L_f_d = 0;
+            slipAcceleration_R_f_d = 0;
 
         } else {
+            //***********dead zone************************//
+            float deadzone = 0.0;
+            if(slipAcceleration_L_f < deadzone & slipAcceleration_L_f > -deadzone)slipAcceleration_L_f_d = 0;
+            else slipAcceleration_L_f_d = slipAcceleration_L_f;
 
-            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;
+            if(slipAcceleration_R_f < deadzone & slipAcceleration_R_f > -deadzone)slipAcceleration_R_f_d = 0;
+            else slipAcceleration_R_f_d = slipAcceleration_R_f;
 
         }
 
+
         //***************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]);
@@ -285,26 +305,26 @@
         //**********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;
+                           + B31_0[0]*slipAcceleration_L_f_d+ B31_0[1]*slipAcceleration_R_f_d;
 
         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;
+                           + B31_1[0]*slipAcceleration_L_f_d+ B31_1[1]*slipAcceleration_R_f_d;
 
         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;
+                           + B31_2[0]*slipAcceleration_L_f_d+ B31_2[1]*slipAcceleration_R_f_d;
 
         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;
+                           + B31_3[0]*slipAcceleration_L_f_d+ B31_3[1]*slipAcceleration_R_f_d;
 
-        torque_L = yc[0] + v1[0];
-        torque_R = yc[1] + v1[1];
+        torque_L = -((yc[0] + v1[0])/Km_L) + 0.015*yawRate;
+        torque_R =  ((yc[1] + v1[1])/Km_R) + 0.015*yawRate;
 
         //************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]);
+//        motorCur_L.Control(saturation(torque_L,1.3,-1.3), state[2]);
+//        motorCur_R.Control(saturation(torque_R,1.3,-1.3), -state[3]);
 
         //**************sending data to PC******************
         if(send_counter < 40)send_counter++;
@@ -335,8 +355,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;
+            data_sent[16] = ((int16_t)((slipAcceleration_R)*multiplier)) >>8;
+            data_sent[17] = ((int16_t)((slipAcceleration_R)*multiplier)) & 0x00FF;
 
 
             sent_flag = 0;
@@ -409,67 +429,63 @@
 
 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;
+
+
+    if(LeftSerial.readable()) {
+        debugLed_l = !debugLed_l;
+        uint8_t c = LeftSerial.getc();
+        cLast_l = c;
+
+        if( c == 0xAA) {
             i_l = 0;
-            debugLed_l = 0;
-        } else {
+            headerCaptured_l = 1;
+
+
+        } else if(headerCaptured_l)  {
             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]))/1000.0;
-                
-                if((abs(slipAcceleration_L) < 15.0 ))if((abs(slipAcceleration_L - slipAcceleration_L_m)< 0.4))\
-                slipAcceleration_L_m = slipAcceleration_L;
-                
+                slipAcceleration_L = (float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1])*0.92)/1000.0;
+
+//                pc.printf("%5.4f\t",(float)slipAcceleration_L_f);
+//                pc.printf("%5.4f\r\n",(float)slipAcceleration_R_f);
+            }
+
+        }
+    }
+
+
+}
+
+
+
+
+void RXIrqRight()
+{
+
+    //  slipAcceleration_L += 0.1;
+    if(RightSerial.readable()) {
+        debugLed_r = !debugLed_r;
+        uint8_t c = RightSerial.getc();
+        cLast_r = c;
+//        pc.printf("%5.4f\r\n",(float)cLast_r);
+
+        if( c == 0xAA) {
+            headerCaptured_r = 1;
+            i_r = 0;
+
+        } else if(headerCaptured_r) {
+
+            fromPhoton_r[i_r] = c;
+            i_r++;
+            if(i_r == 2) {
+                headerCaptured_r = 0;
+                slipAcceleration_R = -(float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1])*1.18)/1000.0;
 
             }
         }
     }
-    cLast_l = c;
-    pc.printf("%5.4f\r\n", (float)cLast_l);
-
-}
-
-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]))/1000.0;
-                
-                if((abs(slipAcceleration_R) < 15.0 ))if((abs(slipAcceleration_R - slipAcceleration_R_m)< 0.4))\
-                slipAcceleration_R_m = slipAcceleration_R;
-            }
-        }
-    }
-    cLast_r = c;
-
 }
 
 
@@ -512,7 +528,7 @@
 
 void SCEnableFunc()
 {
-    debugLed_l = !debugLed_l;
-
+//    debugLed_l = !debugLed_l;
+    accelerateFlag = 1;
     SCEnalbeIndicator = !SCEnalbeIndicator;
 }