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
main.cpp
- Committer:
- adam_z
- Date:
- 2016-07-28
- Revision:
- 9:a91135551be1
- Parent:
- 8:9f4c10787775
- Child:
- 10:2dc43cd59ff0
File content as of revision 9:a91135551be1:
#include "mbed.h"
#include "PID.h"
#include "LSM9DS0.h"
#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
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);
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 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);
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);
LPF sensorFilter1(Ts);
LPF sensorFilter2(Ts);
LPF sensorFilter3(Ts);
LPF sensorFilter4(Ts);
MedianFilter slipA_L_MF(9);
MedianFilter slipA_R_MF(9);
int tim_counter = 0;
float tim = 0.0;
float amp = 0.3;
float omega = 6.0;
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] = {-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};
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.9483, -0.1317, 0.0006, -0.0112}, Ks1[4] = {-1.9483, -0.1317, -0.0112, 0.0006};
float yc[2] = {0.0,0.0};
float Km_L = 1.050*0.163;
float Km_R = 1.187*0.137;
float yawRate = 0.0;
float velocityCmd[2] = {0.0, 0.0};
unsigned int accelerateFlag = 0, decelerationFlag = 0;
//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 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(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 {
pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
}
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);
WIPVTimer.attach_us(WIPVTimerInterrupt, Ts*1000.0*1000.0);
blueTooth.attach(&bluetoothRx, Serial::RxIrq);
LeftSerial.attach(&RXIrqLeft, 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);
//if(sent_flag == 0) {///
// pc.putc(data_sent[send_i]);
// send_i++;
// if(send_i >= 18) {
// send_i = 0;
// 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]);
}
}
void WIPVTimerInterrupt()
{
if(tim_counter <100)tim_counter++;
else if (tim_counter >= 100 && tim_counter <=109) {
motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
tim_counter++;
if(tim_counter == 110) {
motorCur_L.currentOffset = motorCur_L.currentOffset/10;
motorCur_R.currentOffset = motorCur_R.currentOffset/10;
}
} 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();
//
// 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
data_array[0] = sensor.readFloatAccelX();
data_array[1] = sensor.readFloatAccelY();
// 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);
//===============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, 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]>=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],9);
//===============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=============================//
//***********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;
//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_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;
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]);
//**********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_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_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_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_d+ B31_3[1]*slipAcceleration_R_f_d;
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,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++;
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_R)*multiplier)) >>8;
data_sent[17] = ((int16_t)((slipAcceleration_R)*multiplier)) & 0x00FF;
sent_flag = 0;
}
/* //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);
//======================current control=========================//
tim += Ts;
if(tim >= 4*pi/omega)tim = 0.0;
//curCmd_R = amp*sin(omega*tim); //current command
//curCmd_L = 0.8;
motorCur_R.SetPWMDuty(0.75);
motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
*/
}
}
void bluetoothRx()
{
while(pc.readable()) {
char charRx = pc.getc();
switch(charRx) {
case 'w'://forward
velocityCmd[0] = 2.5;
velocityCmd[1] = 2.5;
accelerateFlag = 0;
break;
case 's'://backward
velocityCmd[0] = -3.0;
velocityCmd[1] = -3.0;
accelerateFlag = 0;
break;
case 'a'://turn left
velocityCmd[0] = -4.0;
velocityCmd[1] = 4.0;
accelerateFlag = 0;
break;
case 'd'://turn right
velocityCmd[0] = 4.0;
velocityCmd[1] = -4.0;
accelerateFlag = 0;
break;
case 'x'://stop
velocityCmd[0] = 0.0;
velocityCmd[1] = 0.0;
accelerateFlag = 0;
break;
case 'q'://accelerate
// debugLed_l = !debugLed_l;
accelerateFlag = 1;
break;
}
}
}
void RXIrqLeft()
{
if(LeftSerial.readable()) {
debugLed_l = !debugLed_l;
uint8_t c = LeftSerial.getc();
cLast_l = c;
if( c == 0xAA) {
i_l = 0;
headerCaptured_l = 1;
} else if(headerCaptured_l) {
fromPhoton_l[i_l] = c;
i_l++;
if(i_l == 2) {
headerCaptured_l = 0;
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;
}
}
}
}
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);
// }
//
//}
void SCEnableFunc()
{
// debugLed_l = !debugLed_l;
accelerateFlag = 1;
SCEnalbeIndicator = !SCEnalbeIndicator;
}
