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@4:a2d38818c4e7, 2016-04-28 (annotated)

Committer:
adam_z
Date:
Thu Apr 28 09:09:29 2016 +0000
Revision:
4:a2d38818c4e7
Parent:
2:53a942d1b1e5
Child:
5:842372be775c
PD balancing control, DONE!

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 0:150bb4605023 13
adam_z 0:150bb4605023 14 Ticker WIPVTimer;
adam_z 0:150bb4605023 15 void WIPVTimerInterrupt();
adam_z 4:a2d38818c4e7 16 float saturation(float input, float limit_H, float limit_L);
adam_z 0:150bb4605023 17
adam_z 4:a2d38818c4e7 18 CURRENT_CONTROL motorCur_L(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
adam_z 4:a2d38818c4e7 19 CURRENT_CONTROL motorCur_R(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
adam_z 1:ddc39900f9b8 20
adam_z 4:a2d38818c4e7 21 QEI wheel_L(A1, A2, NC, 280, 50, Ts, QEI::X4_ENCODING);
adam_z 4:a2d38818c4e7 22 QEI wheel_R(D13, D12, NC, 280, 50, Ts, QEI::X4_ENCODING);
adam_z 2:53a942d1b1e5 23
adam_z 4:a2d38818c4e7 24 PID balancingPD(20,0.00,0.0,Ts);
adam_z 4:a2d38818c4e7 25 LPF sensorFilter(Ts);
adam_z 2:53a942d1b1e5 26
adam_z 2:53a942d1b1e5 27
adam_z 4:a2d38818c4e7 28
adam_z 4:a2d38818c4e7 29
adam_z 4:a2d38818c4e7 30 void SensorAcquisition(float * data, float samplingTime);
adam_z 4:a2d38818c4e7 31
adam_z 4:a2d38818c4e7 32 int tim_counter = 0;
adam_z 2:53a942d1b1e5 33 float tim = 0.0;
adam_z 4:a2d38818c4e7 34 float amp = 0.3;
adam_z 4:a2d38818c4e7 35 float omega = 6.0;
adam_z 4:a2d38818c4e7 36 float curCmd_L =0.0, curCmd_R =0.0;
adam_z 2:53a942d1b1e5 37
adam_z 0:150bb4605023 38
adam_z 0:150bb4605023 39 int main()
adam_z 0:150bb4605023 40 {
adam_z 2:53a942d1b1e5 41
adam_z 4:a2d38818c4e7 42 pc.baud(250000);
adam_z 0:150bb4605023 43 if( sensor.begin() != 0 ) {
adam_z 0:150bb4605023 44 pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
adam_z 0:150bb4605023 45 } else {
adam_z 0:150bb4605023 46 pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
adam_z 0:150bb4605023 47 }
adam_z 0:150bb4605023 48 sensor.setGyroOffset(38,-24,-106);
adam_z 0:150bb4605023 49 sensor.setAccelOffset(-793,-511,300);
adam_z 2:53a942d1b1e5 50
adam_z 4:a2d38818c4e7 51 motorCur_L.SetParams(3.3*8/0.6, 1.187*0.137, 0.04348);
adam_z 4:a2d38818c4e7 52 motorCur_R.SetParams(3.3*8/0.6, 1.050*0.163, 0.04348);
adam_z 2:53a942d1b1e5 53
adam_z 0:150bb4605023 54 WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
adam_z 2:53a942d1b1e5 55 while(true) {
adam_z 4:a2d38818c4e7 56 //pc.printf("%5.4f\t", 10*pitch_angle);
adam_z 4:a2d38818c4e7 57 //pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
adam_z 4:a2d38818c4e7 58 //pc.printf("%5.3f\r\n", 10*curCmd_L);
adam_z 4:a2d38818c4e7 59
adam_z 4:a2d38818c4e7 60
adam_z 4:a2d38818c4e7 61 pc.printf("%5.3f\t", 100*curCmd_R);
adam_z 4:a2d38818c4e7 62 pc.printf("%5.3f\r\n", wheel_R.getAngularSpeed());
adam_z 4:a2d38818c4e7 63
adam_z 2:53a942d1b1e5 64 }
adam_z 0:150bb4605023 65 }
adam_z 0:150bb4605023 66
adam_z 0:150bb4605023 67
adam_z 0:150bb4605023 68 void WIPVTimerInterrupt()
adam_z 0:150bb4605023 69 {
adam_z 2:53a942d1b1e5 70 if(tim_counter <100)tim_counter++;
adam_z 2:53a942d1b1e5 71 else if (tim_counter >= 100 && tim_counter <=109) {
adam_z 4:a2d38818c4e7 72 motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
adam_z 4:a2d38818c4e7 73 motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
adam_z 2:53a942d1b1e5 74 tim_counter++;
adam_z 2:53a942d1b1e5 75 if(tim_counter == 110) {
adam_z 4:a2d38818c4e7 76 motorCur_L.currentOffset = motorCur_L.currentOffset/10;
adam_z 4:a2d38818c4e7 77 motorCur_R.currentOffset = motorCur_R.currentOffset/10;
adam_z 2:53a942d1b1e5 78 }
adam_z 2:53a942d1b1e5 79
adam_z 2:53a942d1b1e5 80 } else {
adam_z 2:53a942d1b1e5 81
adam_z 0:150bb4605023 82 /*
adam_z 0:150bb4605023 83 int16_t data_array[6];
adam_z 2:53a942d1b1e5 84
adam_z 0:150bb4605023 85 data_array[0] = sensor.readRawAccelX();
adam_z 0:150bb4605023 86 data_array[1] = sensor.readRawAccelY();
adam_z 0:150bb4605023 87 data_array[2] = sensor.readRawAccelZ();
adam_z 0:150bb4605023 88 data_array[3] = sensor.readRawGyroX();
adam_z 0:150bb4605023 89 data_array[4] = sensor.readRawGyroY();
adam_z 0:150bb4605023 90 data_array[5] = sensor.readRawGyroZ();
adam_z 2:53a942d1b1e5 91
adam_z 0:150bb4605023 92 pc.printf("%d, ", data_array[0]);
adam_z 0:150bb4605023 93 pc.printf("%d, ", data_array[1]);
adam_z 0:150bb4605023 94 pc.printf("%d, ", data_array[2]);
adam_z 0:150bb4605023 95 pc.printf("%d, ", data_array[3]);
adam_z 0:150bb4605023 96 pc.printf("%d, ", data_array[4]);
adam_z 0:150bb4605023 97 pc.printf("%d;\r\n ", data_array[5]);
adam_z 0:150bb4605023 98 */
adam_z 2:53a942d1b1e5 99
adam_z 2:53a942d1b1e5 100
adam_z 0:150bb4605023 101 float data_array[6];
adam_z 0:150bb4605023 102 data_array[0] = sensor.readFloatAccelX();
adam_z 0:150bb4605023 103 data_array[1] = sensor.readFloatAccelY();
adam_z 0:150bb4605023 104 data_array[2] = sensor.readFloatAccelZ();
adam_z 0:150bb4605023 105 data_array[3] = sensor.readFloatGyroX();
adam_z 0:150bb4605023 106 data_array[4] = sensor.readFloatGyroY();
adam_z 0:150bb4605023 107 data_array[5] = sensor.readFloatGyroZ();
adam_z 4:a2d38818c4e7 108 sensor.complementaryFilter(data_array,Ts);
adam_z 4:a2d38818c4e7 109 //SensorAcquisition(data_array, Ts);
adam_z 4:a2d38818c4e7 110
adam_z 2:53a942d1b1e5 111 //*****wheel speed calculation*****//
adam_z 2:53a942d1b1e5 112 wheel_L.Calculate();
adam_z 2:53a942d1b1e5 113 wheel_R.Calculate();
adam_z 2:53a942d1b1e5 114 //*************obtain current values***********//
adam_z 2:53a942d1b1e5 115 //pc.printf("%f\t", current_L_Offset);
adam_z 2:53a942d1b1e5 116 //pc.printf("%f\r\n", current_R_Offset);
adam_z 4:a2d38818c4e7 117
adam_z 4:a2d38818c4e7 118 balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
adam_z 4:a2d38818c4e7 119 curCmd_L = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
adam_z 2:53a942d1b1e5 120 //*************current control********//
adam_z 2:53a942d1b1e5 121 tim += Ts;
adam_z 2:53a942d1b1e5 122 if(tim >= 4*pi/omega)tim = 0.0;
adam_z 4:a2d38818c4e7 123 //curCmd_R = amp*sin(omega*tim); //current command
adam_z 4:a2d38818c4e7 124 //curCmd_L = 0.8;
adam_z 0:150bb4605023 125
adam_z 4:a2d38818c4e7 126 //motorCur_R.SetPWMDuty(0.75);
adam_z 4:a2d38818c4e7 127 motorCur_L.Control(curCmd_L + 0.002*data_array[4], wheel_L.getAngularSpeed());
adam_z 4:a2d38818c4e7 128 motorCur_R.Control(-curCmd_L + 0.002*data_array[4], wheel_R.getAngularSpeed());
adam_z 0:150bb4605023 129
adam_z 4:a2d38818c4e7 130
adam_z 2:53a942d1b1e5 131 }
adam_z 2:53a942d1b1e5 132
adam_z 2:53a942d1b1e5 133
adam_z 4:a2d38818c4e7 134 }
adam_z 4:a2d38818c4e7 135
adam_z 4:a2d38818c4e7 136
adam_z 4:a2d38818c4e7 137 float saturation(float input, float limit_H, float limit_L)
adam_z 4:a2d38818c4e7 138 {
adam_z 4:a2d38818c4e7 139 float output;
adam_z 4:a2d38818c4e7 140 if(input >= limit_H)output = limit_H;
adam_z 4:a2d38818c4e7 141 else if (input <= limit_L)output = limit_L;
adam_z 4:a2d38818c4e7 142 else output = input;
adam_z 4:a2d38818c4e7 143 return output;
adam_z 4:a2d38818c4e7 144 }
adam_z 4:a2d38818c4e7 145
adam_z 4:a2d38818c4e7 146
adam_z 4:a2d38818c4e7 147 void SensorAcquisition(float * data, float samplingTime)
adam_z 4:a2d38818c4e7 148 {
adam_z 4:a2d38818c4e7 149
adam_z 4:a2d38818c4e7 150 axm = data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
adam_z 4:a2d38818c4e7 151 aym = data[1]*(-1)*9.81;
adam_z 4:a2d38818c4e7 152 azm = data[2]*(-1)*9.81;
adam_z 4:a2d38818c4e7 153 u1 = data[0]*3.14159/180; //gyroscope :deg/s to rad/s
adam_z 4:a2d38818c4e7 154 u2 = data[1]*3.14159/180;
adam_z 4:a2d38818c4e7 155 u3 = data[2]*3.14159/180;
adam_z 4:a2d38818c4e7 156
adam_z 4:a2d38818c4e7 157
adam_z 4:a2d38818c4e7 158 if(conv_init <= 3)
adam_z 4:a2d38818c4e7 159 {
adam_z 4:a2d38818c4e7 160 axm_f_old = axm;
adam_z 4:a2d38818c4e7 161 aym_f_old = aym;
adam_z 4:a2d38818c4e7 162 azm_f_old = azm;
adam_z 4:a2d38818c4e7 163
adam_z 4:a2d38818c4e7 164 conv_init++;
adam_z 4:a2d38818c4e7 165 }
adam_z 4:a2d38818c4e7 166 else
adam_z 4:a2d38818c4e7 167 {
adam_z 4:a2d38818c4e7 168 pitch_fusion(axm, aym,azm,u3,u2,20, samplingTime);
adam_z 4:a2d38818c4e7 169 roll_fusion(axm, aym,azm,u3,u1,20, samplingTime);
adam_z 4:a2d38818c4e7 170 x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
adam_z 4:a2d38818c4e7 171 }
adam_z 4:a2d38818c4e7 172
adam_z 4:a2d38818c4e7 173 }
adam_z 4:a2d38818c4e7 174