Developers_of_anti_slip_compensator
/
WIPV
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: CURRENT_CONTROL IIR LSM9DS0 MEDIAN_FILTER PID QEI RF24 SENSOR_FUSION mbed
Diff: main.cpp
- Revision:
- 8:9f4c10787775
- Parent:
- 7:f33a935eb77a
- Child:
- 9:a91135551be1
--- 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;
}
-
