TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@16:a9e0eb97557f, 2016-05-02 (annotated)

Committer:
drelliak
Date:
Mon May 02 00:57:58 2016 +0000
Revision:
16:a9e0eb97557f
Parent:
14:e8cd237c8639
Child:
18:c1cd11db47ed
Added the magnetometer command

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 14:e8cd237c8639 20 #define GYRO_PERIOD 5000 //us
drelliak 14:e8cd237c8639 21 #define LED_ON 0
drelliak 14:e8cd237c8639 22 #define LED_OFF 1
drelliak 14:e8cd237c8639 23
drelliak 14:e8cd237c8639 24 #define MIN -1.5
drelliak 14:e8cd237c8639 25 #define MAX 1.5
drelliak 14:e8cd237c8639 26
drelliak 14:e8cd237c8639 27 enum{
drelliak 14:e8cd237c8639 28 BLACK,
drelliak 14:e8cd237c8639 29 RED,
drelliak 14:e8cd237c8639 30 GREEN,
drelliak 14:e8cd237c8639 31 BLUE,
drelliak 14:e8cd237c8639 32 PURPLE,
drelliak 14:e8cd237c8639 33 YELLOW,
drelliak 14:e8cd237c8639 34 AQUA,
drelliak 14:e8cd237c8639 35 WHITE};
drelliak 14:e8cd237c8639 36
drelliak 14:e8cd237c8639 37 void turn_leds_off(DigitalOut& red, DigitalOut& green, DigitalOut& blue)
drelliak 14:e8cd237c8639 38 {
drelliak 14:e8cd237c8639 39 red = LED_OFF;
drelliak 14:e8cd237c8639 40 green = LED_OFF;
drelliak 14:e8cd237c8639 41 blue = LED_OFF;
drelliak 14:e8cd237c8639 42 }
drelliak 14:e8cd237c8639 43
drelliak 14:e8cd237c8639 44 void set_leds_color(int color, DigitalOut& red, DigitalOut& green, DigitalOut& blue)
drelliak 14:e8cd237c8639 45 {
drelliak 14:e8cd237c8639 46 turn_leds_off(red, green, blue);
drelliak 14:e8cd237c8639 47
drelliak 14:e8cd237c8639 48 switch(color)
drelliak 14:e8cd237c8639 49 {
drelliak 14:e8cd237c8639 50 case RED:
drelliak 14:e8cd237c8639 51 red = LED_ON;
drelliak 14:e8cd237c8639 52 break;
drelliak 14:e8cd237c8639 53 case GREEN:
drelliak 14:e8cd237c8639 54 green = LED_ON;
drelliak 14:e8cd237c8639 55 break;
drelliak 14:e8cd237c8639 56 case BLUE:
drelliak 14:e8cd237c8639 57 blue = LED_ON;
drelliak 14:e8cd237c8639 58 break;
drelliak 14:e8cd237c8639 59 case PURPLE:
drelliak 14:e8cd237c8639 60 red = LED_ON;
drelliak 14:e8cd237c8639 61 blue = LED_ON;
drelliak 14:e8cd237c8639 62 break;
drelliak 14:e8cd237c8639 63 case YELLOW:
drelliak 14:e8cd237c8639 64 red = LED_ON;
drelliak 14:e8cd237c8639 65 green = LED_ON;
drelliak 14:e8cd237c8639 66 break;
drelliak 14:e8cd237c8639 67 case AQUA:
drelliak 14:e8cd237c8639 68 blue = LED_ON;
drelliak 14:e8cd237c8639 69 green = LED_ON;
drelliak 14:e8cd237c8639 70 break;
drelliak 14:e8cd237c8639 71 case WHITE:
drelliak 14:e8cd237c8639 72 red = LED_ON;
drelliak 14:e8cd237c8639 73 green = LED_ON;
drelliak 14:e8cd237c8639 74 blue = LED_ON;
drelliak 14:e8cd237c8639 75 break;
drelliak 14:e8cd237c8639 76 default:
drelliak 14:e8cd237c8639 77 break;
drelliak 14:e8cd237c8639 78 }
drelliak 14:e8cd237c8639 79 }
drelliak 5:b0af0cfb678e 80
drelliak 0:88faaa1afb83 81 Serial ser(USBTX, USBRX); // Initialize Serial port
drelliak 0:88faaa1afb83 82 PwmOut servo(PTD3); // Servo connected to pin PTD3
drelliak 13:f7a7fe9b5c00 83 Motor motor;
drelliak 0:88faaa1afb83 84 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
drelliak 14:e8cd237c8639 85 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
drelliak 0:88faaa1afb83 86 FXAS21002 gyro(PTE25,PTE24);
drelliak 14:e8cd237c8639 87 DigitalOut red_led(LED_RED);
drelliak 14:e8cd237c8639 88 DigitalOut green_led(LED_GREEN);
drelliak 14:e8cd237c8639 89 DigitalOut blue_led(LED_BLUE);
drelliak 14:e8cd237c8639 90 Receiver rcv;
drelliak 14:e8cd237c8639 91 EthernetInterface eth;
drelliak 0:88faaa1afb83 92
drelliak 0:88faaa1afb83 93 // PID controller parameters and functions
drelliak 0:88faaa1afb83 94 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 95 float P, I, D, N, reference = 0;
drelliak 0:88faaa1afb83 96 void controlAnglePID(float P, float I, float D, float N);
drelliak 0:88faaa1afb83 97 void initializeController();
drelliak 14:e8cd237c8639 98 void control();
drelliak 14:e8cd237c8639 99 Ticker controller_ticker;
drelliak 0:88faaa1afb83 100
drelliak 0:88faaa1afb83 101 // Magnetometer variables and functions
drelliak 0:88faaa1afb83 102 float max_x, max_y, min_x, min_y,x,y;
drelliak 0:88faaa1afb83 103 MotionSensorDataUnits mag_data;
drelliak 14:e8cd237c8639 104 MotionSensorDataCounts mag_raw;
drelliak 0:88faaa1afb83 105 float processMagAngle();
drelliak 0:88faaa1afb83 106 void magCal();
drelliak 0:88faaa1afb83 107
drelliak 14:e8cd237c8639 108 // Protocol
drelliak 14:e8cd237c8639 109 void readProtocol();
drelliak 14:e8cd237c8639 110
drelliak 0:88faaa1afb83 111 int main(){
drelliak 14:e8cd237c8639 112 // Initializing sensors:
drelliak 14:e8cd237c8639 113 acc.enable();
drelliak 14:e8cd237c8639 114 gyro.gyro_config(MODE_1);
drelliak 12:273752f540be 115 initializeController();
drelliak 14:e8cd237c8639 116
drelliak 14:e8cd237c8639 117 // Set initial control configurations
drelliak 14:e8cd237c8639 118 motor.setVelocity(0);
drelliak 14:e8cd237c8639 119
drelliak 14:e8cd237c8639 120 // Protocol parameters
drelliak 14:e8cd237c8639 121 set_leds_color(RED, red_led, green_led, blue_led);
drelliak 14:e8cd237c8639 122 eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
drelliak 14:e8cd237c8639 123 eth.connect();
drelliak 14:e8cd237c8639 124 set_leds_color(BLUE, red_led, green_led, blue_led);
drelliak 14:e8cd237c8639 125 rcv.set_socket();
drelliak 14:e8cd237c8639 126
drelliak 14:e8cd237c8639 127 gyro.start_measure(GYRO_PERIOD);
drelliak 14:e8cd237c8639 128 controller_ticker.attach(&control,Ts);
drelliak 14:e8cd237c8639 129 //main loop
drelliak 12:273752f540be 130 while(1){
drelliak 14:e8cd237c8639 131 readProtocol();
drelliak 14:e8cd237c8639 132 wait(0.01);
drelliak 5:b0af0cfb678e 133 }
drelliak 0:88faaa1afb83 134 }
drelliak 14:e8cd237c8639 135 void control(){
drelliak 14:e8cd237c8639 136 controlAnglePID(P,I,D,N);
drelliak 14:e8cd237c8639 137 }
drelliak 0:88faaa1afb83 138 void readProtocol(){
drelliak 14:e8cd237c8639 139 if(!rcv.receive())
drelliak 14:e8cd237c8639 140 return;
drelliak 14:e8cd237c8639 141 char msg = rcv.get_msg();
drelliak 0:88faaa1afb83 142 switch(msg)
drelliak 0:88faaa1afb83 143 {
drelliak 0:88faaa1afb83 144 case NONE:
drelliak 0:88faaa1afb83 145 //ser.printf("sending red signal to led\r\n");
drelliak 14:e8cd237c8639 146 red_led = LED_ON;
drelliak 14:e8cd237c8639 147
drelliak 14:e8cd237c8639 148 printf("NONE\r\n");
drelliak 14:e8cd237c8639 149
drelliak 14:e8cd237c8639 150 wait(1);
drelliak 14:e8cd237c8639 151 red_led = LED_OFF;
drelliak 0:88faaa1afb83 152 break;
drelliak 0:88faaa1afb83 153 case BRAKE:
drelliak 0:88faaa1afb83 154 //ser.printf("sending green signal to led\r\n");
drelliak 14:e8cd237c8639 155 green_led = LED_ON;
drelliak 14:e8cd237c8639 156 motor.stopJogging();
drelliak 14:e8cd237c8639 157 printf("BRAKE\r\n");
drelliak 13:f7a7fe9b5c00 158 motor.brakeMotor();
drelliak 14:e8cd237c8639 159 green_led = LED_OFF;
drelliak 0:88faaa1afb83 160 break;
drelliak 0:88faaa1afb83 161 case ANG_RST:
drelliak 0:88faaa1afb83 162 //ser.printf("sending blue signal to led\r\n");
drelliak 14:e8cd237c8639 163 blue_led = LED_ON;
drelliak 14:e8cd237c8639 164
drelliak 14:e8cd237c8639 165 printf("ANG_RST\r\n");
drelliak 12:273752f540be 166 gyro.stop_measure();
drelliak 12:273752f540be 167 gyro.start_measure(GYRO_PERIOD);
drelliak 14:e8cd237c8639 168 blue_led = LED_OFF;
drelliak 14:e8cd237c8639 169 initializeController();
drelliak 0:88faaa1afb83 170 break;
drelliak 0:88faaa1afb83 171 case ANG_REF:
drelliak 14:e8cd237c8639 172 red_led = LED_ON;
drelliak 14:e8cd237c8639 173 green_led = LED_ON;
drelliak 14:e8cd237c8639 174
drelliak 14:e8cd237c8639 175 reference = rcv.get_ang_ref();// - processMagAngle();
drelliak 14:e8cd237c8639 176 printf("New reference: %f \n\r",reference*180/PI);
drelliak 14:e8cd237c8639 177 if(reference > PI)
drelliak 14:e8cd237c8639 178 reference = reference - 2*PI;
drelliak 14:e8cd237c8639 179 if(reference < -PI)
drelliak 14:e8cd237c8639 180 reference = reference + 2*PI;
drelliak 14:e8cd237c8639 181 red_led = LED_OFF;
drelliak 14:e8cd237c8639 182 green_led = LED_OFF;
drelliak 0:88faaa1afb83 183 break;
drelliak 14:e8cd237c8639 184 case GND_VEL:
drelliak 14:e8cd237c8639 185 red_led = LED_ON;
drelliak 14:e8cd237c8639 186 blue_led = LED_ON;
drelliak 14:e8cd237c8639 187 float vel = rcv.get_gnd_vel();
drelliak 14:e8cd237c8639 188 motor.setVelocity(vel);
drelliak 14:e8cd237c8639 189 printf("GND_VEL = %f\r\n", vel);
drelliak 14:e8cd237c8639 190
drelliak 14:e8cd237c8639 191 red_led = LED_OFF;
drelliak 14:e8cd237c8639 192 blue_led = LED_OFF;
drelliak 14:e8cd237c8639 193 break;
drelliak 14:e8cd237c8639 194 case JOG_VEL:
drelliak 14:e8cd237c8639 195 red_led = LED_ON;
drelliak 14:e8cd237c8639 196 blue_led = LED_ON;
drelliak 14:e8cd237c8639 197
drelliak 14:e8cd237c8639 198 float p, r;
drelliak 14:e8cd237c8639 199 rcv.get_jog_vel(&p,&r);
drelliak 14:e8cd237c8639 200 if(p == 0 || r == 0)
drelliak 14:e8cd237c8639 201 motor.stopJogging();
drelliak 14:e8cd237c8639 202 else
drelliak 14:e8cd237c8639 203 motor.startJogging(r,p);
drelliak 14:e8cd237c8639 204 red_led = LED_OFF;
drelliak 14:e8cd237c8639 205 blue_led = LED_OFF;
drelliak 0:88faaa1afb83 206 break;
drelliak 0:88faaa1afb83 207 case PID_PARAMS:
drelliak 14:e8cd237c8639 208 blue_led = LED_ON;
drelliak 14:e8cd237c8639 209 green_led = LED_ON;
drelliak 14:e8cd237c8639 210
drelliak 14:e8cd237c8639 211 float ar[4];
drelliak 14:e8cd237c8639 212 rcv.get_pid_params(ar);
drelliak 14:e8cd237c8639 213 P = ar[0];
drelliak 14:e8cd237c8639 214 I = ar[1];
drelliak 14:e8cd237c8639 215 D = ar[2];
drelliak 14:e8cd237c8639 216 N = ar[3];
drelliak 14:e8cd237c8639 217 printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n",
drelliak 14:e8cd237c8639 218 ar[0], ar[1], ar[2], ar[3]);
drelliak 14:e8cd237c8639 219
drelliak 14:e8cd237c8639 220 wait(1);
drelliak 14:e8cd237c8639 221 blue_led = LED_OFF;
drelliak 14:e8cd237c8639 222 green_led = LED_OFF;
drelliak 0:88faaa1afb83 223 break;
drelliak 16:a9e0eb97557f 224 case MAG_CALIB:
drelliak 16:a9e0eb97557f 225 float mag[4];
drelliak 16:a9e0eb97557f 226 rcv.get_mag_calib(mag);
drelliak 16:a9e0eb97557f 227 max_x=mag[1];
drelliak 16:a9e0eb97557f 228 max_y=mag[3];
drelliak 16:a9e0eb97557f 229 min_x=mag[0];
drelliak 16:a9e0eb97557f 230 min_y=mag[2];
drelliak 16:a9e0eb97557f 231 break;
drelliak 0:88faaa1afb83 232 default:
drelliak 14:e8cd237c8639 233 blue_led = LED_ON;
drelliak 14:e8cd237c8639 234 green_led = LED_ON;
drelliak 14:e8cd237c8639 235 red_led = LED_ON;
drelliak 14:e8cd237c8639 236 printf("nothing understood\r\n");
drelliak 14:e8cd237c8639 237 wait(1);
drelliak 14:e8cd237c8639 238 blue_led = LED_OFF;
drelliak 14:e8cd237c8639 239 green_led = LED_OFF;
drelliak 14:e8cd237c8639 240 red_led = LED_OFF;
drelliak 14:e8cd237c8639 241 //ser.printf("unknown command!\r\n");
drelliak 0:88faaa1afb83 242 }
drelliak 0:88faaa1afb83 243 }
drelliak 0:88faaa1afb83 244 /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */
drelliak 0:88faaa1afb83 245 void initializeController(){
drelliak 0:88faaa1afb83 246 for(int i =0; i<2; i++){
drelliak 0:88faaa1afb83 247 e[i] = 0;
drelliak 0:88faaa1afb83 248 ui[i] = 0;
drelliak 0:88faaa1afb83 249 ud[i] = 0;
drelliak 0:88faaa1afb83 250 }
drelliak 0:88faaa1afb83 251 P= INITIAL_P;
drelliak 0:88faaa1afb83 252 I= INITIAL_I;
drelliak 0:88faaa1afb83 253 D= INITIAL_D;
drelliak 0:88faaa1afb83 254 N= INITIAL_N;
drelliak 0:88faaa1afb83 255 }
drelliak 0:88faaa1afb83 256
drelliak 0:88faaa1afb83 257 /* PID controller for angular position */
drelliak 0:88faaa1afb83 258 void controlAnglePID(float P, float I, float D, float N){
drelliak 0:88faaa1afb83 259 /* Getting error */
drelliak 12:273752f540be 260 float feedback = gyro.get_angle();
drelliak 0:88faaa1afb83 261 e[1] = e[0];
drelliak 5:b0af0cfb678e 262 e[0] = reference - (feedback*PI/180);
drelliak 0:88faaa1afb83 263 if(e[0] > PI)
drelliak 0:88faaa1afb83 264 e[0]= e[0] - 2*PI;
drelliak 0:88faaa1afb83 265 if(e[0] < -PI)
drelliak 0:88faaa1afb83 266 e[0] = e[0] + 2*PI;
drelliak 0:88faaa1afb83 267 /* Proportinal Part */
drelliak 0:88faaa1afb83 268 up[0] = e[0]*P;
drelliak 0:88faaa1afb83 269 /* Integral Part */
drelliak 0:88faaa1afb83 270 ui[1] = ui[0];
drelliak 0:88faaa1afb83 271 if(abs(u) < PI/8){
drelliak 0:88faaa1afb83 272 ui[0] = (P*I*Ts)*e[1] + ui[1];
drelliak 0:88faaa1afb83 273 }
drelliak 0:88faaa1afb83 274 else if(u > 0)
drelliak 0:88faaa1afb83 275 ui[0] = PI/8 - up[0];
drelliak 0:88faaa1afb83 276 else if(u < 0)
drelliak 0:88faaa1afb83 277 ui[0] = -PI/8 - up[0];
drelliak 0:88faaa1afb83 278 /* Derivative Part */
drelliak 0:88faaa1afb83 279 ud[1] = ud[0];
drelliak 0:88faaa1afb83 280 ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
drelliak 0:88faaa1afb83 281 /** Controller **/
drelliak 0:88faaa1afb83 282 u = up[0] + ud[0] + ui[0];
drelliak 0:88faaa1afb83 283 setServoPWM(u*100/(PI/8), servo);
drelliak 0:88faaa1afb83 284 }
drelliak 0:88faaa1afb83 285 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
drelliak 0:88faaa1afb83 286 /* Function to normalize the magnetometer reading */
drelliak 0:88faaa1afb83 287 void magCal(){
drelliak 14:e8cd237c8639 288 //red_led = 0;
drelliak 0:88faaa1afb83 289 printf("Starting Calibration");
drelliak 0:88faaa1afb83 290 mag.enable();
drelliak 0:88faaa1afb83 291 wait(0.01);
drelliak 0:88faaa1afb83 292 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 293 float x0 = max_x = min_y = mag_data.x;
drelliak 0:88faaa1afb83 294 float y0 = max_y = min_y = mag_data.y;
drelliak 0:88faaa1afb83 295 bool began = false;
drelliak 0:88faaa1afb83 296 while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
drelliak 0:88faaa1afb83 297 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 298 if(mag_data.x > max_x)
drelliak 0:88faaa1afb83 299 max_x = mag_data.x;
drelliak 0:88faaa1afb83 300 if(mag_data.y > max_y)
drelliak 0:88faaa1afb83 301 max_y = mag_data.y;
drelliak 0:88faaa1afb83 302 if(mag_data.y < min_y)
drelliak 0:88faaa1afb83 303 min_y = mag_data.y;
drelliak 0:88faaa1afb83 304 if(mag_data.x < min_x)
drelliak 0:88faaa1afb83 305 min_x = mag_data.x;
drelliak 0:88faaa1afb83 306 if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
drelliak 0:88faaa1afb83 307 began = true;
drelliak 0:88faaa1afb83 308 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 309 }
drelliak 0:88faaa1afb83 310 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 311 }
drelliak 0:88faaa1afb83 312
drelliak 0:88faaa1afb83 313 /* Function to transform the magnetometer reading in angle(rad/s).*/
drelliak 0:88faaa1afb83 314 float processMagAngle(){
drelliak 0:88faaa1afb83 315 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 316 x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
drelliak 0:88faaa1afb83 317 y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
drelliak 14:e8cd237c8639 318 return -atan2(y,x);
drelliak 0:88faaa1afb83 319 }