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:
- 6:5bd08053e95c
- Parent:
- 5:842372be775c
- Child:
- 7:f33a935eb77a
--- a/main.cpp Thu Apr 28 09:39:39 2016 +0000
+++ b/main.cpp Tue May 03 08:05:30 2016 +0000
@@ -10,23 +10,27 @@
LSM9DS0 sensor(SPI_MODE, D9, D6);
Serial pc(SERIAL_TX, SERIAL_RX);
+Serial blueTooth(D10, D2);
Ticker WIPVTimer;
void WIPVTimerInterrupt();
float saturation(float input, float limit_H, float limit_L);
void SensorAcquisition(float * data, float samplingTime);
+void SerialRx();
//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, 50, Ts, QEI::X4_ENCODING);
-QEI wheel_R(A1, A2, NC, 280, 50, Ts, QEI::X4_ENCODING);
+QEI wheel_L(D13, D12, NC, 280, 200, Ts, QEI::X4_ENCODING);
+QEI wheel_R(A1, A2, NC, 280, 200, Ts, QEI::X4_ENCODING);
PID balancingPD(20,0.00,0.0,Ts);
-LPF sensorFilter(Ts);
-
+LPF sensorFilter1(Ts);
+LPF sensorFilter2(Ts);
+LPF sensorFilter3(Ts);
+LPF sensorFilter4(Ts);
@@ -39,10 +43,27 @@
float curCmd_L =0.0, curCmd_R =0.0;
+
+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 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;
+
int main()
{
pc.baud(250000);
+ blueTooth.baud(230400);
if( sensor.begin() != 0 ) {
pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
} else {
@@ -51,19 +72,20 @@
sensor.setGyroOffset(38,-24,-106);
sensor.setAccelOffset(-793,-511,300);
- motorCur_L.SetParams(3.3*8/0.6, 1.050*0.163, 0.04348);
- motorCur_R.SetParams(3.3*8/0.6, 1.187*0.137, 0.04348);
+ 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);
+ blueTooth.attach(&SerialRx, Serial::RxIrq);
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\t", 100*curCmd_R);
- pc.printf("%5.3f\r\n", wheel_R.getAngularSpeed());
-
+
+
+ pc.printf("%5.3f\t", 10*yawRate);
+ pc.printf("%5.3f\r\n", velocityCmd[1]);
+
}
}
@@ -110,34 +132,101 @@
data_array[5] = sensor.readFloatGyroZ();
sensor.complementaryFilter(data_array,Ts);
//SensorAcquisition(data_array, Ts);
-
- //*****wheel speed calculation*****//
+
+ //===============wheel speed calculation============//
wheel_L.Calculate();
wheel_R.Calculate();
-
-
-
-
- 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());
-
-
+
+ /////////////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[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),60.0);
+ state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),60.0);
+
+ if(accelerateFlag == 1) {
+ if(velocityCmd[0]>=7 || velocityCmd[1]>=7)accelerateFlag = 0;
+ else {
+ velocityCmd[0] += 0.004;
+ velocityCmd[1] += 0.004;
+ }
+ }
+
+ ref[2] = velocityCmd[0];
+ ref[3] = velocityCmd[1];
+
+ yawRate = sensorFilter4.filter(data_array[4],10);
+
+
+ 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]));
+
+ 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]);
+
+ //motorCur_L.SetPWMDuty(0.68);
+ //motorCur_R.SetPWMDuty(0.5 - 0.15);
+ /* //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 SerialRx()
+{
+ while(blueTooth.readable()) {
+ char charRx = blueTooth.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
+ accelerateFlag = 1;
+ break;
+ }
+ }
+}
+
+
+
float saturation(float input, float limit_H, float limit_L)
{
float output;
@@ -149,30 +238,27 @@
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 = 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);
- }
-
+ } 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);
+ }
+
}
