TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@14:e8cd237c8639, 2016-05-01 (annotated)

Committer:
drelliak
Date:
Sun May 01 23:01:27 2016 +0000
Revision:
14:e8cd237c8639
Parent:
13:f7a7fe9b5c00
Child:
16:a9e0eb97557f
Some minor fixes

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