TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@5:b0af0cfb678e, 2016-04-17 (annotated)

Committer:
drelliak
Date:
Sun Apr 17 00:58:06 2016 +0000
Revision:
5:b0af0cfb678e
Parent:
3:e213c44a9f6c
Child:
12:273752f540be
BRAKE TEST

Who changed what in which revision?

UserRevisionLine numberNew contents of line
drelliak 0:88faaa1afb83 1 #include "FXAS21002.h"
drelliak 0:88faaa1afb83 2 #include "FXOS8700Q.h"
drelliak 0:88faaa1afb83 3 #include "mbed.h"
drelliak 0:88faaa1afb83 4 #include "CarPWM.h"
drelliak 0:88faaa1afb83 5 #include "receiver.h"
drelliak 0:88faaa1afb83 6
drelliak 0:88faaa1afb83 7
drelliak 0:88faaa1afb83 8 #define PI 3.141592653589793238462
drelliak 0:88faaa1afb83 9 #define Ts 0.02 // Seconds
drelliak 5:b0af0cfb678e 10 #define PWM_PERIOD 13.5 // ms
drelliak 0:88faaa1afb83 11 #define INITIAL_P 0.452531214933414
drelliak 0:88faaa1afb83 12 #define INITIAL_I 5.45748932024049
drelliak 0:88faaa1afb83 13 #define INITIAL_D 0.000233453623255507
drelliak 0:88faaa1afb83 14 #define INITIAL_N 51.0605584484153
drelliak 5:b0af0cfb678e 15 #define BRAKE_CONSTANT 40
drelliak 5:b0af0cfb678e 16 #define BRAKE_WAIT 0.3
drelliak 0:88faaa1afb83 17 #define GYRO_OFFSET 0.0152
drelliak 0:88faaa1afb83 18 #define END_THRESH 4
drelliak 0:88faaa1afb83 19 #define START_THRESH 10
drelliak 0:88faaa1afb83 20 #define MINIMUM_VELOCITY 15
drelliak 5:b0af0cfb678e 21
drelliak 0:88faaa1afb83 22 Serial ser(USBTX, USBRX); // Initialize Serial port
drelliak 0:88faaa1afb83 23 PwmOut motor(PTD1); // Motor connected to pin PTD1
drelliak 0:88faaa1afb83 24 PwmOut servo(PTD3); // Servo connected to pin PTD3
drelliak 0:88faaa1afb83 25
drelliak 0:88faaa1afb83 26 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
drelliak 0:88faaa1afb83 27 FXAS21002 gyro(PTE25,PTE24);
drelliak 0:88faaa1afb83 28
drelliak 0:88faaa1afb83 29
drelliak 0:88faaa1afb83 30
drelliak 0:88faaa1afb83 31 // PID controller parameters and functions
drelliak 0:88faaa1afb83 32 float e[2], u, up[1],ui[2], ud[2]; // The vector coeficient means a time delay, for exemple e[a] = e(k-a) -> z^(-a)e(k)
drelliak 0:88faaa1afb83 33 float P, I, D, N, reference = 0;
drelliak 0:88faaa1afb83 34 void controlAnglePID(float P, float I, float D, float N);
drelliak 0:88faaa1afb83 35 void initializeController();
drelliak 0:88faaa1afb83 36
drelliak 0:88faaa1afb83 37 // Gyroscope variables and functions
drelliak 0:88faaa1afb83 38 float gyro_data[3], gyro_angle;
drelliak 0:88faaa1afb83 39 Timer t;
drelliak 0:88faaa1afb83 40 void processGyroAngle();
drelliak 0:88faaa1afb83 41 void startGyro();
drelliak 0:88faaa1afb83 42 void stopGyro();
drelliak 0:88faaa1afb83 43
drelliak 0:88faaa1afb83 44 // Magnetometer variables and functions
drelliak 0:88faaa1afb83 45 float max_x, max_y, min_x, min_y,x,y;
drelliak 0:88faaa1afb83 46 MotionSensorDataUnits mag_data;
drelliak 0:88faaa1afb83 47 float processMagAngle();
drelliak 0:88faaa1afb83 48 void magCal();
drelliak 0:88faaa1afb83 49
drelliak 0:88faaa1afb83 50 // State variables
drelliak 5:b0af0cfb678e 51 float feedback, velocity = 0;
drelliak 0:88faaa1afb83 52 void readProtocol();
drelliak 0:88faaa1afb83 53 void brakeMotor();
drelliak 5:b0af0cfb678e 54 void reverseMotor(int speed);
drelliak 5:b0af0cfb678e 55 void setVelocity(int new_velocity);
drelliak 5:b0af0cfb678e 56
drelliak 0:88faaa1afb83 57 // Test functions
drelliak 0:88faaa1afb83 58 void debug();
drelliak 0:88faaa1afb83 59
drelliak 0:88faaa1afb83 60 int main(){
drelliak 5:b0af0cfb678e 61 int teste = 0;
drelliak 5:b0af0cfb678e 62 setVelocity(0);
drelliak 5:b0af0cfb678e 63 switch(teste){
drelliak 5:b0af0cfb678e 64 case 0: // vai para tras duas vezes
drelliak 5:b0af0cfb678e 65 brakeMotor();
drelliak 5:b0af0cfb678e 66 wait(1);
drelliak 5:b0af0cfb678e 67 setVelocity(-1);
drelliak 5:b0af0cfb678e 68 setVelocity(0);
drelliak 5:b0af0cfb678e 69 setVelocity(-30);
drelliak 5:b0af0cfb678e 70 wait(2);
drelliak 5:b0af0cfb678e 71 brakeMotor();
drelliak 5:b0af0cfb678e 72 wait(2);
drelliak 5:b0af0cfb678e 73 setVelocity(-1);
drelliak 5:b0af0cfb678e 74 setVelocity(0);
drelliak 5:b0af0cfb678e 75 setVelocity(-30);
drelliak 5:b0af0cfb678e 76 break;
drelliak 5:b0af0cfb678e 77 case 1: // vai para a frente e depois para tras
drelliak 5:b0af0cfb678e 78 brakeMotor();
drelliak 5:b0af0cfb678e 79 wait(1);
drelliak 5:b0af0cfb678e 80 setVelocity(20);
drelliak 5:b0af0cfb678e 81 wait(2);
drelliak 5:b0af0cfb678e 82 brakeMotor();
drelliak 5:b0af0cfb678e 83 wait(2);
drelliak 5:b0af0cfb678e 84 setVelocity(-1);
drelliak 5:b0af0cfb678e 85 setVelocity(0);
drelliak 5:b0af0cfb678e 86 setVelocity(-30);
drelliak 5:b0af0cfb678e 87 break;
drelliak 5:b0af0cfb678e 88 case 2: // vai para tras e depois para frente
drelliak 5:b0af0cfb678e 89 brakeMotor();
drelliak 5:b0af0cfb678e 90 wait(1);
drelliak 5:b0af0cfb678e 91 setVelocity(-1);
drelliak 5:b0af0cfb678e 92 setVelocity(0);
drelliak 5:b0af0cfb678e 93 setVelocity(-30);
drelliak 5:b0af0cfb678e 94 wait(2);
drelliak 5:b0af0cfb678e 95 brakeMotor();
drelliak 5:b0af0cfb678e 96 wait(2);
drelliak 5:b0af0cfb678e 97 setVelocity(20);
drelliak 5:b0af0cfb678e 98 break;
drelliak 5:b0af0cfb678e 99 case 3: // vai para frente duas vezes
drelliak 5:b0af0cfb678e 100 brakeMotor();
drelliak 5:b0af0cfb678e 101 wait(1);
drelliak 5:b0af0cfb678e 102 setVelocity(20);
drelliak 5:b0af0cfb678e 103 wait(2);
drelliak 5:b0af0cfb678e 104 brakeMotor();
drelliak 5:b0af0cfb678e 105 wait(2);
drelliak 5:b0af0cfb678e 106 setVelocity(20);
drelliak 5:b0af0cfb678e 107 break;
drelliak 5:b0af0cfb678e 108 }
drelliak 5:b0af0cfb678e 109 while(1);
drelliak 0:88faaa1afb83 110 }
drelliak 0:88faaa1afb83 111 void readProtocol(){
drelliak 0:88faaa1afb83 112 char msg = ser.getc();
drelliak 0:88faaa1afb83 113 switch(msg)
drelliak 0:88faaa1afb83 114 {
drelliak 0:88faaa1afb83 115 case NONE:
drelliak 0:88faaa1afb83 116 //ser.printf("sending red signal to led\r\n");
drelliak 0:88faaa1afb83 117 return;
drelliak 0:88faaa1afb83 118 break;
drelliak 0:88faaa1afb83 119 case BRAKE:
drelliak 0:88faaa1afb83 120 //ser.printf("sending green signal to led\r\n");
drelliak 0:88faaa1afb83 121 brakeMotor();
drelliak 0:88faaa1afb83 122 break;
drelliak 0:88faaa1afb83 123 case ANG_RST:
drelliak 0:88faaa1afb83 124 //ser.printf("sending blue signal to led\r\n");
drelliak 0:88faaa1afb83 125 stopGyro();
drelliak 0:88faaa1afb83 126 gyro_angle = 0;
drelliak 0:88faaa1afb83 127 startGyro();
drelliak 0:88faaa1afb83 128 return;
drelliak 0:88faaa1afb83 129 break;
drelliak 0:88faaa1afb83 130 case ANG_REF:
drelliak 0:88faaa1afb83 131 reference = get_ang_ref(ser);
drelliak 0:88faaa1afb83 132 break;
drelliak 0:88faaa1afb83 133 case GND_SPEED:
drelliak 0:88faaa1afb83 134 velocity = get_gnd_speed(ser);
drelliak 0:88faaa1afb83 135 setMotorPWM(velocity,motor);
drelliak 0:88faaa1afb83 136 break;
drelliak 0:88faaa1afb83 137 case PID_PARAMS:
drelliak 0:88faaa1afb83 138 ser.putc('p');
drelliak 0:88faaa1afb83 139 get_pid_params(ser, &P, &I, &D, &N);
drelliak 0:88faaa1afb83 140 break;
drelliak 0:88faaa1afb83 141 default:
drelliak 0:88faaa1afb83 142 // ser.flush();
drelliak 0:88faaa1afb83 143
drelliak 0:88faaa1afb83 144 }
drelliak 0:88faaa1afb83 145 }
drelliak 0:88faaa1afb83 146 /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */
drelliak 0:88faaa1afb83 147 void initializeController(){
drelliak 0:88faaa1afb83 148 for(int i =0; i<2; i++){
drelliak 0:88faaa1afb83 149 e[i] = 0;
drelliak 0:88faaa1afb83 150 ui[i] = 0;
drelliak 0:88faaa1afb83 151 ud[i] = 0;
drelliak 0:88faaa1afb83 152 }
drelliak 0:88faaa1afb83 153 P= INITIAL_P;
drelliak 0:88faaa1afb83 154 I= INITIAL_I;
drelliak 0:88faaa1afb83 155 D= INITIAL_D;
drelliak 0:88faaa1afb83 156 N= INITIAL_N;
drelliak 0:88faaa1afb83 157 }
drelliak 0:88faaa1afb83 158
drelliak 0:88faaa1afb83 159 /* Start the Gyroscope timer and set the initial configuration */
drelliak 0:88faaa1afb83 160 void startGyro(){
drelliak 0:88faaa1afb83 161 gyro.gyro_config();
drelliak 0:88faaa1afb83 162 t.start();
drelliak 0:88faaa1afb83 163 }
drelliak 0:88faaa1afb83 164
drelliak 0:88faaa1afb83 165 /* Stop and reset the Gyroscope */
drelliak 0:88faaa1afb83 166 void stopGyro(){
drelliak 0:88faaa1afb83 167 t.stop();
drelliak 0:88faaa1afb83 168 t.reset();
drelliak 0:88faaa1afb83 169 gyro_angle = 0;
drelliak 0:88faaa1afb83 170 }
drelliak 0:88faaa1afb83 171
drelliak 0:88faaa1afb83 172 /* Integrate the Gyroscope to get the angular position (Deegre/seconds) */
drelliak 0:88faaa1afb83 173 void processGyroAngle(){
drelliak 0:88faaa1afb83 174 gyro.acquire_gyro_data_dps(gyro_data);
drelliak 0:88faaa1afb83 175 t.stop();
drelliak 0:88faaa1afb83 176 gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
drelliak 0:88faaa1afb83 177 t.reset();
drelliak 0:88faaa1afb83 178 t.start();
drelliak 5:b0af0cfb678e 179 feedback = gyro_angle;
drelliak 5:b0af0cfb678e 180 if(feedback > 180)
drelliak 5:b0af0cfb678e 181 feedback = feedback - 360;
drelliak 5:b0af0cfb678e 182 if(feedback < -180)
drelliak 5:b0af0cfb678e 183 feedback = feedback + 360;
drelliak 0:88faaa1afb83 184 }
drelliak 0:88faaa1afb83 185
drelliak 0:88faaa1afb83 186 /* PID controller for angular position */
drelliak 0:88faaa1afb83 187 void controlAnglePID(float P, float I, float D, float N){
drelliak 0:88faaa1afb83 188 /* Getting error */
drelliak 0:88faaa1afb83 189 e[1] = e[0];
drelliak 5:b0af0cfb678e 190 e[0] = reference - (feedback*PI/180);
drelliak 0:88faaa1afb83 191 if(e[0] > PI)
drelliak 0:88faaa1afb83 192 e[0]= e[0] - 2*PI;
drelliak 0:88faaa1afb83 193 if(e[0] < -PI)
drelliak 0:88faaa1afb83 194 e[0] = e[0] + 2*PI;
drelliak 0:88faaa1afb83 195 /* Proportinal Part */
drelliak 0:88faaa1afb83 196 up[0] = e[0]*P;
drelliak 0:88faaa1afb83 197 /* Integral Part */
drelliak 0:88faaa1afb83 198 ui[1] = ui[0];
drelliak 0:88faaa1afb83 199 if(abs(u) < PI/8){
drelliak 0:88faaa1afb83 200 ui[0] = (P*I*Ts)*e[1] + ui[1];
drelliak 0:88faaa1afb83 201 }
drelliak 0:88faaa1afb83 202 else if(u > 0)
drelliak 0:88faaa1afb83 203 ui[0] = PI/8 - up[0];
drelliak 0:88faaa1afb83 204 else if(u < 0)
drelliak 0:88faaa1afb83 205 ui[0] = -PI/8 - up[0];
drelliak 0:88faaa1afb83 206 /* Derivative Part */
drelliak 0:88faaa1afb83 207 ud[1] = ud[0];
drelliak 0:88faaa1afb83 208 ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
drelliak 0:88faaa1afb83 209 /** Controller **/
drelliak 0:88faaa1afb83 210 u = up[0] + ud[0] + ui[0];
drelliak 0:88faaa1afb83 211 setServoPWM(u*100/(PI/8), servo);
drelliak 0:88faaa1afb83 212 }
drelliak 0:88faaa1afb83 213 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
drelliak 0:88faaa1afb83 214 void brakeMotor(){
drelliak 5:b0af0cfb678e 215 if(velocity >= 0){
drelliak 3:e213c44a9f6c 216 setMotorPWM(-BRAKE_CONSTANT, motor);
drelliak 5:b0af0cfb678e 217 wait(BRAKE_WAIT);
drelliak 5:b0af0cfb678e 218 velocity = 0;
drelliak 5:b0af0cfb678e 219 setMotorPWM(velocity,motor);
drelliak 5:b0af0cfb678e 220 }
drelliak 5:b0af0cfb678e 221 else {
drelliak 5:b0af0cfb678e 222 setVelocity(0);
drelliak 5:b0af0cfb678e 223 }
drelliak 5:b0af0cfb678e 224 }
drelliak 5:b0af0cfb678e 225 void reverseMotor(int speed){
drelliak 5:b0af0cfb678e 226 for(int i=0 ; i >= -speed; i--){
drelliak 5:b0af0cfb678e 227 setMotorPWM((float)i,motor);
drelliak 5:b0af0cfb678e 228 wait_ms(13.5);
drelliak 5:b0af0cfb678e 229 }
drelliak 5:b0af0cfb678e 230 for(int i=-speed ; i <= 0; i++){
drelliak 5:b0af0cfb678e 231 setMotorPWM((float)i,motor);
drelliak 5:b0af0cfb678e 232 wait_ms(13.5);
drelliak 5:b0af0cfb678e 233 }
drelliak 5:b0af0cfb678e 234 for(int i=0 ; i >= -speed; i--){
drelliak 5:b0af0cfb678e 235 setMotorPWM((float)i,motor);
drelliak 5:b0af0cfb678e 236 wait_ms(13.5);
drelliak 5:b0af0cfb678e 237 }
drelliak 5:b0af0cfb678e 238 }
drelliak 5:b0af0cfb678e 239 void setVelocity(int new_velocity){
drelliak 5:b0af0cfb678e 240 if( velocity > new_velocity){
drelliak 5:b0af0cfb678e 241 for(; velocity >= new_velocity; velocity--){
drelliak 5:b0af0cfb678e 242 setMotorPWM(velocity,motor);
drelliak 5:b0af0cfb678e 243 wait_ms(PWM_PERIOD);
drelliak 5:b0af0cfb678e 244 }
drelliak 5:b0af0cfb678e 245 velocity++;
drelliak 5:b0af0cfb678e 246 }
drelliak 5:b0af0cfb678e 247 else if(velocity < new_velocity){
drelliak 5:b0af0cfb678e 248 for(; velocity <= new_velocity; velocity++){
drelliak 5:b0af0cfb678e 249 setMotorPWM(velocity,motor);
drelliak 5:b0af0cfb678e 250 wait_ms(PWM_PERIOD);
drelliak 5:b0af0cfb678e 251 }
drelliak 5:b0af0cfb678e 252 velocity--;
drelliak 5:b0af0cfb678e 253 }
drelliak 0:88faaa1afb83 254 }
drelliak 0:88faaa1afb83 255 /* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used */
drelliak 0:88faaa1afb83 256 /* in the main loop or the controller performance may be affected. */
drelliak 0:88faaa1afb83 257 void debug(){
drelliak 0:88faaa1afb83 258 //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8));
drelliak 0:88faaa1afb83 259 //printf("Erro: %f Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI);
drelliak 5:b0af0cfb678e 260 printf(" %f \r\n",feedback);
drelliak 0:88faaa1afb83 261 }
drelliak 0:88faaa1afb83 262
drelliak 0:88faaa1afb83 263 /* Function to normalize the magnetometer reading */
drelliak 0:88faaa1afb83 264 void magCal(){
drelliak 0:88faaa1afb83 265 printf("Starting Calibration");
drelliak 0:88faaa1afb83 266 mag.enable();
drelliak 0:88faaa1afb83 267 wait(0.01);
drelliak 0:88faaa1afb83 268 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 269 float x0 = max_x = min_y = mag_data.x;
drelliak 0:88faaa1afb83 270 float y0 = max_y = min_y = mag_data.y;
drelliak 0:88faaa1afb83 271 bool began = false;
drelliak 0:88faaa1afb83 272 while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
drelliak 0:88faaa1afb83 273 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 274 if(mag_data.x > max_x)
drelliak 0:88faaa1afb83 275 max_x = mag_data.x;
drelliak 0:88faaa1afb83 276 if(mag_data.y > max_y)
drelliak 0:88faaa1afb83 277 max_y = mag_data.y;
drelliak 0:88faaa1afb83 278 if(mag_data.y < min_y)
drelliak 0:88faaa1afb83 279 min_y = mag_data.y;
drelliak 0:88faaa1afb83 280 if(mag_data.x < min_x)
drelliak 0:88faaa1afb83 281 min_x = mag_data.x;
drelliak 0:88faaa1afb83 282 if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
drelliak 0:88faaa1afb83 283 began = true;
drelliak 0:88faaa1afb83 284 printf("began: %d X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x-x0),abs(mag_data.y-y0));
drelliak 0:88faaa1afb83 285 }
drelliak 0:88faaa1afb83 286 printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r",max_x,max_y,min_x,min_y);
drelliak 0:88faaa1afb83 287 }
drelliak 0:88faaa1afb83 288
drelliak 0:88faaa1afb83 289 /* Function to transform the magnetometer reading in angle(rad/s).*/
drelliak 0:88faaa1afb83 290 float processMagAngle(){
drelliak 0:88faaa1afb83 291 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 292 x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
drelliak 0:88faaa1afb83 293 y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
drelliak 0:88faaa1afb83 294 return atan2(y,x);
drelliak 0:88faaa1afb83 295 }