TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@18:c1cd11db47ed, 2016-05-15 (annotated)

Committer:
drelliak
Date:
Sun May 15 19:13:53 2016 +0000
Revision:
18:c1cd11db47ed
Parent:
16:a9e0eb97557f
Child:
20:7138ab2f93f7
Updated protocol

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 18:c1cd11db47ed 102 float max_x=0, max_y=0, min_x=0, min_y=0,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 18:c1cd11db47ed 121 printf("Initializing Ethernet!\r\n");
drelliak 14:e8cd237c8639 122 set_leds_color(RED, red_led, green_led, blue_led);
drelliak 14:e8cd237c8639 123 eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
drelliak 14:e8cd237c8639 124 eth.connect();
drelliak 18:c1cd11db47ed 125 printf("Protocol initialized! \r\n");
drelliak 14:e8cd237c8639 126 set_leds_color(BLUE, red_led, green_led, blue_led);
drelliak 14:e8cd237c8639 127 rcv.set_socket();
drelliak 14:e8cd237c8639 128 gyro.start_measure(GYRO_PERIOD);
drelliak 14:e8cd237c8639 129 controller_ticker.attach(&control,Ts);
drelliak 14:e8cd237c8639 130 //main loop
drelliak 12:273752f540be 131 while(1){
drelliak 14:e8cd237c8639 132 readProtocol();
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 18:c1cd11db47ed 142 printf("Message received!");
drelliak 0:88faaa1afb83 143 switch(msg)
drelliak 0:88faaa1afb83 144 {
drelliak 0:88faaa1afb83 145 case NONE:
drelliak 18:c1cd11db47ed 146 //ser.printf("sending red signal to led\r\n");
drelliak 18:c1cd11db47ed 147 set_leds_color(BLACK,red_led,green_led,blue_led);
drelliak 14:e8cd237c8639 148 printf("NONE\r\n");
drelliak 14:e8cd237c8639 149 wait(1);
drelliak 0:88faaa1afb83 150 break;
drelliak 0:88faaa1afb83 151 case BRAKE:
drelliak 0:88faaa1afb83 152 //ser.printf("sending green signal to led\r\n");
drelliak 18:c1cd11db47ed 153 set_leds_color(YELLOW,red_led,green_led,blue_led);
drelliak 14:e8cd237c8639 154 motor.stopJogging();
drelliak 14:e8cd237c8639 155 printf("BRAKE\r\n");
drelliak 13:f7a7fe9b5c00 156 motor.brakeMotor();
drelliak 0:88faaa1afb83 157 break;
drelliak 0:88faaa1afb83 158 case ANG_RST:
drelliak 0:88faaa1afb83 159 //ser.printf("sending blue signal to led\r\n");
drelliak 18:c1cd11db47ed 160 set_leds_color(PURPLE,red_led,green_led,blue_led);
drelliak 14:e8cd237c8639 161 printf("ANG_RST\r\n");
drelliak 12:273752f540be 162 gyro.stop_measure();
drelliak 18:c1cd11db47ed 163 printf("Stopped gyro\r\n");
drelliak 12:273752f540be 164 gyro.start_measure(GYRO_PERIOD);
drelliak 18:c1cd11db47ed 165 printf("Starting Gyro\r\n");
drelliak 14:e8cd237c8639 166 initializeController();
drelliak 0:88faaa1afb83 167 break;
drelliak 0:88faaa1afb83 168 case ANG_REF:
drelliak 18:c1cd11db47ed 169 set_leds_color(GREEN,red_led,green_led,blue_led);
drelliak 18:c1cd11db47ed 170 reference = rcv.get_ang_ref() - processMagAngle();
drelliak 14:e8cd237c8639 171 printf("New reference: %f \n\r",reference*180/PI);
drelliak 14:e8cd237c8639 172 if(reference > PI)
drelliak 14:e8cd237c8639 173 reference = reference - 2*PI;
drelliak 18:c1cd11db47ed 174 else if(reference < -PI)
drelliak 14:e8cd237c8639 175 reference = reference + 2*PI;
drelliak 18:c1cd11db47ed 176 break;
drelliak 18:c1cd11db47ed 177 case CAM_ANG_REF:
drelliak 18:c1cd11db47ed 178 set_leds_color(RED,red_led,green_led,blue_led);
drelliak 18:c1cd11db47ed 179 reference = rcv.get_cam_ang_ref();
drelliak 18:c1cd11db47ed 180 printf("New reference: %f \n\r",reference*180/PI);
drelliak 0:88faaa1afb83 181 break;
drelliak 14:e8cd237c8639 182 case GND_VEL:
drelliak 18:c1cd11db47ed 183 set_leds_color(AQUA,red_led,green_led,blue_led);
drelliak 14:e8cd237c8639 184 float vel = rcv.get_gnd_vel();
drelliak 14:e8cd237c8639 185 motor.setVelocity(vel);
drelliak 14:e8cd237c8639 186 printf("GND_VEL = %f\r\n", vel);
drelliak 14:e8cd237c8639 187 break;
drelliak 14:e8cd237c8639 188 case JOG_VEL:
drelliak 18:c1cd11db47ed 189 set_leds_color(WHITE,red_led,green_led,blue_led);
drelliak 14:e8cd237c8639 190 float p, r;
drelliak 14:e8cd237c8639 191 rcv.get_jog_vel(&p,&r);
drelliak 18:c1cd11db47ed 192 printf("Joggin with period %f and duty cycle %f\r\n",p,r);
drelliak 14:e8cd237c8639 193 if(p == 0 || r == 0)
drelliak 14:e8cd237c8639 194 motor.stopJogging();
drelliak 14:e8cd237c8639 195 else
drelliak 14:e8cd237c8639 196 motor.startJogging(r,p);
drelliak 0:88faaa1afb83 197 break;
drelliak 0:88faaa1afb83 198 case PID_PARAMS:
drelliak 18:c1cd11db47ed 199 set_leds_color(RED,red_led,green_led,blue_led);
drelliak 14:e8cd237c8639 200 float ar[4];
drelliak 14:e8cd237c8639 201 rcv.get_pid_params(ar);
drelliak 14:e8cd237c8639 202 P = ar[0];
drelliak 14:e8cd237c8639 203 I = ar[1];
drelliak 14:e8cd237c8639 204 D = ar[2];
drelliak 14:e8cd237c8639 205 N = ar[3];
drelliak 14:e8cd237c8639 206 printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n",
drelliak 14:e8cd237c8639 207 ar[0], ar[1], ar[2], ar[3]);
drelliak 14:e8cd237c8639 208
drelliak 14:e8cd237c8639 209 wait(1);
drelliak 0:88faaa1afb83 210 break;
drelliak 16:a9e0eb97557f 211 case MAG_CALIB:
drelliak 18:c1cd11db47ed 212 set_leds_color(BLUE,red_led,green_led,blue_led);
drelliak 18:c1cd11db47ed 213 printf("MAG_CALIB\r\n");
drelliak 16:a9e0eb97557f 214 float mag[4];
drelliak 16:a9e0eb97557f 215 rcv.get_mag_calib(mag);
drelliak 16:a9e0eb97557f 216 max_x=mag[1];
drelliak 16:a9e0eb97557f 217 max_y=mag[3];
drelliak 16:a9e0eb97557f 218 min_x=mag[0];
drelliak 16:a9e0eb97557f 219 min_y=mag[2];
drelliak 18:c1cd11db47ed 220 printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
drelliak 16:a9e0eb97557f 221 break;
drelliak 0:88faaa1afb83 222 default:
drelliak 14:e8cd237c8639 223 printf("nothing understood\r\n");
drelliak 14:e8cd237c8639 224 //ser.printf("unknown command!\r\n");
drelliak 0:88faaa1afb83 225 }
drelliak 0:88faaa1afb83 226 }
drelliak 0:88faaa1afb83 227 /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */
drelliak 0:88faaa1afb83 228 void initializeController(){
drelliak 0:88faaa1afb83 229 for(int i =0; i<2; i++){
drelliak 0:88faaa1afb83 230 e[i] = 0;
drelliak 0:88faaa1afb83 231 ui[i] = 0;
drelliak 0:88faaa1afb83 232 ud[i] = 0;
drelliak 0:88faaa1afb83 233 }
drelliak 0:88faaa1afb83 234 P= INITIAL_P;
drelliak 0:88faaa1afb83 235 I= INITIAL_I;
drelliak 0:88faaa1afb83 236 D= INITIAL_D;
drelliak 0:88faaa1afb83 237 N= INITIAL_N;
drelliak 0:88faaa1afb83 238 }
drelliak 0:88faaa1afb83 239
drelliak 0:88faaa1afb83 240 /* PID controller for angular position */
drelliak 0:88faaa1afb83 241 void controlAnglePID(float P, float I, float D, float N){
drelliak 0:88faaa1afb83 242 /* Getting error */
drelliak 12:273752f540be 243 float feedback = gyro.get_angle();
drelliak 0:88faaa1afb83 244 e[1] = e[0];
drelliak 5:b0af0cfb678e 245 e[0] = reference - (feedback*PI/180);
drelliak 0:88faaa1afb83 246 if(e[0] > PI)
drelliak 0:88faaa1afb83 247 e[0]= e[0] - 2*PI;
drelliak 0:88faaa1afb83 248 if(e[0] < -PI)
drelliak 0:88faaa1afb83 249 e[0] = e[0] + 2*PI;
drelliak 0:88faaa1afb83 250 /* Proportinal Part */
drelliak 0:88faaa1afb83 251 up[0] = e[0]*P;
drelliak 0:88faaa1afb83 252 /* Integral Part */
drelliak 0:88faaa1afb83 253 ui[1] = ui[0];
drelliak 0:88faaa1afb83 254 if(abs(u) < PI/8){
drelliak 0:88faaa1afb83 255 ui[0] = (P*I*Ts)*e[1] + ui[1];
drelliak 0:88faaa1afb83 256 }
drelliak 0:88faaa1afb83 257 else if(u > 0)
drelliak 0:88faaa1afb83 258 ui[0] = PI/8 - up[0];
drelliak 0:88faaa1afb83 259 else if(u < 0)
drelliak 0:88faaa1afb83 260 ui[0] = -PI/8 - up[0];
drelliak 0:88faaa1afb83 261 /* Derivative Part */
drelliak 0:88faaa1afb83 262 ud[1] = ud[0];
drelliak 0:88faaa1afb83 263 ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
drelliak 0:88faaa1afb83 264 /** Controller **/
drelliak 0:88faaa1afb83 265 u = up[0] + ud[0] + ui[0];
drelliak 0:88faaa1afb83 266 setServoPWM(u*100/(PI/8), servo);
drelliak 0:88faaa1afb83 267 }
drelliak 0:88faaa1afb83 268 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
drelliak 0:88faaa1afb83 269 /* Function to normalize the magnetometer reading */
drelliak 0:88faaa1afb83 270 void magCal(){
drelliak 14:e8cd237c8639 271 //red_led = 0;
drelliak 0:88faaa1afb83 272 printf("Starting Calibration");
drelliak 0:88faaa1afb83 273 mag.enable();
drelliak 0:88faaa1afb83 274 wait(0.01);
drelliak 0:88faaa1afb83 275 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 276 float x0 = max_x = min_y = mag_data.x;
drelliak 0:88faaa1afb83 277 float y0 = max_y = min_y = mag_data.y;
drelliak 0:88faaa1afb83 278 bool began = false;
drelliak 0:88faaa1afb83 279 while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
drelliak 0:88faaa1afb83 280 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 281 if(mag_data.x > max_x)
drelliak 0:88faaa1afb83 282 max_x = mag_data.x;
drelliak 0:88faaa1afb83 283 if(mag_data.y > max_y)
drelliak 0:88faaa1afb83 284 max_y = mag_data.y;
drelliak 0:88faaa1afb83 285 if(mag_data.y < min_y)
drelliak 0:88faaa1afb83 286 min_y = mag_data.y;
drelliak 0:88faaa1afb83 287 if(mag_data.x < min_x)
drelliak 0:88faaa1afb83 288 min_x = mag_data.x;
drelliak 0:88faaa1afb83 289 if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
drelliak 0:88faaa1afb83 290 began = true;
drelliak 0:88faaa1afb83 291 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 292 }
drelliak 0:88faaa1afb83 293 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 294 }
drelliak 0:88faaa1afb83 295
drelliak 0:88faaa1afb83 296 /* Function to transform the magnetometer reading in angle(rad/s).*/
drelliak 0:88faaa1afb83 297 float processMagAngle(){
drelliak 18:c1cd11db47ed 298 printf("starting ProcessMagAngle()\n\r");
drelliak 18:c1cd11db47ed 299 float mag_lpf = 0;
drelliak 18:c1cd11db47ed 300 Timer t1;
drelliak 18:c1cd11db47ed 301 for(int i = 0; i<20; i++){
drelliak 18:c1cd11db47ed 302 //printf("\r\n");
drelliak 18:c1cd11db47ed 303 //wait(0.1);
drelliak 18:c1cd11db47ed 304 t1.start();
drelliak 18:c1cd11db47ed 305 __disable_irq();
drelliak 18:c1cd11db47ed 306 mag.getAxis(mag_data);
drelliak 18:c1cd11db47ed 307 __enable_irq();
drelliak 18:c1cd11db47ed 308 t1.stop();
drelliak 18:c1cd11db47ed 309 x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
drelliak 18:c1cd11db47ed 310 y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
drelliak 18:c1cd11db47ed 311 mag_lpf = mag_lpf + (-atan2(y,x));
drelliak 18:c1cd11db47ed 312 wait(0.015);
drelliak 18:c1cd11db47ed 313 }
drelliak 18:c1cd11db47ed 314 // wait(20*0.01);
drelliak 18:c1cd11db47ed 315 printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
drelliak 18:c1cd11db47ed 316 return mag_lpf/20;
drelliak 0:88faaa1afb83 317 }