TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@12:273752f540be, 2016-04-30 (annotated)

Committer:
drelliak
Date:
Sat Apr 30 21:23:13 2016 +0000
Revision:
12:273752f540be
Parent:
5:b0af0cfb678e
Child:
13:f7a7fe9b5c00
TrekkingController with the gyroscope class and motor class

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 12:273752f540be 6 #include "Motor.h"
drelliak 0:88faaa1afb83 7
drelliak 0:88faaa1afb83 8 #define PI 3.141592653589793238462
drelliak 0:88faaa1afb83 9 #define Ts 0.02 // Seconds
drelliak 12:273752f540be 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 END_THRESH 4
drelliak 0:88faaa1afb83 18 #define START_THRESH 10
drelliak 0:88faaa1afb83 19 #define MINIMUM_VELOCITY 15
drelliak 12:273752f540be 20 #define GYRO_PERIOD 1300 //us
drelliak 5:b0af0cfb678e 21
drelliak 0:88faaa1afb83 22 Serial ser(USBTX, USBRX); // Initialize Serial port
drelliak 0:88faaa1afb83 23 PwmOut servo(PTD3); // Servo connected to pin PTD3
drelliak 0:88faaa1afb83 24
drelliak 0:88faaa1afb83 25 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
drelliak 0:88faaa1afb83 26 FXAS21002 gyro(PTE25,PTE24);
drelliak 0:88faaa1afb83 27
drelliak 0:88faaa1afb83 28
drelliak 0:88faaa1afb83 29
drelliak 0:88faaa1afb83 30 // PID controller parameters and functions
drelliak 0:88faaa1afb83 31 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 32 float P, I, D, N, reference = 0;
drelliak 0:88faaa1afb83 33 void controlAnglePID(float P, float I, float D, float N);
drelliak 0:88faaa1afb83 34 void initializeController();
drelliak 0:88faaa1afb83 35
drelliak 0:88faaa1afb83 36 // Magnetometer variables and functions
drelliak 0:88faaa1afb83 37 float max_x, max_y, min_x, min_y,x,y;
drelliak 0:88faaa1afb83 38 MotionSensorDataUnits mag_data;
drelliak 0:88faaa1afb83 39 float processMagAngle();
drelliak 0:88faaa1afb83 40 void magCal();
drelliak 0:88faaa1afb83 41
drelliak 0:88faaa1afb83 42 // State variables
drelliak 12:273752f540be 43 float velocity = 0;
drelliak 0:88faaa1afb83 44 void readProtocol();
drelliak 0:88faaa1afb83 45 void brakeMotor();
drelliak 5:b0af0cfb678e 46 void reverseMotor(int speed);
drelliak 5:b0af0cfb678e 47 void setVelocity(int new_velocity);
drelliak 5:b0af0cfb678e 48
drelliak 0:88faaa1afb83 49 int main(){
drelliak 12:273752f540be 50 gyro.gyro_config(MODE_2);
drelliak 12:273752f540be 51 gyro.start_measure(GYRO_PERIOD);
drelliak 12:273752f540be 52 initializeController();
drelliak 12:273752f540be 53 while(1){
drelliak 12:273752f540be 54 controlAnglePID(P,I,D,N);
drelliak 12:273752f540be 55 printf("%f \r\n",gyro.get_angle());
drelliak 12:273752f540be 56 wait(Ts);
drelliak 5:b0af0cfb678e 57 }
drelliak 0:88faaa1afb83 58 }
drelliak 0:88faaa1afb83 59 void readProtocol(){
drelliak 0:88faaa1afb83 60 char msg = ser.getc();
drelliak 0:88faaa1afb83 61 switch(msg)
drelliak 0:88faaa1afb83 62 {
drelliak 0:88faaa1afb83 63 case NONE:
drelliak 0:88faaa1afb83 64 //ser.printf("sending red signal to led\r\n");
drelliak 0:88faaa1afb83 65 return;
drelliak 0:88faaa1afb83 66 break;
drelliak 0:88faaa1afb83 67 case BRAKE:
drelliak 0:88faaa1afb83 68 //ser.printf("sending green signal to led\r\n");
drelliak 0:88faaa1afb83 69 brakeMotor();
drelliak 0:88faaa1afb83 70 break;
drelliak 0:88faaa1afb83 71 case ANG_RST:
drelliak 0:88faaa1afb83 72 //ser.printf("sending blue signal to led\r\n");
drelliak 12:273752f540be 73 gyro.stop_measure();
drelliak 12:273752f540be 74 gyro.start_measure(GYRO_PERIOD);
drelliak 0:88faaa1afb83 75 return;
drelliak 0:88faaa1afb83 76 break;
drelliak 0:88faaa1afb83 77 case ANG_REF:
drelliak 0:88faaa1afb83 78 reference = get_ang_ref(ser);
drelliak 0:88faaa1afb83 79 break;
drelliak 0:88faaa1afb83 80 case GND_SPEED:
drelliak 0:88faaa1afb83 81 velocity = get_gnd_speed(ser);
drelliak 0:88faaa1afb83 82 setMotorPWM(velocity,motor);
drelliak 0:88faaa1afb83 83 break;
drelliak 0:88faaa1afb83 84 case PID_PARAMS:
drelliak 0:88faaa1afb83 85 ser.putc('p');
drelliak 0:88faaa1afb83 86 get_pid_params(ser, &P, &I, &D, &N);
drelliak 0:88faaa1afb83 87 break;
drelliak 0:88faaa1afb83 88 default:
drelliak 0:88faaa1afb83 89 // ser.flush();
drelliak 0:88faaa1afb83 90
drelliak 0:88faaa1afb83 91 }
drelliak 0:88faaa1afb83 92 }
drelliak 0:88faaa1afb83 93 /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */
drelliak 0:88faaa1afb83 94 void initializeController(){
drelliak 0:88faaa1afb83 95 for(int i =0; i<2; i++){
drelliak 0:88faaa1afb83 96 e[i] = 0;
drelliak 0:88faaa1afb83 97 ui[i] = 0;
drelliak 0:88faaa1afb83 98 ud[i] = 0;
drelliak 0:88faaa1afb83 99 }
drelliak 0:88faaa1afb83 100 P= INITIAL_P;
drelliak 0:88faaa1afb83 101 I= INITIAL_I;
drelliak 0:88faaa1afb83 102 D= INITIAL_D;
drelliak 0:88faaa1afb83 103 N= INITIAL_N;
drelliak 0:88faaa1afb83 104 }
drelliak 0:88faaa1afb83 105
drelliak 0:88faaa1afb83 106 /* PID controller for angular position */
drelliak 0:88faaa1afb83 107 void controlAnglePID(float P, float I, float D, float N){
drelliak 0:88faaa1afb83 108 /* Getting error */
drelliak 12:273752f540be 109 float feedback = gyro.get_angle();
drelliak 0:88faaa1afb83 110 e[1] = e[0];
drelliak 5:b0af0cfb678e 111 e[0] = reference - (feedback*PI/180);
drelliak 0:88faaa1afb83 112 if(e[0] > PI)
drelliak 0:88faaa1afb83 113 e[0]= e[0] - 2*PI;
drelliak 0:88faaa1afb83 114 if(e[0] < -PI)
drelliak 0:88faaa1afb83 115 e[0] = e[0] + 2*PI;
drelliak 0:88faaa1afb83 116 /* Proportinal Part */
drelliak 0:88faaa1afb83 117 up[0] = e[0]*P;
drelliak 0:88faaa1afb83 118 /* Integral Part */
drelliak 0:88faaa1afb83 119 ui[1] = ui[0];
drelliak 0:88faaa1afb83 120 if(abs(u) < PI/8){
drelliak 0:88faaa1afb83 121 ui[0] = (P*I*Ts)*e[1] + ui[1];
drelliak 0:88faaa1afb83 122 }
drelliak 0:88faaa1afb83 123 else if(u > 0)
drelliak 0:88faaa1afb83 124 ui[0] = PI/8 - up[0];
drelliak 0:88faaa1afb83 125 else if(u < 0)
drelliak 0:88faaa1afb83 126 ui[0] = -PI/8 - up[0];
drelliak 0:88faaa1afb83 127 /* Derivative Part */
drelliak 0:88faaa1afb83 128 ud[1] = ud[0];
drelliak 0:88faaa1afb83 129 ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
drelliak 0:88faaa1afb83 130 /** Controller **/
drelliak 0:88faaa1afb83 131 u = up[0] + ud[0] + ui[0];
drelliak 0:88faaa1afb83 132 setServoPWM(u*100/(PI/8), servo);
drelliak 0:88faaa1afb83 133 }
drelliak 0:88faaa1afb83 134 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
drelliak 0:88faaa1afb83 135 void brakeMotor(){
drelliak 5:b0af0cfb678e 136 if(velocity >= 0){
drelliak 3:e213c44a9f6c 137 setMotorPWM(-BRAKE_CONSTANT, motor);
drelliak 5:b0af0cfb678e 138 wait(BRAKE_WAIT);
drelliak 5:b0af0cfb678e 139 velocity = 0;
drelliak 5:b0af0cfb678e 140 setMotorPWM(velocity,motor);
drelliak 5:b0af0cfb678e 141 }
drelliak 5:b0af0cfb678e 142 else {
drelliak 5:b0af0cfb678e 143 setVelocity(0);
drelliak 5:b0af0cfb678e 144 }
drelliak 5:b0af0cfb678e 145 }
drelliak 5:b0af0cfb678e 146 void reverseMotor(int speed){
drelliak 5:b0af0cfb678e 147 for(int i=0 ; i >= -speed; i--){
drelliak 5:b0af0cfb678e 148 setMotorPWM((float)i,motor);
drelliak 5:b0af0cfb678e 149 wait_ms(13.5);
drelliak 5:b0af0cfb678e 150 }
drelliak 5:b0af0cfb678e 151 for(int i=-speed ; i <= 0; i++){
drelliak 5:b0af0cfb678e 152 setMotorPWM((float)i,motor);
drelliak 5:b0af0cfb678e 153 wait_ms(13.5);
drelliak 5:b0af0cfb678e 154 }
drelliak 5:b0af0cfb678e 155 for(int i=0 ; i >= -speed; i--){
drelliak 5:b0af0cfb678e 156 setMotorPWM((float)i,motor);
drelliak 5:b0af0cfb678e 157 wait_ms(13.5);
drelliak 5:b0af0cfb678e 158 }
drelliak 5:b0af0cfb678e 159 }
drelliak 5:b0af0cfb678e 160 void setVelocity(int new_velocity){
drelliak 5:b0af0cfb678e 161 if( velocity > new_velocity){
drelliak 5:b0af0cfb678e 162 for(; velocity >= new_velocity; velocity--){
drelliak 5:b0af0cfb678e 163 setMotorPWM(velocity,motor);
drelliak 5:b0af0cfb678e 164 wait_ms(PWM_PERIOD);
drelliak 5:b0af0cfb678e 165 }
drelliak 5:b0af0cfb678e 166 velocity++;
drelliak 5:b0af0cfb678e 167 }
drelliak 5:b0af0cfb678e 168 else if(velocity < new_velocity){
drelliak 5:b0af0cfb678e 169 for(; velocity <= new_velocity; velocity++){
drelliak 5:b0af0cfb678e 170 setMotorPWM(velocity,motor);
drelliak 5:b0af0cfb678e 171 wait_ms(PWM_PERIOD);
drelliak 5:b0af0cfb678e 172 }
drelliak 5:b0af0cfb678e 173 velocity--;
drelliak 5:b0af0cfb678e 174 }
drelliak 0:88faaa1afb83 175 }
drelliak 0:88faaa1afb83 176 /* Function to normalize the magnetometer reading */
drelliak 0:88faaa1afb83 177 void magCal(){
drelliak 0:88faaa1afb83 178 printf("Starting Calibration");
drelliak 0:88faaa1afb83 179 mag.enable();
drelliak 0:88faaa1afb83 180 wait(0.01);
drelliak 0:88faaa1afb83 181 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 182 float x0 = max_x = min_y = mag_data.x;
drelliak 0:88faaa1afb83 183 float y0 = max_y = min_y = mag_data.y;
drelliak 0:88faaa1afb83 184 bool began = false;
drelliak 0:88faaa1afb83 185 while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
drelliak 0:88faaa1afb83 186 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 187 if(mag_data.x > max_x)
drelliak 0:88faaa1afb83 188 max_x = mag_data.x;
drelliak 0:88faaa1afb83 189 if(mag_data.y > max_y)
drelliak 0:88faaa1afb83 190 max_y = mag_data.y;
drelliak 0:88faaa1afb83 191 if(mag_data.y < min_y)
drelliak 0:88faaa1afb83 192 min_y = mag_data.y;
drelliak 0:88faaa1afb83 193 if(mag_data.x < min_x)
drelliak 0:88faaa1afb83 194 min_x = mag_data.x;
drelliak 0:88faaa1afb83 195 if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
drelliak 0:88faaa1afb83 196 began = true;
drelliak 0:88faaa1afb83 197 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 198 }
drelliak 0:88faaa1afb83 199 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 200 }
drelliak 0:88faaa1afb83 201
drelliak 0:88faaa1afb83 202 /* Function to transform the magnetometer reading in angle(rad/s).*/
drelliak 0:88faaa1afb83 203 float processMagAngle(){
drelliak 0:88faaa1afb83 204 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 205 x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
drelliak 0:88faaa1afb83 206 y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
drelliak 0:88faaa1afb83 207 return atan2(y,x);
drelliak 0:88faaa1afb83 208 }