Developers_of_anti_slip_compensator / Mbed 2 deprecated 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@6:5bd08053e95c, 2016-05-03 (annotated)

Committer:
adam_z
Date:
Tue May 03 08:05:30 2016 +0000
Revision:
6:5bd08053e95c
Parent:
5:842372be775c
Child:
7:f33a935eb77a
PD and angular velocity feedback control with blue tooth command

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adam_z 0:150bb4605023 1 #include "mbed.h"
adam_z 0:150bb4605023 2 #include "PID.h"
adam_z 0:150bb4605023 3 #include "LSM9DS0.h"
adam_z 0:150bb4605023 4 #include "QEI.h"
adam_z 4:a2d38818c4e7 5 #include "CURRENT_CONTROL.h"
adam_z 4:a2d38818c4e7 6 #include "SENSOR_FUSION.h"
adam_z 0:150bb4605023 7
adam_z 0:150bb4605023 8 #define Ts 0.001
adam_z 2:53a942d1b1e5 9 #define pi 3.14159
adam_z 0:150bb4605023 10
adam_z 0:150bb4605023 11 LSM9DS0 sensor(SPI_MODE, D9, D6);
adam_z 0:150bb4605023 12 Serial pc(SERIAL_TX, SERIAL_RX);
adam_z 6:5bd08053e95c 13 Serial blueTooth(D10, D2);
adam_z 0:150bb4605023 14
adam_z 0:150bb4605023 15 Ticker WIPVTimer;
adam_z 0:150bb4605023 16 void WIPVTimerInterrupt();
adam_z 4:a2d38818c4e7 17 float saturation(float input, float limit_H, float limit_L);
adam_z 5:842372be775c 18 void SensorAcquisition(float * data, float samplingTime);
adam_z 6:5bd08053e95c 19 void SerialRx();
adam_z 0:150bb4605023 20
adam_z 5:842372be775c 21 //MOTOR L == MOTOR 1; MOTOR R = MOTOR 2
adam_z 5:842372be775c 22 CURRENT_CONTROL motorCur_L(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
adam_z 5:842372be775c 23 CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
adam_z 1:ddc39900f9b8 24
adam_z 6:5bd08053e95c 25 QEI wheel_L(D13, D12, NC, 280, 200, Ts, QEI::X4_ENCODING);
adam_z 6:5bd08053e95c 26 QEI wheel_R(A1, A2, NC, 280, 200, Ts, QEI::X4_ENCODING);
adam_z 5:842372be775c 27
adam_z 2:53a942d1b1e5 28
adam_z 4:a2d38818c4e7 29 PID balancingPD(20,0.00,0.0,Ts);
adam_z 6:5bd08053e95c 30 LPF sensorFilter1(Ts);
adam_z 6:5bd08053e95c 31 LPF sensorFilter2(Ts);
adam_z 6:5bd08053e95c 32 LPF sensorFilter3(Ts);
adam_z 6:5bd08053e95c 33 LPF sensorFilter4(Ts);
adam_z 2:53a942d1b1e5 34
adam_z 4:a2d38818c4e7 35
adam_z 4:a2d38818c4e7 36
adam_z 5:842372be775c 37
adam_z 4:a2d38818c4e7 38
adam_z 4:a2d38818c4e7 39 int tim_counter = 0;
adam_z 2:53a942d1b1e5 40 float tim = 0.0;
adam_z 4:a2d38818c4e7 41 float amp = 0.3;
adam_z 4:a2d38818c4e7 42 float omega = 6.0;
adam_z 4:a2d38818c4e7 43 float curCmd_L =0.0, curCmd_R =0.0;
adam_z 2:53a942d1b1e5 44
adam_z 0:150bb4605023 45
adam_z 6:5bd08053e95c 46
adam_z 6:5bd08053e95c 47 float state[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 6:5bd08053e95c 48 float ref[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 6:5bd08053e95c 49
adam_z 6:5bd08053e95c 50 float torque_L = 0.0, torque_R = 0.0;
adam_z 6:5bd08053e95c 51 float KL[4] = {-0.7057 , -0.0733 , -0.0085 , -0.0300};
adam_z 6:5bd08053e95c 52 float KR[4] = {-0.7057 , -0.0733 , -0.0300 , -0.0085};
adam_z 6:5bd08053e95c 53
adam_z 6:5bd08053e95c 54 float Km_L = 1.050*0.163;
adam_z 6:5bd08053e95c 55 float Km_R = 1.187*0.137;
adam_z 6:5bd08053e95c 56
adam_z 6:5bd08053e95c 57 float yawRate = 0.0;
adam_z 6:5bd08053e95c 58
adam_z 6:5bd08053e95c 59 float velocityCmd[2] = {0.0, 0.0};
adam_z 6:5bd08053e95c 60 unsigned int accelerateFlag = 0;
adam_z 6:5bd08053e95c 61
adam_z 0:150bb4605023 62 int main()
adam_z 0:150bb4605023 63 {
adam_z 2:53a942d1b1e5 64
adam_z 4:a2d38818c4e7 65 pc.baud(250000);
adam_z 6:5bd08053e95c 66 blueTooth.baud(230400);
adam_z 0:150bb4605023 67 if( sensor.begin() != 0 ) {
adam_z 0:150bb4605023 68 pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
adam_z 0:150bb4605023 69 } else {
adam_z 0:150bb4605023 70 pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
adam_z 0:150bb4605023 71 }
adam_z 0:150bb4605023 72 sensor.setGyroOffset(38,-24,-106);
adam_z 0:150bb4605023 73 sensor.setAccelOffset(-793,-511,300);
adam_z 2:53a942d1b1e5 74
adam_z 6:5bd08053e95c 75 motorCur_L.SetParams(3.3*8/0.6, Km_L, 0.04348);
adam_z 6:5bd08053e95c 76 motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);
adam_z 2:53a942d1b1e5 77
adam_z 0:150bb4605023 78 WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
adam_z 6:5bd08053e95c 79 blueTooth.attach(&SerialRx, Serial::RxIrq);
adam_z 2:53a942d1b1e5 80 while(true) {
adam_z 4:a2d38818c4e7 81 //pc.printf("%5.4f\t", 10*pitch_angle);
adam_z 4:a2d38818c4e7 82 //pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
adam_z 4:a2d38818c4e7 83 //pc.printf("%5.3f\r\n", 10*curCmd_L);
adam_z 6:5bd08053e95c 84
adam_z 6:5bd08053e95c 85
adam_z 6:5bd08053e95c 86 pc.printf("%5.3f\t", 10*yawRate);
adam_z 6:5bd08053e95c 87 pc.printf("%5.3f\r\n", velocityCmd[1]);
adam_z 6:5bd08053e95c 88
adam_z 2:53a942d1b1e5 89 }
adam_z 0:150bb4605023 90 }
adam_z 0:150bb4605023 91
adam_z 0:150bb4605023 92
adam_z 0:150bb4605023 93 void WIPVTimerInterrupt()
adam_z 0:150bb4605023 94 {
adam_z 2:53a942d1b1e5 95 if(tim_counter <100)tim_counter++;
adam_z 2:53a942d1b1e5 96 else if (tim_counter >= 100 && tim_counter <=109) {
adam_z 4:a2d38818c4e7 97 motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
adam_z 4:a2d38818c4e7 98 motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
adam_z 2:53a942d1b1e5 99 tim_counter++;
adam_z 2:53a942d1b1e5 100 if(tim_counter == 110) {
adam_z 4:a2d38818c4e7 101 motorCur_L.currentOffset = motorCur_L.currentOffset/10;
adam_z 4:a2d38818c4e7 102 motorCur_R.currentOffset = motorCur_R.currentOffset/10;
adam_z 2:53a942d1b1e5 103 }
adam_z 2:53a942d1b1e5 104
adam_z 2:53a942d1b1e5 105 } else {
adam_z 2:53a942d1b1e5 106
adam_z 0:150bb4605023 107 /*
adam_z 0:150bb4605023 108 int16_t data_array[6];
adam_z 2:53a942d1b1e5 109
adam_z 0:150bb4605023 110 data_array[0] = sensor.readRawAccelX();
adam_z 0:150bb4605023 111 data_array[1] = sensor.readRawAccelY();
adam_z 0:150bb4605023 112 data_array[2] = sensor.readRawAccelZ();
adam_z 0:150bb4605023 113 data_array[3] = sensor.readRawGyroX();
adam_z 0:150bb4605023 114 data_array[4] = sensor.readRawGyroY();
adam_z 0:150bb4605023 115 data_array[5] = sensor.readRawGyroZ();
adam_z 2:53a942d1b1e5 116
adam_z 0:150bb4605023 117 pc.printf("%d, ", data_array[0]);
adam_z 0:150bb4605023 118 pc.printf("%d, ", data_array[1]);
adam_z 0:150bb4605023 119 pc.printf("%d, ", data_array[2]);
adam_z 0:150bb4605023 120 pc.printf("%d, ", data_array[3]);
adam_z 0:150bb4605023 121 pc.printf("%d, ", data_array[4]);
adam_z 0:150bb4605023 122 pc.printf("%d;\r\n ", data_array[5]);
adam_z 0:150bb4605023 123 */
adam_z 2:53a942d1b1e5 124
adam_z 2:53a942d1b1e5 125
adam_z 5:842372be775c 126 float data_array[6];//Gs and deg/s
adam_z 0:150bb4605023 127 data_array[0] = sensor.readFloatAccelX();
adam_z 0:150bb4605023 128 data_array[1] = sensor.readFloatAccelY();
adam_z 0:150bb4605023 129 data_array[2] = sensor.readFloatAccelZ();
adam_z 0:150bb4605023 130 data_array[3] = sensor.readFloatGyroX();
adam_z 0:150bb4605023 131 data_array[4] = sensor.readFloatGyroY();
adam_z 0:150bb4605023 132 data_array[5] = sensor.readFloatGyroZ();
adam_z 4:a2d38818c4e7 133 sensor.complementaryFilter(data_array,Ts);
adam_z 4:a2d38818c4e7 134 //SensorAcquisition(data_array, Ts);
adam_z 6:5bd08053e95c 135
adam_z 6:5bd08053e95c 136 //===============wheel speed calculation============//
adam_z 2:53a942d1b1e5 137 wheel_L.Calculate();
adam_z 2:53a942d1b1e5 138 wheel_R.Calculate();
adam_z 6:5bd08053e95c 139
adam_z 6:5bd08053e95c 140 /////////////Balancing Control/////////////////////////
adam_z 6:5bd08053e95c 141 //SI dimension
adam_z 6:5bd08053e95c 142 state[0] = sensor.pitch*3.14159/180.0;
adam_z 6:5bd08053e95c 143 state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 10.0);
adam_z 6:5bd08053e95c 144 state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),60.0);
adam_z 6:5bd08053e95c 145 state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),60.0);
adam_z 6:5bd08053e95c 146
adam_z 6:5bd08053e95c 147 if(accelerateFlag == 1) {
adam_z 6:5bd08053e95c 148 if(velocityCmd[0]>=7 || velocityCmd[1]>=7)accelerateFlag = 0;
adam_z 6:5bd08053e95c 149 else {
adam_z 6:5bd08053e95c 150 velocityCmd[0] += 0.004;
adam_z 6:5bd08053e95c 151 velocityCmd[1] += 0.004;
adam_z 6:5bd08053e95c 152 }
adam_z 6:5bd08053e95c 153 }
adam_z 6:5bd08053e95c 154
adam_z 6:5bd08053e95c 155 ref[2] = velocityCmd[0];
adam_z 6:5bd08053e95c 156 ref[3] = velocityCmd[1];
adam_z 6:5bd08053e95c 157
adam_z 6:5bd08053e95c 158 yawRate = sensorFilter4.filter(data_array[4],10);
adam_z 6:5bd08053e95c 159
adam_z 6:5bd08053e95c 160
adam_z 6:5bd08053e95c 161 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]));
adam_z 6:5bd08053e95c 162 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]));
adam_z 6:5bd08053e95c 163
adam_z 6:5bd08053e95c 164 motorCur_L.Control(saturation(torque_L/Km_L + 0.008*yawRate,1.2,-1.2), state[2]);
adam_z 6:5bd08053e95c 165 motorCur_R.Control(saturation(torque_R/Km_R + 0.008*yawRate,1.2,-1.2), -state[3]);
adam_z 6:5bd08053e95c 166
adam_z 6:5bd08053e95c 167 //motorCur_L.SetPWMDuty(0.68);
adam_z 6:5bd08053e95c 168 //motorCur_R.SetPWMDuty(0.5 - 0.15);
adam_z 6:5bd08053e95c 169 /* //PD Balancing Control
adam_z 6:5bd08053e95c 170 balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
adam_z 6:5bd08053e95c 171 curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
adam_z 6:5bd08053e95c 172 //======================current control=========================//
adam_z 6:5bd08053e95c 173 tim += Ts;
adam_z 6:5bd08053e95c 174 if(tim >= 4*pi/omega)tim = 0.0;
adam_z 6:5bd08053e95c 175 //curCmd_R = amp*sin(omega*tim); //current command
adam_z 6:5bd08053e95c 176 //curCmd_L = 0.8;
adam_z 6:5bd08053e95c 177
adam_z 6:5bd08053e95c 178 motorCur_R.SetPWMDuty(0.75);
adam_z 6:5bd08053e95c 179
adam_z 6:5bd08053e95c 180 motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
adam_z 6:5bd08053e95c 181 motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
adam_z 6:5bd08053e95c 182 */
adam_z 6:5bd08053e95c 183
adam_z 2:53a942d1b1e5 184 }
adam_z 2:53a942d1b1e5 185
adam_z 2:53a942d1b1e5 186
adam_z 4:a2d38818c4e7 187 }
adam_z 4:a2d38818c4e7 188
adam_z 4:a2d38818c4e7 189
adam_z 6:5bd08053e95c 190 void SerialRx()
adam_z 6:5bd08053e95c 191 {
adam_z 6:5bd08053e95c 192 while(blueTooth.readable()) {
adam_z 6:5bd08053e95c 193 char charRx = blueTooth.getc();
adam_z 6:5bd08053e95c 194 switch(charRx) {
adam_z 6:5bd08053e95c 195 case 'w'://forward
adam_z 6:5bd08053e95c 196 velocityCmd[0] = 2.5;
adam_z 6:5bd08053e95c 197 velocityCmd[1] = 2.5;
adam_z 6:5bd08053e95c 198 accelerateFlag = 0;
adam_z 6:5bd08053e95c 199 break;
adam_z 6:5bd08053e95c 200 case 's'://backward
adam_z 6:5bd08053e95c 201 velocityCmd[0] = -3.0;
adam_z 6:5bd08053e95c 202 velocityCmd[1] = -3.0;
adam_z 6:5bd08053e95c 203 accelerateFlag = 0;
adam_z 6:5bd08053e95c 204 break;
adam_z 6:5bd08053e95c 205 case 'a'://turn left
adam_z 6:5bd08053e95c 206 velocityCmd[0] = -4.0;
adam_z 6:5bd08053e95c 207 velocityCmd[1] = 4.0;
adam_z 6:5bd08053e95c 208 accelerateFlag = 0;
adam_z 6:5bd08053e95c 209 break;
adam_z 6:5bd08053e95c 210 case 'd'://turn right
adam_z 6:5bd08053e95c 211 velocityCmd[0] = 4.0;
adam_z 6:5bd08053e95c 212 velocityCmd[1] = -4.0;
adam_z 6:5bd08053e95c 213 accelerateFlag = 0;
adam_z 6:5bd08053e95c 214 break;
adam_z 6:5bd08053e95c 215 case 'x'://stop
adam_z 6:5bd08053e95c 216 velocityCmd[0] = 0.0;
adam_z 6:5bd08053e95c 217 velocityCmd[1] = 0.0;
adam_z 6:5bd08053e95c 218 accelerateFlag = 0;
adam_z 6:5bd08053e95c 219 break;
adam_z 6:5bd08053e95c 220
adam_z 6:5bd08053e95c 221 case 'q'://accelerate
adam_z 6:5bd08053e95c 222 accelerateFlag = 1;
adam_z 6:5bd08053e95c 223 break;
adam_z 6:5bd08053e95c 224 }
adam_z 6:5bd08053e95c 225 }
adam_z 6:5bd08053e95c 226 }
adam_z 6:5bd08053e95c 227
adam_z 6:5bd08053e95c 228
adam_z 6:5bd08053e95c 229
adam_z 4:a2d38818c4e7 230 float saturation(float input, float limit_H, float limit_L)
adam_z 4:a2d38818c4e7 231 {
adam_z 4:a2d38818c4e7 232 float output;
adam_z 4:a2d38818c4e7 233 if(input >= limit_H)output = limit_H;
adam_z 4:a2d38818c4e7 234 else if (input <= limit_L)output = limit_L;
adam_z 4:a2d38818c4e7 235 else output = input;
adam_z 4:a2d38818c4e7 236 return output;
adam_z 4:a2d38818c4e7 237 }
adam_z 4:a2d38818c4e7 238
adam_z 4:a2d38818c4e7 239
adam_z 4:a2d38818c4e7 240 void SensorAcquisition(float * data, float samplingTime)
adam_z 6:5bd08053e95c 241 {
adam_z 4:a2d38818c4e7 242
adam_z 6:5bd08053e95c 243 axm = data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
adam_z 6:5bd08053e95c 244 aym = data[1]*(-1)*9.81;
adam_z 6:5bd08053e95c 245 azm = data[2]*(-1)*9.81;
adam_z 6:5bd08053e95c 246 u1 = data[0]*3.14159/180; //gyroscope :deg/s to rad/s
adam_z 6:5bd08053e95c 247 u2 = data[1]*3.14159/180;
adam_z 6:5bd08053e95c 248 u3 = data[2]*3.14159/180;
adam_z 6:5bd08053e95c 249
adam_z 6:5bd08053e95c 250
adam_z 6:5bd08053e95c 251 if(conv_init <= 3) {
adam_z 4:a2d38818c4e7 252 axm_f_old = axm;
adam_z 4:a2d38818c4e7 253 aym_f_old = aym;
adam_z 4:a2d38818c4e7 254 azm_f_old = azm;
adam_z 6:5bd08053e95c 255
adam_z 4:a2d38818c4e7 256 conv_init++;
adam_z 6:5bd08053e95c 257 } else {
adam_z 6:5bd08053e95c 258 pitch_fusion(axm, aym,azm,u3,u2,20, samplingTime);
adam_z 6:5bd08053e95c 259 roll_fusion(axm, aym,azm,u3,u1,20, samplingTime);
adam_z 6:5bd08053e95c 260 x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
adam_z 6:5bd08053e95c 261 }
adam_z 6:5bd08053e95c 262
adam_z 4:a2d38818c4e7 263 }
adam_z 4:a2d38818c4e7 264