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@9:a91135551be1, 2016-07-28 (annotated)

Committer:
adam_z
Date:
Thu Jul 28 08:17:30 2016 +0000
Revision:
9:a91135551be1
Parent:
8:9f4c10787775
Child:
10:2dc43cd59ff0
ready to combine RF24L01 to this board;

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 8:9f4c10787775 7 #include "MEDIAN_FILTER.h"
adam_z 8:9f4c10787775 8 #include <algorithm>
adam_z 0:150bb4605023 9
adam_z 0:150bb4605023 10 #define Ts 0.001
adam_z 2:53a942d1b1e5 11 #define pi 3.14159
adam_z 0:150bb4605023 12
adam_z 0:150bb4605023 13 LSM9DS0 sensor(SPI_MODE, D9, D6);
adam_z 0:150bb4605023 14 Serial pc(SERIAL_TX, SERIAL_RX);
adam_z 6:5bd08053e95c 15 Serial blueTooth(D10, D2);
adam_z 7:f33a935eb77a 16 Serial LeftSerial(PC_12, PD_2);
adam_z 7:f33a935eb77a 17 Serial RightSerial(PC_10, PC_11);
adam_z 7:f33a935eb77a 18
adam_z 7:f33a935eb77a 19 DigitalOut debugLed_l(A4);
adam_z 7:f33a935eb77a 20 DigitalOut debugLed_r(A5);
adam_z 0:150bb4605023 21
adam_z 8:9f4c10787775 22 InterruptIn SCEnable(USER_BUTTON);
adam_z 8:9f4c10787775 23
adam_z 8:9f4c10787775 24
adam_z 0:150bb4605023 25 Ticker WIPVTimer;
adam_z 0:150bb4605023 26 void WIPVTimerInterrupt();
adam_z 4:a2d38818c4e7 27 float saturation(float input, float limit_H, float limit_L);
adam_z 8:9f4c10787775 28 //void SensorAcquisition(float * data, float samplingTime);
adam_z 7:f33a935eb77a 29
adam_z 7:f33a935eb77a 30 void bluetoothRx();
adam_z 7:f33a935eb77a 31 void RXIrqLeft();
adam_z 7:f33a935eb77a 32 void RXIrqRight();
adam_z 0:150bb4605023 33
adam_z 8:9f4c10787775 34 void SCEnableFunc();
adam_z 8:9f4c10787775 35 bool SCEnalbeIndicator;
adam_z 8:9f4c10787775 36
adam_z 5:842372be775c 37 //MOTOR L == MOTOR 1; MOTOR R = MOTOR 2
adam_z 5:842372be775c 38 CURRENT_CONTROL motorCur_L(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
adam_z 5:842372be775c 39 CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
adam_z 1:ddc39900f9b8 40
adam_z 9:a91135551be1 41 QEI wheel_L(D13, D12, NC, 280, 250, Ts, QEI::X4_ENCODING);
adam_z 9:a91135551be1 42 QEI wheel_R(A1, A2, NC, 280, 250, Ts, QEI::X4_ENCODING);
adam_z 5:842372be775c 43
adam_z 2:53a942d1b1e5 44
adam_z 4:a2d38818c4e7 45 PID balancingPD(20,0.00,0.0,Ts);
adam_z 6:5bd08053e95c 46 LPF sensorFilter1(Ts);
adam_z 6:5bd08053e95c 47 LPF sensorFilter2(Ts);
adam_z 6:5bd08053e95c 48 LPF sensorFilter3(Ts);
adam_z 6:5bd08053e95c 49 LPF sensorFilter4(Ts);
adam_z 2:53a942d1b1e5 50
adam_z 8:9f4c10787775 51 MedianFilter slipA_L_MF(9);
adam_z 8:9f4c10787775 52 MedianFilter slipA_R_MF(9);
adam_z 8:9f4c10787775 53
adam_z 4:a2d38818c4e7 54
adam_z 4:a2d38818c4e7 55
adam_z 5:842372be775c 56
adam_z 4:a2d38818c4e7 57
adam_z 4:a2d38818c4e7 58 int tim_counter = 0;
adam_z 2:53a942d1b1e5 59 float tim = 0.0;
adam_z 4:a2d38818c4e7 60 float amp = 0.3;
adam_z 4:a2d38818c4e7 61 float omega = 6.0;
adam_z 4:a2d38818c4e7 62 float curCmd_L =0.0, curCmd_R =0.0;
adam_z 2:53a942d1b1e5 63
adam_z 0:150bb4605023 64
adam_z 8:9f4c10787775 65 //*************real plant states and matrices*****************************
adam_z 6:5bd08053e95c 66 float state[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 6:5bd08053e95c 67 float ref[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 6:5bd08053e95c 68
adam_z 6:5bd08053e95c 69 float torque_L = 0.0, torque_R = 0.0;
adam_z 9:a91135551be1 70 float KL[4] = {-1.9483 , -0.1317 , 0.0006*2 , -0.0112*2};//{-0.7057 , -0.0733 , -0.0085 , -0.0300};
adam_z 9:a91135551be1 71 float KR[4] = {-1.9483, -0.1317 , -0.0112*2 , 0.0006*2};//{-0.7057 , -0.0733 , -0.0300 , -0.0085};
adam_z 8:9f4c10787775 72
adam_z 8:9f4c10787775 73 //********simulated states and multiplying matrices***********************
adam_z 8:9f4c10787775 74 float z1_prime[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 8:9f4c10787775 75 float z1_prime_dot[4] = {0.0, 0.0, 0.0, 0.0};
adam_z 8:9f4c10787775 76 float v1[2] = {0.0,0.0};
adam_z 8:9f4c10787775 77 float A1_prime0[4] = {0,1,0,0},A1_prime1[4] = {176.4899,0,0,0},A1_prime2[4] = {-83.1468,0, 0, 0},A1_prime3[4] = {-83.1468, 0, 0,0};
adam_z 8:9f4c10787775 78 float B11_prime0[2] = {0,0}, B11_prime1[2] = {-537.6123, -537.6123}, B11_prime2[2] = { 679.2587, 104.3936}, B11_prime3[2] = { 104.3936, 679.2587};
adam_z 8:9f4c10787775 79 float B31_0[2] = {0,0},B31_1[2] = {0.7840,0.7840},B31_2[2] = {12.1836, 0.3086},B31_3[2] = {0.3086, 12.1836};
adam_z 8:9f4c10787775 80
adam_z 9:a91135551be1 81 float Ks0[4] = {-1.9483, -0.1317, 0.0006, -0.0112}, Ks1[4] = {-1.9483, -0.1317, -0.0112, 0.0006};
adam_z 8:9f4c10787775 82 float yc[2] = {0.0,0.0};
adam_z 8:9f4c10787775 83
adam_z 8:9f4c10787775 84
adam_z 6:5bd08053e95c 85
adam_z 6:5bd08053e95c 86 float Km_L = 1.050*0.163;
adam_z 6:5bd08053e95c 87 float Km_R = 1.187*0.137;
adam_z 6:5bd08053e95c 88
adam_z 6:5bd08053e95c 89 float yawRate = 0.0;
adam_z 6:5bd08053e95c 90
adam_z 6:5bd08053e95c 91 float velocityCmd[2] = {0.0, 0.0};
adam_z 9:a91135551be1 92 unsigned int accelerateFlag = 0, decelerationFlag = 0;
adam_z 6:5bd08053e95c 93
adam_z 7:f33a935eb77a 94
adam_z 9:a91135551be1 95 //uint8_t cLast_l = 0x00, cLast_r = 0x00;
adam_z 9:a91135551be1 96 float cLast_l, cLast_r;
adam_z 7:f33a935eb77a 97 bool headerCaptured_l = 0, headerCaptured_r = 0;
adam_z 7:f33a935eb77a 98 uint8_t fromPhoton_l[2] = {0,0}, fromPhoton_r[2] = {0,0};
adam_z 8:9f4c10787775 99 float slipAcceleration_L = 0.0, slipAcceleration_R = 0.0, slipAcceleration_L_f, slipAcceleration_R_f, slipAcceleration_L_m, slipAcceleration_R_m;
adam_z 9:a91135551be1 100 float slipAcceleration_L_f_d, slipAcceleration_R_f_d;
adam_z 7:f33a935eb77a 101 int i_l, i_r;
adam_z 7:f33a935eb77a 102
adam_z 8:9f4c10787775 103
adam_z 8:9f4c10787775 104 float a_vehicle;
adam_z 9:a91135551be1 105 float a_slip_L,a_slip_R;
adam_z 8:9f4c10787775 106
adam_z 8:9f4c10787775 107 int send_i = 0, send_counter = 0;
adam_z 8:9f4c10787775 108 bool sent_flag = 1;
adam_z 8:9f4c10787775 109 uint8_t data_sent[18];
adam_z 8:9f4c10787775 110
adam_z 9:a91135551be1 111 int delay_num = 37;
adam_z 9:a91135551be1 112 float fifo_array[37];
adam_z 9:a91135551be1 113 float a_vehicle_fifo;
adam_z 9:a91135551be1 114 int fifo_i = 0;
adam_z 9:a91135551be1 115
adam_z 8:9f4c10787775 116
adam_z 0:150bb4605023 117 int main()
adam_z 0:150bb4605023 118 {
adam_z 2:53a942d1b1e5 119
adam_z 7:f33a935eb77a 120 pc.baud(230400);
adam_z 6:5bd08053e95c 121 blueTooth.baud(230400);
adam_z 9:a91135551be1 122 LeftSerial.baud(230400); //uart commu with photon left
adam_z 7:f33a935eb77a 123 RightSerial.baud(230400); //uart commu with photon right
adam_z 7:f33a935eb77a 124
adam_z 0:150bb4605023 125 if( sensor.begin() != 0 ) {
adam_z 0:150bb4605023 126 pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
adam_z 0:150bb4605023 127 } else {
adam_z 0:150bb4605023 128 pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
adam_z 0:150bb4605023 129 }
adam_z 9:a91135551be1 130 sensor.setGyroOffset(38,-20,-95);
adam_z 9:a91135551be1 131 sensor.setAccelOffset(-680,-460,300);
adam_z 2:53a942d1b1e5 132
adam_z 6:5bd08053e95c 133 motorCur_L.SetParams(3.3*8/0.6, Km_L, 0.04348);
adam_z 6:5bd08053e95c 134 motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);
adam_z 2:53a942d1b1e5 135
adam_z 8:9f4c10787775 136 WIPVTimer.attach_us(WIPVTimerInterrupt, Ts*1000.0*1000.0);
adam_z 7:f33a935eb77a 137 blueTooth.attach(&bluetoothRx, Serial::RxIrq);
adam_z 7:f33a935eb77a 138 LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
adam_z 9:a91135551be1 139 RightSerial.attach(&RXIrqRight, Serial::RxIrq);
adam_z 8:9f4c10787775 140
adam_z 8:9f4c10787775 141 SCEnable.rise(&SCEnableFunc);
adam_z 8:9f4c10787775 142 SCEnalbeIndicator = 0;
adam_z 7:f33a935eb77a 143
adam_z 2:53a942d1b1e5 144 while(true) {
adam_z 4:a2d38818c4e7 145 //pc.printf("%5.4f\t", 10*pitch_angle);
adam_z 8:9f4c10787775 146 // pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
adam_z 8:9f4c10787775 147 // pc.printf("%5.3f\r\n", 10*curCmd_L);
adam_z 8:9f4c10787775 148
adam_z 8:9f4c10787775 149
adam_z 8:9f4c10787775 150 //if(sent_flag == 0) {///
adam_z 8:9f4c10787775 151 // pc.putc(data_sent[send_i]);
adam_z 8:9f4c10787775 152 // send_i++;
adam_z 8:9f4c10787775 153 // if(send_i >= 18) {
adam_z 8:9f4c10787775 154 // send_i = 0;
adam_z 8:9f4c10787775 155 // sent_flag = 1;
adam_z 8:9f4c10787775 156 // }
adam_z 8:9f4c10787775 157 // }///
adam_z 9:a91135551be1 158 pc.printf("%5.4f\t",a_vehicle);
adam_z 9:a91135551be1 159 pc.printf("%5.4f\t",a_vehicle_fifo);
adam_z 9:a91135551be1 160 // pc.printf("%5.4f\t", slipAcceleration_L);
adam_z 9:a91135551be1 161 pc.printf("%5.4f\r\n",slipAcceleration_L);
adam_z 9:a91135551be1 162 // pc.printf("%5.4f\t",yc[0]);
adam_z 9:a91135551be1 163 // pc.printf("%5.4f\r\n",yc[1]);
adam_z 6:5bd08053e95c 164
adam_z 2:53a942d1b1e5 165 }
adam_z 0:150bb4605023 166 }
adam_z 0:150bb4605023 167
adam_z 0:150bb4605023 168
adam_z 0:150bb4605023 169 void WIPVTimerInterrupt()
adam_z 0:150bb4605023 170 {
adam_z 2:53a942d1b1e5 171 if(tim_counter <100)tim_counter++;
adam_z 2:53a942d1b1e5 172 else if (tim_counter >= 100 && tim_counter <=109) {
adam_z 4:a2d38818c4e7 173 motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
adam_z 4:a2d38818c4e7 174 motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
adam_z 2:53a942d1b1e5 175 tim_counter++;
adam_z 2:53a942d1b1e5 176 if(tim_counter == 110) {
adam_z 4:a2d38818c4e7 177 motorCur_L.currentOffset = motorCur_L.currentOffset/10;
adam_z 4:a2d38818c4e7 178 motorCur_R.currentOffset = motorCur_R.currentOffset/10;
adam_z 2:53a942d1b1e5 179 }
adam_z 2:53a942d1b1e5 180
adam_z 2:53a942d1b1e5 181 } else {
adam_z 2:53a942d1b1e5 182
adam_z 2:53a942d1b1e5 183
adam_z 9:a91135551be1 184 //int16_t data_array[6];
adam_z 9:a91135551be1 185 //
adam_z 9:a91135551be1 186 // data_array[0] = sensor.readRawAccelX();
adam_z 9:a91135551be1 187 // data_array[1] = sensor.readRawAccelY();
adam_z 9:a91135551be1 188 //// data_array[2] = sensor.readRawAccelZ();
adam_z 9:a91135551be1 189 //// data_array[3] = sensor.readRawGyroX();
adam_z 9:a91135551be1 190 // data_array[4] = sensor.readRawGyroY();
adam_z 9:a91135551be1 191 // data_array[5] = sensor.readRawGyroZ();
adam_z 9:a91135551be1 192 //
adam_z 9:a91135551be1 193 // pc.printf("%d, ", data_array[0]);
adam_z 9:a91135551be1 194 // pc.printf("%d, ", data_array[1]);
adam_z 9:a91135551be1 195 //// pc.printf("%d, ", data_array[2]);
adam_z 9:a91135551be1 196 //// pc.printf("%d, ", data_array[3]);
adam_z 9:a91135551be1 197 // pc.printf("%d, ", data_array[4]);
adam_z 9:a91135551be1 198 // pc.printf("%d;\r\n ", data_array[5]);
adam_z 2:53a942d1b1e5 199
adam_z 2:53a942d1b1e5 200
adam_z 2:53a942d1b1e5 201
adam_z 5:842372be775c 202 float data_array[6];//Gs and deg/s
adam_z 0:150bb4605023 203 data_array[0] = sensor.readFloatAccelX();
adam_z 0:150bb4605023 204 data_array[1] = sensor.readFloatAccelY();
adam_z 8:9f4c10787775 205 // data_array[2] = sensor.readFloatAccelZ();
adam_z 8:9f4c10787775 206 // data_array[3] = sensor.readFloatGyroX();
adam_z 0:150bb4605023 207 data_array[4] = sensor.readFloatGyroY();
adam_z 0:150bb4605023 208 data_array[5] = sensor.readFloatGyroZ();
adam_z 8:9f4c10787775 209
adam_z 4:a2d38818c4e7 210 sensor.complementaryFilter(data_array,Ts);
adam_z 9:a91135551be1 211
adam_z 6:5bd08053e95c 212
adam_z 6:5bd08053e95c 213 //===============wheel speed calculation============//
adam_z 2:53a942d1b1e5 214 wheel_L.Calculate();
adam_z 2:53a942d1b1e5 215 wheel_R.Calculate();
adam_z 6:5bd08053e95c 216
adam_z 9:a91135551be1 217
adam_z 6:5bd08053e95c 218 /////////////Balancing Control/////////////////////////
adam_z 6:5bd08053e95c 219 //SI dimension
adam_z 6:5bd08053e95c 220 state[0] = sensor.pitch*3.14159/180.0;
adam_z 9:a91135551be1 221 state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 6.0);
adam_z 9:a91135551be1 222 state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),80.0);
adam_z 9:a91135551be1 223 state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),80.0);
adam_z 6:5bd08053e95c 224
adam_z 6:5bd08053e95c 225 if(accelerateFlag == 1) {
adam_z 9:a91135551be1 226 if(velocityCmd[0]>=20 || velocityCmd[1]>=20) {
adam_z 9:a91135551be1 227 accelerateFlag = 0;
adam_z 9:a91135551be1 228 decelerationFlag = 1;
adam_z 9:a91135551be1 229 } else {
adam_z 9:a91135551be1 230 velocityCmd[0] += 0.010;
adam_z 9:a91135551be1 231 velocityCmd[1] += 0.010;
adam_z 6:5bd08053e95c 232 }
adam_z 6:5bd08053e95c 233 }
adam_z 6:5bd08053e95c 234
adam_z 9:a91135551be1 235 if(decelerationFlag == 1 & velocityCmd[0] > 0.0) {
adam_z 9:a91135551be1 236 velocityCmd[0] -= 0.015;
adam_z 9:a91135551be1 237 velocityCmd[1] -= 0.015;
adam_z 9:a91135551be1 238 if(velocityCmd[0] <= 0.0)decelerationFlag = 0;
adam_z 9:a91135551be1 239 }
adam_z 9:a91135551be1 240
adam_z 6:5bd08053e95c 241 ref[2] = velocityCmd[0];
adam_z 6:5bd08053e95c 242 ref[3] = velocityCmd[1];
adam_z 6:5bd08053e95c 243
adam_z 9:a91135551be1 244 yawRate = sensorFilter4.filter(data_array[4],9);
adam_z 6:5bd08053e95c 245
adam_z 6:5bd08053e95c 246
adam_z 8:9f4c10787775 247 //===============integration of z1_prime=========================
adam_z 8:9f4c10787775 248 z1_prime[0] += z1_prime_dot[0]*Ts;
adam_z 8:9f4c10787775 249 z1_prime[1] += z1_prime_dot[1]*Ts;
adam_z 8:9f4c10787775 250 z1_prime[2] += z1_prime_dot[2]*Ts;
adam_z 8:9f4c10787775 251 z1_prime[3] += z1_prime_dot[3]*Ts;
adam_z 8:9f4c10787775 252
adam_z 8:9f4c10787775 253
adam_z 8:9f4c10787775 254 yc[0] = (KL[0]*(ref[0] - (state[0] - z1_prime[0])) + KL[1]*(ref[1] - (state[1] - z1_prime[1]))\
adam_z 8:9f4c10787775 255 + KL[2]*(ref[2] - (state[2] - z1_prime[2])) + KL[3]*(ref[3] - (state[3] - z1_prime[3])));
adam_z 8:9f4c10787775 256
adam_z 8:9f4c10787775 257 yc[1] = (KR[0]*(ref[0] - (state[0] - z1_prime[0])) + KR[1]*(ref[1] - (state[1] - z1_prime[1]))\
adam_z 8:9f4c10787775 258 + KR[2]*(ref[2] - (state[2] - z1_prime[2])) + KR[3]*(ref[3] - (state[3] - z1_prime[3])));
adam_z 8:9f4c10787775 259
adam_z 8:9f4c10787775 260
adam_z 7:f33a935eb77a 261 //=========================Anti slip control=============================//
adam_z 8:9f4c10787775 262 //***********filtering the slip acceleration of two wheels********************
adam_z 8:9f4c10787775 263 a_vehicle = (data_array[0]*cos(state[0]) - data_array[1]*sin(state[0]))*9.791;//compute acceleration of vehicle using lsm9ds0
adam_z 9:a91135551be1 264 //fifo delay
adam_z 9:a91135551be1 265 a_vehicle_fifo = fifo_array[fifo_i];
adam_z 9:a91135551be1 266 // delay_num = sizeof(fifo_array);
adam_z 9:a91135551be1 267 fifo_array[fifo_i] = a_vehicle;
adam_z 9:a91135551be1 268 fifo_i++;
adam_z 9:a91135551be1 269 if(fifo_i >= delay_num)fifo_i = 0;
adam_z 8:9f4c10787775 270
adam_z 9:a91135551be1 271
adam_z 9:a91135551be1 272
adam_z 9:a91135551be1 273
adam_z 9:a91135551be1 274 //pc.printf("%5.2f\t", a_vehicle);
adam_z 9:a91135551be1 275 //// pc.printf("%5.2f\t",slipAcceleration_L_f);
adam_z 9:a91135551be1 276 // pc.printf("%5.2f\r\n",slipAcceleration_R_f);
adam_z 9:a91135551be1 277
adam_z 9:a91135551be1 278 a_slip_L = slipAcceleration_L - a_vehicle_fifo;
adam_z 9:a91135551be1 279 a_slip_R = slipAcceleration_R - a_vehicle_fifo;
adam_z 9:a91135551be1 280
adam_z 9:a91135551be1 281 float alpha = 40.0;//HZ
adam_z 9:a91135551be1 282 slipAcceleration_L_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_L_f + alpha*2*3.141593*Ts*a_slip_L;
adam_z 9:a91135551be1 283 slipAcceleration_R_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_R_f + alpha*2*3.141593*Ts*a_slip_R;
adam_z 8:9f4c10787775 284
adam_z 8:9f4c10787775 285 if(SCEnalbeIndicator == 0) {
adam_z 9:a91135551be1 286 slipAcceleration_L_f_d = 0;
adam_z 9:a91135551be1 287 slipAcceleration_R_f_d = 0;
adam_z 8:9f4c10787775 288
adam_z 8:9f4c10787775 289 } else {
adam_z 9:a91135551be1 290 //***********dead zone************************//
adam_z 9:a91135551be1 291 float deadzone = 0.0;
adam_z 9:a91135551be1 292 if(slipAcceleration_L_f < deadzone & slipAcceleration_L_f > -deadzone)slipAcceleration_L_f_d = 0;
adam_z 9:a91135551be1 293 else slipAcceleration_L_f_d = slipAcceleration_L_f;
adam_z 8:9f4c10787775 294
adam_z 9:a91135551be1 295 if(slipAcceleration_R_f < deadzone & slipAcceleration_R_f > -deadzone)slipAcceleration_R_f_d = 0;
adam_z 9:a91135551be1 296 else slipAcceleration_R_f_d = slipAcceleration_R_f;
adam_z 8:9f4c10787775 297
adam_z 8:9f4c10787775 298 }
adam_z 8:9f4c10787775 299
adam_z 9:a91135551be1 300
adam_z 8:9f4c10787775 301 //***************simulated plant feedback**************
adam_z 8:9f4c10787775 302 v1[0] = -(Ks0[0]*z1_prime[0] + Ks0[1]*z1_prime[1] + Ks0[2]*z1_prime[2] + Ks0[3]*z1_prime[3]);
adam_z 8:9f4c10787775 303 v1[1] = -(Ks1[0]*z1_prime[0] + Ks1[1]*z1_prime[1] + Ks1[2]*z1_prime[2] + Ks1[3]*z1_prime[3]);
adam_z 8:9f4c10787775 304
adam_z 8:9f4c10787775 305 //**********simulated dynamics*****************
adam_z 8:9f4c10787775 306 z1_prime_dot[0] = A1_prime0[0]*z1_prime[0] + A1_prime0[1]*z1_prime[1]+A1_prime0[2]*z1_prime[2]+A1_prime0[3]*z1_prime[3]\
adam_z 8:9f4c10787775 307 + B11_prime0[0]*v1[0]+ B11_prime0[1]*v1[1]\
adam_z 9:a91135551be1 308 + B31_0[0]*slipAcceleration_L_f_d+ B31_0[1]*slipAcceleration_R_f_d;
adam_z 8:9f4c10787775 309
adam_z 8:9f4c10787775 310 z1_prime_dot[1] = A1_prime1[0]*z1_prime[0] + A1_prime1[1]*z1_prime[1]+A1_prime1[2]*z1_prime[2]+A1_prime1[3]*z1_prime[3]\
adam_z 8:9f4c10787775 311 + B11_prime1[0]*v1[0]+ B11_prime1[1]*v1[1]\
adam_z 9:a91135551be1 312 + B31_1[0]*slipAcceleration_L_f_d+ B31_1[1]*slipAcceleration_R_f_d;
adam_z 8:9f4c10787775 313
adam_z 8:9f4c10787775 314 z1_prime_dot[2] = A1_prime2[0]*z1_prime[0] + A1_prime2[1]*z1_prime[1]+A1_prime2[2]*z1_prime[2]+A1_prime2[3]*z1_prime[3]\
adam_z 8:9f4c10787775 315 + B11_prime2[0]*v1[0]+ B11_prime2[1]*v1[1]\
adam_z 9:a91135551be1 316 + B31_2[0]*slipAcceleration_L_f_d+ B31_2[1]*slipAcceleration_R_f_d;
adam_z 8:9f4c10787775 317
adam_z 8:9f4c10787775 318 z1_prime_dot[3] = A1_prime3[0]*z1_prime[0] + A1_prime3[1]*z1_prime[1]+A1_prime3[2]*z1_prime[2]+A1_prime3[3]*z1_prime[3]\
adam_z 8:9f4c10787775 319 + B11_prime3[0]*v1[0]+ B11_prime3[1]*v1[1]\
adam_z 9:a91135551be1 320 + B31_3[0]*slipAcceleration_L_f_d+ B31_3[1]*slipAcceleration_R_f_d;
adam_z 8:9f4c10787775 321
adam_z 9:a91135551be1 322 torque_L = -((yc[0] + v1[0])/Km_L) + 0.015*yawRate;
adam_z 9:a91135551be1 323 torque_R = ((yc[1] + v1[1])/Km_R) + 0.015*yawRate;
adam_z 6:5bd08053e95c 324
adam_z 8:9f4c10787775 325 //************motor torque control*****************
adam_z 9:a91135551be1 326 // motorCur_L.Control(saturation(torque_L,1.3,-1.3), state[2]);
adam_z 9:a91135551be1 327 // motorCur_R.Control(saturation(torque_R,1.3,-1.3), -state[3]);
adam_z 8:9f4c10787775 328
adam_z 8:9f4c10787775 329 //**************sending data to PC******************
adam_z 8:9f4c10787775 330 if(send_counter < 40)send_counter++;
adam_z 8:9f4c10787775 331
adam_z 8:9f4c10787775 332 else if( sent_flag == 1 & send_counter >= 40) {
adam_z 8:9f4c10787775 333 send_counter = 0;
adam_z 8:9f4c10787775 334 int multiplier = 500;
adam_z 8:9f4c10787775 335 data_sent[0] = 0xAA;
adam_z 8:9f4c10787775 336 data_sent[1] = 0x55;
adam_z 8:9f4c10787775 337
adam_z 8:9f4c10787775 338 data_sent[2] = ((int16_t)((state[0])*multiplier)) >>8;
adam_z 8:9f4c10787775 339 data_sent[3] = ((int16_t)((state[0])*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 340 data_sent[4] = ((int16_t)(state[1]*multiplier)) >>8;
adam_z 8:9f4c10787775 341 data_sent[5] = ((int16_t)(state[1]*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 342
adam_z 8:9f4c10787775 343 data_sent[6] = ((int16_t)((v1[0])*multiplier)) >>8;
adam_z 8:9f4c10787775 344 data_sent[7] = ((int16_t)((v1[0])*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 345 data_sent[8] = ((int16_t)((v1[1])*multiplier)) >>8;
adam_z 8:9f4c10787775 346 data_sent[9] = ((int16_t)((v1[1])*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 347
adam_z 8:9f4c10787775 348 data_sent[10] = ((int16_t)((yc[0])*multiplier)) >>8;
adam_z 8:9f4c10787775 349 data_sent[11] = ((int16_t)((yc[0])*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 350 data_sent[12] = ((int16_t)((yc[1])*multiplier)) >>8;
adam_z 8:9f4c10787775 351 data_sent[13] = ((int16_t)((yc[1])*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 352 //data_sent[10] = ((int16_t)((-a_vehicle)*multiplier)) >>8;
adam_z 8:9f4c10787775 353 // data_sent[11] = ((int16_t)((-a_vehicle)*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 354 // data_sent[12] = ((int16_t)((slipAcceleration_R_m)*multiplier)) >>8;
adam_z 8:9f4c10787775 355 // data_sent[13] = ((int16_t)((slipAcceleration_R_m)*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 356 data_sent[14] = ((int16_t)((slipAcceleration_L)*multiplier)) >>8;
adam_z 8:9f4c10787775 357 data_sent[15] = ((int16_t)((slipAcceleration_L)*multiplier)) & 0x00FF;
adam_z 9:a91135551be1 358 data_sent[16] = ((int16_t)((slipAcceleration_R)*multiplier)) >>8;
adam_z 9:a91135551be1 359 data_sent[17] = ((int16_t)((slipAcceleration_R)*multiplier)) & 0x00FF;
adam_z 8:9f4c10787775 360
adam_z 8:9f4c10787775 361
adam_z 8:9f4c10787775 362 sent_flag = 0;
adam_z 8:9f4c10787775 363
adam_z 8:9f4c10787775 364 }
adam_z 8:9f4c10787775 365
adam_z 8:9f4c10787775 366
adam_z 8:9f4c10787775 367
adam_z 6:5bd08053e95c 368
adam_z 7:f33a935eb77a 369
adam_z 6:5bd08053e95c 370 /* //PD Balancing Control
adam_z 6:5bd08053e95c 371 balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
adam_z 6:5bd08053e95c 372 curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
adam_z 6:5bd08053e95c 373 //======================current control=========================//
adam_z 6:5bd08053e95c 374 tim += Ts;
adam_z 6:5bd08053e95c 375 if(tim >= 4*pi/omega)tim = 0.0;
adam_z 6:5bd08053e95c 376 //curCmd_R = amp*sin(omega*tim); //current command
adam_z 6:5bd08053e95c 377 //curCmd_L = 0.8;
adam_z 6:5bd08053e95c 378
adam_z 6:5bd08053e95c 379 motorCur_R.SetPWMDuty(0.75);
adam_z 6:5bd08053e95c 380
adam_z 6:5bd08053e95c 381 motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
adam_z 6:5bd08053e95c 382 motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
adam_z 6:5bd08053e95c 383 */
adam_z 6:5bd08053e95c 384
adam_z 2:53a942d1b1e5 385 }
adam_z 2:53a942d1b1e5 386
adam_z 2:53a942d1b1e5 387
adam_z 4:a2d38818c4e7 388 }
adam_z 4:a2d38818c4e7 389
adam_z 4:a2d38818c4e7 390
adam_z 7:f33a935eb77a 391 void bluetoothRx()
adam_z 6:5bd08053e95c 392 {
adam_z 8:9f4c10787775 393 while(pc.readable()) {
adam_z 8:9f4c10787775 394 char charRx = pc.getc();
adam_z 6:5bd08053e95c 395 switch(charRx) {
adam_z 6:5bd08053e95c 396 case 'w'://forward
adam_z 6:5bd08053e95c 397 velocityCmd[0] = 2.5;
adam_z 6:5bd08053e95c 398 velocityCmd[1] = 2.5;
adam_z 6:5bd08053e95c 399 accelerateFlag = 0;
adam_z 6:5bd08053e95c 400 break;
adam_z 6:5bd08053e95c 401 case 's'://backward
adam_z 6:5bd08053e95c 402 velocityCmd[0] = -3.0;
adam_z 6:5bd08053e95c 403 velocityCmd[1] = -3.0;
adam_z 6:5bd08053e95c 404 accelerateFlag = 0;
adam_z 6:5bd08053e95c 405 break;
adam_z 6:5bd08053e95c 406 case 'a'://turn left
adam_z 6:5bd08053e95c 407 velocityCmd[0] = -4.0;
adam_z 6:5bd08053e95c 408 velocityCmd[1] = 4.0;
adam_z 6:5bd08053e95c 409 accelerateFlag = 0;
adam_z 6:5bd08053e95c 410 break;
adam_z 6:5bd08053e95c 411 case 'd'://turn right
adam_z 6:5bd08053e95c 412 velocityCmd[0] = 4.0;
adam_z 6:5bd08053e95c 413 velocityCmd[1] = -4.0;
adam_z 6:5bd08053e95c 414 accelerateFlag = 0;
adam_z 6:5bd08053e95c 415 break;
adam_z 6:5bd08053e95c 416 case 'x'://stop
adam_z 6:5bd08053e95c 417 velocityCmd[0] = 0.0;
adam_z 6:5bd08053e95c 418 velocityCmd[1] = 0.0;
adam_z 6:5bd08053e95c 419 accelerateFlag = 0;
adam_z 6:5bd08053e95c 420 break;
adam_z 7:f33a935eb77a 421
adam_z 6:5bd08053e95c 422 case 'q'://accelerate
adam_z 8:9f4c10787775 423 // debugLed_l = !debugLed_l;
adam_z 6:5bd08053e95c 424 accelerateFlag = 1;
adam_z 6:5bd08053e95c 425 break;
adam_z 6:5bd08053e95c 426 }
adam_z 6:5bd08053e95c 427 }
adam_z 6:5bd08053e95c 428 }
adam_z 6:5bd08053e95c 429
adam_z 7:f33a935eb77a 430 void RXIrqLeft()
adam_z 7:f33a935eb77a 431 {
adam_z 9:a91135551be1 432
adam_z 9:a91135551be1 433
adam_z 9:a91135551be1 434 if(LeftSerial.readable()) {
adam_z 9:a91135551be1 435 debugLed_l = !debugLed_l;
adam_z 9:a91135551be1 436 uint8_t c = LeftSerial.getc();
adam_z 9:a91135551be1 437 cLast_l = c;
adam_z 9:a91135551be1 438
adam_z 9:a91135551be1 439 if( c == 0xAA) {
adam_z 7:f33a935eb77a 440 i_l = 0;
adam_z 9:a91135551be1 441 headerCaptured_l = 1;
adam_z 9:a91135551be1 442
adam_z 9:a91135551be1 443
adam_z 9:a91135551be1 444 } else if(headerCaptured_l) {
adam_z 7:f33a935eb77a 445 fromPhoton_l[i_l] = c;
adam_z 7:f33a935eb77a 446 i_l++;
adam_z 7:f33a935eb77a 447 if(i_l == 2) {
adam_z 7:f33a935eb77a 448 headerCaptured_l = 0;
adam_z 9:a91135551be1 449 slipAcceleration_L = (float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1])*0.92)/1000.0;
adam_z 9:a91135551be1 450
adam_z 9:a91135551be1 451 // pc.printf("%5.4f\t",(float)slipAcceleration_L_f);
adam_z 9:a91135551be1 452 // pc.printf("%5.4f\r\n",(float)slipAcceleration_R_f);
adam_z 9:a91135551be1 453 }
adam_z 9:a91135551be1 454
adam_z 9:a91135551be1 455 }
adam_z 9:a91135551be1 456 }
adam_z 9:a91135551be1 457
adam_z 9:a91135551be1 458
adam_z 9:a91135551be1 459 }
adam_z 9:a91135551be1 460
adam_z 9:a91135551be1 461
adam_z 9:a91135551be1 462
adam_z 9:a91135551be1 463
adam_z 9:a91135551be1 464 void RXIrqRight()
adam_z 9:a91135551be1 465 {
adam_z 9:a91135551be1 466
adam_z 9:a91135551be1 467 // slipAcceleration_L += 0.1;
adam_z 9:a91135551be1 468 if(RightSerial.readable()) {
adam_z 9:a91135551be1 469 debugLed_r = !debugLed_r;
adam_z 9:a91135551be1 470 uint8_t c = RightSerial.getc();
adam_z 9:a91135551be1 471 cLast_r = c;
adam_z 9:a91135551be1 472 // pc.printf("%5.4f\r\n",(float)cLast_r);
adam_z 9:a91135551be1 473
adam_z 9:a91135551be1 474 if( c == 0xAA) {
adam_z 9:a91135551be1 475 headerCaptured_r = 1;
adam_z 9:a91135551be1 476 i_r = 0;
adam_z 9:a91135551be1 477
adam_z 9:a91135551be1 478 } else if(headerCaptured_r) {
adam_z 9:a91135551be1 479
adam_z 9:a91135551be1 480 fromPhoton_r[i_r] = c;
adam_z 9:a91135551be1 481 i_r++;
adam_z 9:a91135551be1 482 if(i_r == 2) {
adam_z 9:a91135551be1 483 headerCaptured_r = 0;
adam_z 9:a91135551be1 484 slipAcceleration_R = -(float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1])*1.18)/1000.0;
adam_z 8:9f4c10787775 485
adam_z 7:f33a935eb77a 486 }
adam_z 7:f33a935eb77a 487 }
adam_z 7:f33a935eb77a 488 }
adam_z 7:f33a935eb77a 489 }
adam_z 7:f33a935eb77a 490
adam_z 6:5bd08053e95c 491
adam_z 6:5bd08053e95c 492
adam_z 4:a2d38818c4e7 493 float saturation(float input, float limit_H, float limit_L)
adam_z 4:a2d38818c4e7 494 {
adam_z 4:a2d38818c4e7 495 float output;
adam_z 4:a2d38818c4e7 496 if(input >= limit_H)output = limit_H;
adam_z 4:a2d38818c4e7 497 else if (input <= limit_L)output = limit_L;
adam_z 4:a2d38818c4e7 498 else output = input;
adam_z 4:a2d38818c4e7 499 return output;
adam_z 4:a2d38818c4e7 500 }
adam_z 4:a2d38818c4e7 501
adam_z 4:a2d38818c4e7 502
adam_z 8:9f4c10787775 503 //void SensorAcquisition(float * data, float samplingTime)
adam_z 8:9f4c10787775 504 //{
adam_z 8:9f4c10787775 505 //
adam_z 8:9f4c10787775 506 // axm = data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
adam_z 8:9f4c10787775 507 // aym = data[1]*(-1)*9.81;
adam_z 8:9f4c10787775 508 // azm = data[2]*(-1)*9.81;
adam_z 8:9f4c10787775 509 // u1 = data[0]*3.14159/180; //gyroscope :deg/s to rad/s
adam_z 8:9f4c10787775 510 // u2 = data[1]*3.14159/180;
adam_z 8:9f4c10787775 511 // u3 = data[2]*3.14159/180;
adam_z 8:9f4c10787775 512 //
adam_z 8:9f4c10787775 513 //
adam_z 8:9f4c10787775 514 // if(conv_init <= 3) {
adam_z 8:9f4c10787775 515 // axm_f_old = axm;
adam_z 8:9f4c10787775 516 // aym_f_old = aym;
adam_z 8:9f4c10787775 517 // azm_f_old = azm;
adam_z 8:9f4c10787775 518 //
adam_z 8:9f4c10787775 519 // conv_init++;
adam_z 8:9f4c10787775 520 // } else {
adam_z 8:9f4c10787775 521 // pitch_fusion(axm, aym,azm,u3,u2,20, samplingTime);
adam_z 8:9f4c10787775 522 // roll_fusion(axm, aym,azm,u3,u1,20, samplingTime);
adam_z 8:9f4c10787775 523 // x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
adam_z 8:9f4c10787775 524 // }
adam_z 8:9f4c10787775 525 //
adam_z 8:9f4c10787775 526 //}
adam_z 6:5bd08053e95c 527
adam_z 6:5bd08053e95c 528
adam_z 8:9f4c10787775 529 void SCEnableFunc()
adam_z 8:9f4c10787775 530 {
adam_z 9:a91135551be1 531 // debugLed_l = !debugLed_l;
adam_z 9:a91135551be1 532 accelerateFlag = 1;
adam_z 8:9f4c10787775 533 SCEnalbeIndicator = !SCEnalbeIndicator;
adam_z 4:a2d38818c4e7 534 }