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@5:842372be775c, 2016-04-28 (annotated)

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