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@7:f33a935eb77a, 2016-06-01 (annotated)

Committer:
adam_z
Date:
Wed Jun 01 12:26:41 2016 +0000
Revision:
7:f33a935eb77a
Parent:
6:5bd08053e95c
Child:
8:9f4c10787775
Receiving message  from photons via bluetooth conducted! ; direction of slip acceleration...checked!;

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 7:f33a935eb77a 14 Serial LeftSerial(PC_12, PD_2);
adam_z 7:f33a935eb77a 15 Serial RightSerial(PC_10, PC_11);
adam_z 7:f33a935eb77a 16
adam_z 7:f33a935eb77a 17 DigitalOut debugLed_l(A4);
adam_z 7:f33a935eb77a 18 DigitalOut debugLed_r(A5);
adam_z 0:150bb4605023 19
adam_z 0:150bb4605023 20 Ticker WIPVTimer;
adam_z 0:150bb4605023 21 void WIPVTimerInterrupt();
adam_z 4:a2d38818c4e7 22 float saturation(float input, float limit_H, float limit_L);
adam_z 5:842372be775c 23 void SensorAcquisition(float * data, float samplingTime);
adam_z 7:f33a935eb77a 24
adam_z 7:f33a935eb77a 25 void bluetoothRx();
adam_z 7:f33a935eb77a 26 void RXIrqLeft();
adam_z 7:f33a935eb77a 27 void RXIrqRight();
adam_z 0:150bb4605023 28
adam_z 5:842372be775c 29 //MOTOR L == MOTOR 1; MOTOR R = MOTOR 2
adam_z 5:842372be775c 30 CURRENT_CONTROL motorCur_L(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
adam_z 5:842372be775c 31 CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
adam_z 1:ddc39900f9b8 32
adam_z 6:5bd08053e95c 33 QEI wheel_L(D13, D12, NC, 280, 200, Ts, QEI::X4_ENCODING);
adam_z 6:5bd08053e95c 34 QEI wheel_R(A1, A2, NC, 280, 200, Ts, QEI::X4_ENCODING);
adam_z 5:842372be775c 35
adam_z 2:53a942d1b1e5 36
adam_z 4:a2d38818c4e7 37 PID balancingPD(20,0.00,0.0,Ts);
adam_z 6:5bd08053e95c 38 LPF sensorFilter1(Ts);
adam_z 6:5bd08053e95c 39 LPF sensorFilter2(Ts);
adam_z 6:5bd08053e95c 40 LPF sensorFilter3(Ts);
adam_z 6:5bd08053e95c 41 LPF sensorFilter4(Ts);
adam_z 2:53a942d1b1e5 42
adam_z 4:a2d38818c4e7 43
adam_z 4:a2d38818c4e7 44
adam_z 5:842372be775c 45
adam_z 4:a2d38818c4e7 46
adam_z 4:a2d38818c4e7 47 int tim_counter = 0;
adam_z 2:53a942d1b1e5 48 float tim = 0.0;
adam_z 4:a2d38818c4e7 49 float amp = 0.3;
adam_z 4:a2d38818c4e7 50 float omega = 6.0;
adam_z 4:a2d38818c4e7 51 float curCmd_L =0.0, curCmd_R =0.0;
adam_z 2:53a942d1b1e5 52
adam_z 0:150bb4605023 53
adam_z 6:5bd08053e95c 54
adam_z 6:5bd08053e95c 55 float state[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 6:5bd08053e95c 56 float ref[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 6:5bd08053e95c 57
adam_z 6:5bd08053e95c 58 float torque_L = 0.0, torque_R = 0.0;
adam_z 6:5bd08053e95c 59 float KL[4] = {-0.7057 , -0.0733 , -0.0085 , -0.0300};
adam_z 6:5bd08053e95c 60 float KR[4] = {-0.7057 , -0.0733 , -0.0300 , -0.0085};
adam_z 6:5bd08053e95c 61
adam_z 6:5bd08053e95c 62 float Km_L = 1.050*0.163;
adam_z 6:5bd08053e95c 63 float Km_R = 1.187*0.137;
adam_z 6:5bd08053e95c 64
adam_z 6:5bd08053e95c 65 float yawRate = 0.0;
adam_z 6:5bd08053e95c 66
adam_z 6:5bd08053e95c 67 float velocityCmd[2] = {0.0, 0.0};
adam_z 6:5bd08053e95c 68 unsigned int accelerateFlag = 0;
adam_z 6:5bd08053e95c 69
adam_z 7:f33a935eb77a 70
adam_z 7:f33a935eb77a 71 uint8_t cLast_l = 0x00, cLast_r = 0x00;
adam_z 7:f33a935eb77a 72 bool headerCaptured_l = 0, headerCaptured_r = 0;
adam_z 7:f33a935eb77a 73 uint8_t fromPhoton_l[2] = {0,0}, fromPhoton_r[2] = {0,0};
adam_z 7:f33a935eb77a 74 float slipAcceleration_L = 0.1, slipAcceleration_R = 0.1, slipAcceleration_L_f, slipAcceleration_R_f;
adam_z 7:f33a935eb77a 75 int i_l, i_r;
adam_z 7:f33a935eb77a 76
adam_z 0:150bb4605023 77 int main()
adam_z 0:150bb4605023 78 {
adam_z 2:53a942d1b1e5 79
adam_z 7:f33a935eb77a 80 pc.baud(230400);
adam_z 6:5bd08053e95c 81 blueTooth.baud(230400);
adam_z 7:f33a935eb77a 82 LeftSerial.baud(230400); //uart commu with photon left
adam_z 7:f33a935eb77a 83 RightSerial.baud(230400); //uart commu with photon right
adam_z 7:f33a935eb77a 84
adam_z 0:150bb4605023 85 if( sensor.begin() != 0 ) {
adam_z 0:150bb4605023 86 pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
adam_z 0:150bb4605023 87 } else {
adam_z 0:150bb4605023 88 pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
adam_z 0:150bb4605023 89 }
adam_z 0:150bb4605023 90 sensor.setGyroOffset(38,-24,-106);
adam_z 0:150bb4605023 91 sensor.setAccelOffset(-793,-511,300);
adam_z 2:53a942d1b1e5 92
adam_z 6:5bd08053e95c 93 motorCur_L.SetParams(3.3*8/0.6, Km_L, 0.04348);
adam_z 6:5bd08053e95c 94 motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);
adam_z 2:53a942d1b1e5 95
adam_z 0:150bb4605023 96 WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
adam_z 7:f33a935eb77a 97 blueTooth.attach(&bluetoothRx, Serial::RxIrq);
adam_z 7:f33a935eb77a 98 LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
adam_z 7:f33a935eb77a 99 RightSerial.attach(&RXIrqRight, Serial::RxIrq);
adam_z 7:f33a935eb77a 100
adam_z 2:53a942d1b1e5 101 while(true) {
adam_z 4:a2d38818c4e7 102 //pc.printf("%5.4f\t", 10*pitch_angle);
adam_z 4:a2d38818c4e7 103 //pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
adam_z 4:a2d38818c4e7 104 //pc.printf("%5.3f\r\n", 10*curCmd_L);
adam_z 6:5bd08053e95c 105
adam_z 6:5bd08053e95c 106
adam_z 7:f33a935eb77a 107 pc.printf("%5.4f\t", slipAcceleration_L_f);
adam_z 7:f33a935eb77a 108 pc.printf("%5.4f\r\n", slipAcceleration_R_f);
adam_z 7:f33a935eb77a 109
adam_z 7:f33a935eb77a 110 // pc.putc(fromPhoton_l[0]);
adam_z 7:f33a935eb77a 111 // pc.putc(fromPhoton_l[1]);
adam_z 7:f33a935eb77a 112 // pc.putc(fromPhoton_r[0]);
adam_z 7:f33a935eb77a 113 // pc.putc(fromPhoton_r[1]);
adam_z 6:5bd08053e95c 114
adam_z 2:53a942d1b1e5 115 }
adam_z 0:150bb4605023 116 }
adam_z 0:150bb4605023 117
adam_z 0:150bb4605023 118
adam_z 0:150bb4605023 119 void WIPVTimerInterrupt()
adam_z 0:150bb4605023 120 {
adam_z 2:53a942d1b1e5 121 if(tim_counter <100)tim_counter++;
adam_z 2:53a942d1b1e5 122 else if (tim_counter >= 100 && tim_counter <=109) {
adam_z 4:a2d38818c4e7 123 motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
adam_z 4:a2d38818c4e7 124 motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
adam_z 2:53a942d1b1e5 125 tim_counter++;
adam_z 2:53a942d1b1e5 126 if(tim_counter == 110) {
adam_z 4:a2d38818c4e7 127 motorCur_L.currentOffset = motorCur_L.currentOffset/10;
adam_z 4:a2d38818c4e7 128 motorCur_R.currentOffset = motorCur_R.currentOffset/10;
adam_z 2:53a942d1b1e5 129 }
adam_z 2:53a942d1b1e5 130
adam_z 2:53a942d1b1e5 131 } else {
adam_z 2:53a942d1b1e5 132
adam_z 0:150bb4605023 133 /*
adam_z 0:150bb4605023 134 int16_t data_array[6];
adam_z 2:53a942d1b1e5 135
adam_z 0:150bb4605023 136 data_array[0] = sensor.readRawAccelX();
adam_z 0:150bb4605023 137 data_array[1] = sensor.readRawAccelY();
adam_z 0:150bb4605023 138 data_array[2] = sensor.readRawAccelZ();
adam_z 0:150bb4605023 139 data_array[3] = sensor.readRawGyroX();
adam_z 0:150bb4605023 140 data_array[4] = sensor.readRawGyroY();
adam_z 0:150bb4605023 141 data_array[5] = sensor.readRawGyroZ();
adam_z 2:53a942d1b1e5 142
adam_z 0:150bb4605023 143 pc.printf("%d, ", data_array[0]);
adam_z 0:150bb4605023 144 pc.printf("%d, ", data_array[1]);
adam_z 0:150bb4605023 145 pc.printf("%d, ", data_array[2]);
adam_z 0:150bb4605023 146 pc.printf("%d, ", data_array[3]);
adam_z 0:150bb4605023 147 pc.printf("%d, ", data_array[4]);
adam_z 0:150bb4605023 148 pc.printf("%d;\r\n ", data_array[5]);
adam_z 0:150bb4605023 149 */
adam_z 2:53a942d1b1e5 150
adam_z 2:53a942d1b1e5 151
adam_z 5:842372be775c 152 float data_array[6];//Gs and deg/s
adam_z 0:150bb4605023 153 data_array[0] = sensor.readFloatAccelX();
adam_z 0:150bb4605023 154 data_array[1] = sensor.readFloatAccelY();
adam_z 0:150bb4605023 155 data_array[2] = sensor.readFloatAccelZ();
adam_z 0:150bb4605023 156 data_array[3] = sensor.readFloatGyroX();
adam_z 0:150bb4605023 157 data_array[4] = sensor.readFloatGyroY();
adam_z 0:150bb4605023 158 data_array[5] = sensor.readFloatGyroZ();
adam_z 4:a2d38818c4e7 159 sensor.complementaryFilter(data_array,Ts);
adam_z 4:a2d38818c4e7 160 //SensorAcquisition(data_array, Ts);
adam_z 6:5bd08053e95c 161
adam_z 6:5bd08053e95c 162 //===============wheel speed calculation============//
adam_z 2:53a942d1b1e5 163 wheel_L.Calculate();
adam_z 2:53a942d1b1e5 164 wheel_R.Calculate();
adam_z 6:5bd08053e95c 165
adam_z 6:5bd08053e95c 166 /////////////Balancing Control/////////////////////////
adam_z 6:5bd08053e95c 167 //SI dimension
adam_z 6:5bd08053e95c 168 state[0] = sensor.pitch*3.14159/180.0;
adam_z 6:5bd08053e95c 169 state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 10.0);
adam_z 6:5bd08053e95c 170 state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),60.0);
adam_z 6:5bd08053e95c 171 state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),60.0);
adam_z 6:5bd08053e95c 172
adam_z 6:5bd08053e95c 173 if(accelerateFlag == 1) {
adam_z 6:5bd08053e95c 174 if(velocityCmd[0]>=7 || velocityCmd[1]>=7)accelerateFlag = 0;
adam_z 6:5bd08053e95c 175 else {
adam_z 6:5bd08053e95c 176 velocityCmd[0] += 0.004;
adam_z 6:5bd08053e95c 177 velocityCmd[1] += 0.004;
adam_z 6:5bd08053e95c 178 }
adam_z 6:5bd08053e95c 179 }
adam_z 6:5bd08053e95c 180
adam_z 6:5bd08053e95c 181 ref[2] = velocityCmd[0];
adam_z 6:5bd08053e95c 182 ref[3] = velocityCmd[1];
adam_z 6:5bd08053e95c 183
adam_z 7:f33a935eb77a 184 yawRate = sensorFilter4.filter(data_array[4],20);
adam_z 6:5bd08053e95c 185
adam_z 6:5bd08053e95c 186
adam_z 6:5bd08053e95c 187 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 188 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 7:f33a935eb77a 189 //=========================Anti slip control=============================//
adam_z 7:f33a935eb77a 190 slipAcceleration_L_f = (1 - 10*2*3.141593*Ts)*slipAcceleration_L_f + 10*2*3.141593*Ts*slipAcceleration_L;
adam_z 7:f33a935eb77a 191 slipAcceleration_R_f = (1 - 10*2*3.141593*Ts)*slipAcceleration_R_f + 10*2*3.141593*Ts*slipAcceleration_R;
adam_z 6:5bd08053e95c 192
adam_z 7:f33a935eb77a 193 //motorCur_L.Control(saturation(torque_L/Km_L + 0.008*yawRate,1.2,-1.2), state[2]);
adam_z 7:f33a935eb77a 194 // motorCur_R.Control(saturation(torque_R/Km_R + 0.008*yawRate,1.2,-1.2), -state[3]);
adam_z 6:5bd08053e95c 195
adam_z 7:f33a935eb77a 196
adam_z 6:5bd08053e95c 197 /* //PD Balancing Control
adam_z 6:5bd08053e95c 198 balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
adam_z 6:5bd08053e95c 199 curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
adam_z 6:5bd08053e95c 200 //======================current control=========================//
adam_z 6:5bd08053e95c 201 tim += Ts;
adam_z 6:5bd08053e95c 202 if(tim >= 4*pi/omega)tim = 0.0;
adam_z 6:5bd08053e95c 203 //curCmd_R = amp*sin(omega*tim); //current command
adam_z 6:5bd08053e95c 204 //curCmd_L = 0.8;
adam_z 6:5bd08053e95c 205
adam_z 6:5bd08053e95c 206 motorCur_R.SetPWMDuty(0.75);
adam_z 6:5bd08053e95c 207
adam_z 6:5bd08053e95c 208 motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
adam_z 6:5bd08053e95c 209 motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
adam_z 6:5bd08053e95c 210 */
adam_z 6:5bd08053e95c 211
adam_z 2:53a942d1b1e5 212 }
adam_z 2:53a942d1b1e5 213
adam_z 2:53a942d1b1e5 214
adam_z 4:a2d38818c4e7 215 }
adam_z 4:a2d38818c4e7 216
adam_z 4:a2d38818c4e7 217
adam_z 7:f33a935eb77a 218 void bluetoothRx()
adam_z 6:5bd08053e95c 219 {
adam_z 6:5bd08053e95c 220 while(blueTooth.readable()) {
adam_z 6:5bd08053e95c 221 char charRx = blueTooth.getc();
adam_z 6:5bd08053e95c 222 switch(charRx) {
adam_z 6:5bd08053e95c 223 case 'w'://forward
adam_z 6:5bd08053e95c 224 velocityCmd[0] = 2.5;
adam_z 6:5bd08053e95c 225 velocityCmd[1] = 2.5;
adam_z 6:5bd08053e95c 226 accelerateFlag = 0;
adam_z 6:5bd08053e95c 227 break;
adam_z 6:5bd08053e95c 228 case 's'://backward
adam_z 6:5bd08053e95c 229 velocityCmd[0] = -3.0;
adam_z 6:5bd08053e95c 230 velocityCmd[1] = -3.0;
adam_z 6:5bd08053e95c 231 accelerateFlag = 0;
adam_z 6:5bd08053e95c 232 break;
adam_z 6:5bd08053e95c 233 case 'a'://turn left
adam_z 6:5bd08053e95c 234 velocityCmd[0] = -4.0;
adam_z 6:5bd08053e95c 235 velocityCmd[1] = 4.0;
adam_z 6:5bd08053e95c 236 accelerateFlag = 0;
adam_z 6:5bd08053e95c 237 break;
adam_z 6:5bd08053e95c 238 case 'd'://turn right
adam_z 6:5bd08053e95c 239 velocityCmd[0] = 4.0;
adam_z 6:5bd08053e95c 240 velocityCmd[1] = -4.0;
adam_z 6:5bd08053e95c 241 accelerateFlag = 0;
adam_z 6:5bd08053e95c 242 break;
adam_z 6:5bd08053e95c 243 case 'x'://stop
adam_z 6:5bd08053e95c 244 velocityCmd[0] = 0.0;
adam_z 6:5bd08053e95c 245 velocityCmd[1] = 0.0;
adam_z 6:5bd08053e95c 246 accelerateFlag = 0;
adam_z 6:5bd08053e95c 247 break;
adam_z 7:f33a935eb77a 248
adam_z 6:5bd08053e95c 249 case 'q'://accelerate
adam_z 6:5bd08053e95c 250 accelerateFlag = 1;
adam_z 6:5bd08053e95c 251 break;
adam_z 6:5bd08053e95c 252 }
adam_z 6:5bd08053e95c 253 }
adam_z 6:5bd08053e95c 254 }
adam_z 6:5bd08053e95c 255
adam_z 7:f33a935eb77a 256 void RXIrqLeft()
adam_z 7:f33a935eb77a 257 {
adam_z 7:f33a935eb77a 258 // slipAcceleration_L += 0.1;
adam_z 7:f33a935eb77a 259 while(!LeftSerial.readable());
adam_z 7:f33a935eb77a 260 uint8_t c = LeftSerial.getc();
adam_z 7:f33a935eb77a 261 if(!headerCaptured_l & cLast_l == 0xAA & c == 0x55) {
adam_z 7:f33a935eb77a 262 headerCaptured_l = 1;
adam_z 7:f33a935eb77a 263 i_l = 0;
adam_z 7:f33a935eb77a 264 debugLed_l = 1;
adam_z 7:f33a935eb77a 265 } else if(headerCaptured_l) {
adam_z 7:f33a935eb77a 266 if(c == 0xAA | c == 0x55 ) {
adam_z 7:f33a935eb77a 267 headerCaptured_l = 0;
adam_z 7:f33a935eb77a 268 i_l = 0;
adam_z 7:f33a935eb77a 269 debugLed_l = 0;
adam_z 7:f33a935eb77a 270 }
adam_z 7:f33a935eb77a 271 else {
adam_z 7:f33a935eb77a 272 fromPhoton_l[i_l] = c;
adam_z 7:f33a935eb77a 273 i_l++;
adam_z 7:f33a935eb77a 274 if(i_l == 2) {
adam_z 7:f33a935eb77a 275 headerCaptured_l = 0;
adam_z 7:f33a935eb77a 276 debugLed_l = 0;
adam_z 7:f33a935eb77a 277 slipAcceleration_L = -(float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1]))/100.0;
adam_z 7:f33a935eb77a 278
adam_z 7:f33a935eb77a 279 }
adam_z 7:f33a935eb77a 280 }
adam_z 7:f33a935eb77a 281 }
adam_z 7:f33a935eb77a 282 cLast_l = c;
adam_z 7:f33a935eb77a 283
adam_z 7:f33a935eb77a 284 }
adam_z 7:f33a935eb77a 285
adam_z 7:f33a935eb77a 286 void RXIrqRight()
adam_z 7:f33a935eb77a 287 {
adam_z 7:f33a935eb77a 288 // slipAcceleration_L += 0.1;
adam_z 7:f33a935eb77a 289 while(!RightSerial.readable());
adam_z 7:f33a935eb77a 290 uint8_t c = RightSerial.getc();
adam_z 7:f33a935eb77a 291 if(!headerCaptured_r & cLast_r == 0xAA & c == 0x55) {
adam_z 7:f33a935eb77a 292 headerCaptured_r = 1;
adam_z 7:f33a935eb77a 293 i_r = 0;
adam_z 7:f33a935eb77a 294 debugLed_r = 1;
adam_z 7:f33a935eb77a 295 } else if(headerCaptured_r) {
adam_z 7:f33a935eb77a 296 if(c == 0xAA | c == 0x55 ) {
adam_z 7:f33a935eb77a 297 headerCaptured_r = 0;
adam_z 7:f33a935eb77a 298 i_r = 0;
adam_z 7:f33a935eb77a 299 debugLed_r = 0;
adam_z 7:f33a935eb77a 300 }
adam_z 7:f33a935eb77a 301 else {
adam_z 7:f33a935eb77a 302 fromPhoton_r[i_r] = c;
adam_z 7:f33a935eb77a 303 i_r++;
adam_z 7:f33a935eb77a 304 if(i_r == 2) {
adam_z 7:f33a935eb77a 305 headerCaptured_r = 0;
adam_z 7:f33a935eb77a 306 debugLed_r = 0;
adam_z 7:f33a935eb77a 307 slipAcceleration_R = (float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1]))/100.0;
adam_z 7:f33a935eb77a 308 }
adam_z 7:f33a935eb77a 309 }
adam_z 7:f33a935eb77a 310 }
adam_z 7:f33a935eb77a 311 cLast_r = c;
adam_z 7:f33a935eb77a 312
adam_z 7:f33a935eb77a 313 }
adam_z 7:f33a935eb77a 314
adam_z 6:5bd08053e95c 315
adam_z 6:5bd08053e95c 316
adam_z 4:a2d38818c4e7 317 float saturation(float input, float limit_H, float limit_L)
adam_z 4:a2d38818c4e7 318 {
adam_z 4:a2d38818c4e7 319 float output;
adam_z 4:a2d38818c4e7 320 if(input >= limit_H)output = limit_H;
adam_z 4:a2d38818c4e7 321 else if (input <= limit_L)output = limit_L;
adam_z 4:a2d38818c4e7 322 else output = input;
adam_z 4:a2d38818c4e7 323 return output;
adam_z 4:a2d38818c4e7 324 }
adam_z 4:a2d38818c4e7 325
adam_z 4:a2d38818c4e7 326
adam_z 4:a2d38818c4e7 327 void SensorAcquisition(float * data, float samplingTime)
adam_z 6:5bd08053e95c 328 {
adam_z 4:a2d38818c4e7 329
adam_z 6:5bd08053e95c 330 axm = data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
adam_z 6:5bd08053e95c 331 aym = data[1]*(-1)*9.81;
adam_z 6:5bd08053e95c 332 azm = data[2]*(-1)*9.81;
adam_z 6:5bd08053e95c 333 u1 = data[0]*3.14159/180; //gyroscope :deg/s to rad/s
adam_z 6:5bd08053e95c 334 u2 = data[1]*3.14159/180;
adam_z 6:5bd08053e95c 335 u3 = data[2]*3.14159/180;
adam_z 6:5bd08053e95c 336
adam_z 6:5bd08053e95c 337
adam_z 6:5bd08053e95c 338 if(conv_init <= 3) {
adam_z 4:a2d38818c4e7 339 axm_f_old = axm;
adam_z 4:a2d38818c4e7 340 aym_f_old = aym;
adam_z 4:a2d38818c4e7 341 azm_f_old = azm;
adam_z 6:5bd08053e95c 342
adam_z 4:a2d38818c4e7 343 conv_init++;
adam_z 6:5bd08053e95c 344 } else {
adam_z 6:5bd08053e95c 345 pitch_fusion(axm, aym,azm,u3,u2,20, samplingTime);
adam_z 6:5bd08053e95c 346 roll_fusion(axm, aym,azm,u3,u1,20, samplingTime);
adam_z 6:5bd08053e95c 347 x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
adam_z 6:5bd08053e95c 348 }
adam_z 6:5bd08053e95c 349
adam_z 4:a2d38818c4e7 350 }
adam_z 4:a2d38818c4e7 351