Diff: main.cpp

Revision:

4:a2d38818c4e7

Parent:

2:53a942d1b1e5

Child:

5:842372be775c

--- a/main.cpp	Fri Apr 22 14:32:21 2016 +0000
+++ b/main.cpp	Thu Apr 28 09:09:29 2016 +0000
@@ -2,6 +2,8 @@
 #include "PID.h"
 #include "LSM9DS0.h"
 #include "QEI.h"
+#include "CURRENT_CONTROL.h"
+#include "SENSOR_FUSION.h"
 
 #define Ts 0.001
 #define pi 3.14159
@@ -11,56 +13,54 @@
 
 Ticker WIPVTimer;
 void WIPVTimerInterrupt();
+float saturation(float input, float limit_H, float limit_L);
 
-PwmOut M1C1(D7);
-PwmOut M1C2(D11);
-PwmOut M2C1(D8);
-PwmOut M2C2(A3);
+CURRENT_CONTROL motorCur_L(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
+CURRENT_CONTROL motorCur_R(PC_3, D8,  A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
 
-QEI wheel_L(A1, A2, NC, 280, 50, 0.001, QEI::X4_ENCODING);
-QEI wheel_R(D13, D12, NC, 280, 50, 0.001, QEI::X4_ENCODING);
+QEI wheel_L(A1,  A2,  NC, 280, 50, Ts, QEI::X4_ENCODING);
+QEI wheel_R(D13, D12, NC, 280, 50, Ts, QEI::X4_ENCODING);
 
-AnalogIn current_L(PC_2);
-AnalogIn current_R(PC_3);
-
-PID curL_PID(0.6,2,0.0,0.001);
+PID balancingPD(20,0.00,0.0,Ts);
+LPF sensorFilter(Ts);
 
 
-int tim_counter;
-float current_L_Offset;
-float current_R_Offset;
+
+
+void SensorAcquisition(float * data, float samplingTime);
+
+int tim_counter = 0;
 float tim = 0.0;
-float amp = 0.4;
-float omega = 2.0;
-float curCmd_L;
+float amp = 0.3;
+float omega = 6.0;
+float curCmd_L =0.0, curCmd_R =0.0;
 
 
 int main()
 {
-    //float abias[3], gbias[3];
 
-    pc.baud(115200);
+    pc.baud(250000);
     if( sensor.begin() != 0 ) {
         pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
     } else {
         pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
     }
-    //sensor.calLSM9DS0(gbias, abias);
     sensor.setGyroOffset(38,-24,-106);
     sensor.setAccelOffset(-793,-511,300);
 
-    M1C1.period_us(50);
-    M1C1.write(0.5);
-    M2C1.period_us(50);
-    M2C1.write(0.5);
-    TIM1->CCER |= 4; //enable ch1 complimentary output
-    TIM1->CCER |= 64;//enable ch1 complimentary output
-
-
+    motorCur_L.SetParams(3.3*8/0.6, 1.187*0.137, 0.04348);
+    motorCur_R.SetParams(3.3*8/0.6, 1.050*0.163, 0.04348);
 
     WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
     while(true) {
-        //pc.printf("%5f\t%5f\r\n", speed1.getAngularSpeed(), speed2.getAngularSpeed());
+        //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", 100*curCmd_R);
+        pc.printf("%5.3f\r\n", wheel_R.getAngularSpeed());
+        
     }
 }
 
@@ -69,12 +69,12 @@
 {
     if(tim_counter <100)tim_counter++;
     else if (tim_counter >= 100 && tim_counter <=109) {
-        current_L_Offset += current_L.read();
-        current_R_Offset += current_R.read();
+        motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
+        motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
         tim_counter++;
         if(tim_counter == 110) {
-            current_L_Offset = current_L_Offset/10;
-            current_R_Offset = current_R_Offset/10;
+            motorCur_L.currentOffset = motorCur_L.currentOffset/10;
+            motorCur_R.currentOffset = motorCur_R.currentOffset/10;
         }
 
     } else {
@@ -105,39 +105,70 @@
         data_array[3]  = sensor.readFloatGyroX();
         data_array[4]  = sensor.readFloatGyroY();
         data_array[5]  = sensor.readFloatGyroZ();
-        sensor.complementaryFilter(data_array, Ts);
-        /*
-                pc.printf("%5.4f, ", sensor.pitch);
-                pc.printf("%5.4f   ", sensor.roll);
-                pc.printf("%5.4f, ", data_array[0]);
-                pc.printf("%5.4f, ", data_array[1]);
-                pc.printf("%5.4f    ", data_array[2]);
-                pc.printf("%5.4f, ", data_array[3]);
-                pc.printf("%5.4f, ", data_array[4]);
-                pc.printf("%5.4f;\r\n", data_array[5]);
-         */
+        sensor.complementaryFilter(data_array,Ts);
+        //SensorAcquisition(data_array, Ts);
+        
         //*****wheel speed calculation*****//
         wheel_L.Calculate();
         wheel_R.Calculate();
         //*************obtain current values***********//
         //pc.printf("%f\t", current_L_Offset);
         //pc.printf("%f\r\n", current_R_Offset);
+        
+        balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
+        curCmd_L = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
         //*************current control********//
         tim += Ts;
         if(tim >= 4*pi/omega)tim = 0.0;
-        curCmd_L = amp*sin(omega*tim); //current command
-        
-        pc.printf("%5.4f\t", 10*curCmd_L);
-        float cur_L = (current_L.read() - current_L_Offset)*3.3*8/0.6; //8A when the voltage is 0.6 (assuming linear)
-        pc.printf("%5.4f\t", 10*cur_L);
+        //curCmd_R = amp*sin(omega*tim); //current command
+        //curCmd_L = 0.8;
         
-        curL_PID.Compute(curCmd_L, cur_L);
-        M1C1 = 0.5 - curL_PID.output;
-        pc.printf("%5.4f\r\n", curL_PID.output);
-        TIM1->CCER |= 4;
+        //motorCur_R.SetPWMDuty(0.75);
+        motorCur_L.Control(curCmd_L  + 0.002*data_array[4], wheel_L.getAngularSpeed());
+        motorCur_R.Control(-curCmd_L + 0.002*data_array[4], wheel_R.getAngularSpeed());
         
-
+        
     }
 
 
-}
\ No newline at end of file
+}
+
+
+float saturation(float input, float limit_H, float limit_L)
+{
+    float output;
+    if(input >= limit_H)output = limit_H;
+    else if (input <= limit_L)output = limit_L;
+    else output = input;
+    return output;
+}
+
+
+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);   
+        }      
+       
+}
+