TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@22:b7cca3089dfe, 2020-09-21 (annotated)

Committer:
starling
Date:
Mon Sep 21 21:45:08 2020 +0000
Revision:
22:b7cca3089dfe
Parent:
20:7138ab2f93f7 
01 mar 2020

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 20:7138ab2f93f7 7 #include "rtos.h"
drelliak 0:88faaa1afb83 8
drelliak 20:7138ab2f93f7 9 #define PI 3.141593
drelliak 20:7138ab2f93f7 10 #define Ts 0.02 // Controller period(Seconds)
drelliak 12:273752f540be 11 #define PWM_PERIOD 13.5 // ms
drelliak 0:88faaa1afb83 12 #define INITIAL_P 0.452531214933414
drelliak 0:88faaa1afb83 13 #define INITIAL_I 5.45748932024049
drelliak 0:88faaa1afb83 14 #define INITIAL_D 0.000233453623255507
drelliak 0:88faaa1afb83 15 #define INITIAL_N 51.0605584484153
drelliak 20:7138ab2f93f7 16 #define END_THRESH 4 //For magnetometer calibration
drelliak 20:7138ab2f93f7 17 #define START_THRESH 10 //For magnetometer calibration
drelliak 0:88faaa1afb83 18 #define MINIMUM_VELOCITY 15
drelliak 20:7138ab2f93f7 19 #define MINIMUM_CURVE_VELOCITY 19
drelliak 20:7138ab2f93f7 20 #define ERROR_THRESH PI/5
drelliak 20:7138ab2f93f7 21 #define GYRO_PERIOD 15000 //us
drelliak 20:7138ab2f93f7 22 #define RGB_LED_ON 0 //active Low
drelliak 20:7138ab2f93f7 23 #define RGB_LED_OFF 1 //active Low
drelliak 14:e8cd237c8639 24
drelliak 14:e8cd237c8639 25 #define MIN -1.5
drelliak 14:e8cd237c8639 26 #define MAX 1.5
drelliak 14:e8cd237c8639 27
drelliak 14:e8cd237c8639 28 enum{
drelliak 14:e8cd237c8639 29 BLACK,
drelliak 14:e8cd237c8639 30 RED,
drelliak 14:e8cd237c8639 31 GREEN,
drelliak 14:e8cd237c8639 32 BLUE,
drelliak 14:e8cd237c8639 33 PURPLE,
drelliak 14:e8cd237c8639 34 YELLOW,
drelliak 14:e8cd237c8639 35 AQUA,
drelliak 14:e8cd237c8639 36 WHITE};
drelliak 14:e8cd237c8639 37
drelliak 14:e8cd237c8639 38
drelliak 20:7138ab2f93f7 39 //Control Objetcs
starling 22:b7cca3089dfe 40 PwmOut servo(PTD1); // Servo connected to pin PTD3
drelliak 13:f7a7fe9b5c00 41 Motor motor;
drelliak 0:88faaa1afb83 42 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
drelliak 14:e8cd237c8639 43 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
drelliak 0:88faaa1afb83 44 FXAS21002 gyro(PTE25,PTE24);
drelliak 20:7138ab2f93f7 45 //Leds Objects
drelliak 14:e8cd237c8639 46 DigitalOut red_led(LED_RED);
drelliak 14:e8cd237c8639 47 DigitalOut green_led(LED_GREEN);
drelliak 14:e8cd237c8639 48 DigitalOut blue_led(LED_BLUE);
starling 22:b7cca3089dfe 49 DigitalOut red_led_s(PTD0);
starling 22:b7cca3089dfe 50 DigitalOut green_led_s(PTB2); //PTC4
starling 22:b7cca3089dfe 51 DigitalOut blue_led_s(PTC12);
starling 22:b7cca3089dfe 52 DigitalOut main_led(PTC4);
drelliak 20:7138ab2f93f7 53 //Protocol Objects
drelliak 14:e8cd237c8639 54 Receiver rcv;
drelliak 14:e8cd237c8639 55 EthernetInterface eth;
drelliak 0:88faaa1afb83 56
drelliak 0:88faaa1afb83 57 // PID controller parameters and functions
drelliak 0:88faaa1afb83 58 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 59 float P, I, D, N, reference = 0;
drelliak 0:88faaa1afb83 60 void controlAnglePID(float P, float I, float D, float N);
drelliak 0:88faaa1afb83 61 void initializeController();
drelliak 14:e8cd237c8639 62 void control();
drelliak 14:e8cd237c8639 63 Ticker controller_ticker;
drelliak 0:88faaa1afb83 64
drelliak 20:7138ab2f93f7 65 // Motor and servo variables
drelliak 20:7138ab2f93f7 66 float saved_velocity = 0;
drelliak 20:7138ab2f93f7 67 bool brake = false;
drelliak 20:7138ab2f93f7 68
drelliak 20:7138ab2f93f7 69 // Magnetometer/Gyro variables and functions
drelliak 18:c1cd11db47ed 70 float max_x=0, max_y=0, min_x=0, min_y=0,x,y;
drelliak 0:88faaa1afb83 71 MotionSensorDataUnits mag_data;
drelliak 14:e8cd237c8639 72 MotionSensorDataCounts mag_raw;
drelliak 0:88faaa1afb83 73 float processMagAngle();
drelliak 20:7138ab2f93f7 74 float gyro_reference = 0;
drelliak 0:88faaa1afb83 75 void magCal();
drelliak 0:88faaa1afb83 76
drelliak 14:e8cd237c8639 77 // Protocol
drelliak 14:e8cd237c8639 78 void readProtocol();
drelliak 20:7138ab2f93f7 79 int contp = 0; // for debug only
drelliak 20:7138ab2f93f7 80
drelliak 20:7138ab2f93f7 81 // NXP RGB_LEDs control
drelliak 20:7138ab2f93f7 82 void set_leds_color(int color);
drelliak 20:7138ab2f93f7 83 void turn_leds_off();
drelliak 14:e8cd237c8639 84
drelliak 0:88faaa1afb83 85 int main(){
starling 22:b7cca3089dfe 86
drelliak 14:e8cd237c8639 87 // Initializing sensors:
drelliak 20:7138ab2f93f7 88 main_led = 1;
drelliak 14:e8cd237c8639 89 acc.enable();
drelliak 14:e8cd237c8639 90 gyro.gyro_config(MODE_1);
drelliak 12:273752f540be 91 initializeController();
drelliak 14:e8cd237c8639 92
drelliak 14:e8cd237c8639 93 // Set initial control configurations
drelliak 14:e8cd237c8639 94 motor.setVelocity(0);
drelliak 14:e8cd237c8639 95
drelliak 20:7138ab2f93f7 96 // Protocol initialization
drelliak 20:7138ab2f93f7 97
starling 22:b7cca3089dfe 98
starling 22:b7cca3089dfe 99
starling 22:b7cca3089dfe 100
drelliak 18:c1cd11db47ed 101 printf("Initializing Ethernet!\r\n");
drelliak 20:7138ab2f93f7 102 set_leds_color(RED);
drelliak 14:e8cd237c8639 103 eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
drelliak 14:e8cd237c8639 104 eth.connect();
drelliak 18:c1cd11db47ed 105 printf("Protocol initialized! \r\n");
starling 22:b7cca3089dfe 106 gyro.start_measure(GYRO_PERIOD);
drelliak 20:7138ab2f93f7 107 set_leds_color(BLUE);
drelliak 14:e8cd237c8639 108 rcv.set_socket();
starling 22:b7cca3089dfe 109
drelliak 20:7138ab2f93f7 110 main_led = 0;
drelliak 14:e8cd237c8639 111 controller_ticker.attach(&control,Ts);
starling 22:b7cca3089dfe 112
starling 22:b7cca3089dfe 113
starling 22:b7cca3089dfe 114
drelliak 14:e8cd237c8639 115 //main loop
drelliak 12:273752f540be 116 while(1){
drelliak 14:e8cd237c8639 117 readProtocol();
drelliak 20:7138ab2f93f7 118 // printf("%f \r\n",gyro.get_angle());
drelliak 5:b0af0cfb678e 119 }
drelliak 0:88faaa1afb83 120 }
drelliak 14:e8cd237c8639 121 void control(){
drelliak 14:e8cd237c8639 122 controlAnglePID(P,I,D,N);
drelliak 14:e8cd237c8639 123 }
drelliak 0:88faaa1afb83 124 void readProtocol(){
drelliak 14:e8cd237c8639 125 if(!rcv.receive())
drelliak 14:e8cd237c8639 126 return;
drelliak 14:e8cd237c8639 127 char msg = rcv.get_msg();
drelliak 20:7138ab2f93f7 128 //printf("Message received!");
drelliak 20:7138ab2f93f7 129 //contp++;
drelliak 20:7138ab2f93f7 130 //printf(" %d \r\n",contp);
drelliak 0:88faaa1afb83 131 switch(msg)
drelliak 0:88faaa1afb83 132 {
drelliak 0:88faaa1afb83 133 case NONE:
drelliak 0:88faaa1afb83 134 break;
drelliak 0:88faaa1afb83 135 case BRAKE:
drelliak 20:7138ab2f93f7 136 //printf("BRAKE ");
drelliak 20:7138ab2f93f7 137 float intensity, b_wait;
drelliak 20:7138ab2f93f7 138 rcv.get_brake(&intensity,&b_wait);
drelliak 20:7138ab2f93f7 139 if(!brake){
drelliak 20:7138ab2f93f7 140 set_leds_color(YELLOW);
drelliak 20:7138ab2f93f7 141 motor.stopJogging();
drelliak 20:7138ab2f93f7 142 // printf("BRAKE\r\n");
drelliak 20:7138ab2f93f7 143 setServoPWM(0,servo);
drelliak 20:7138ab2f93f7 144 //reference = 0;
drelliak 20:7138ab2f93f7 145 controller_ticker.detach();
drelliak 20:7138ab2f93f7 146 motor.brakeMotor(intensity,b_wait);
drelliak 20:7138ab2f93f7 147 controller_ticker.attach(&control,Ts);
drelliak 20:7138ab2f93f7 148 saved_velocity = 0;
drelliak 20:7138ab2f93f7 149 brake = true;
drelliak 20:7138ab2f93f7 150 }
drelliak 20:7138ab2f93f7 151 break;
drelliak 20:7138ab2f93f7 152 case GYRO_ZERO:
drelliak 20:7138ab2f93f7 153 gyro.stop_measure();
drelliak 20:7138ab2f93f7 154 wait(0.05);
drelliak 20:7138ab2f93f7 155 gyro.start_measure(GYRO_PERIOD);
drelliak 20:7138ab2f93f7 156 break;
drelliak 20:7138ab2f93f7 157 case ANG_SET:
drelliak 20:7138ab2f93f7 158 set_leds_color(PURPLE);
drelliak 20:7138ab2f93f7 159 //printf("ANG_SET\r\n");
drelliak 20:7138ab2f93f7 160 gyro_reference = gyro.get_angle();
drelliak 20:7138ab2f93f7 161 initializeController();
drelliak 0:88faaa1afb83 162 break;
drelliak 0:88faaa1afb83 163 case ANG_RST:
drelliak 20:7138ab2f93f7 164 //printf("ANG_RST\r\n");
drelliak 20:7138ab2f93f7 165 gyro_reference = 0;
drelliak 20:7138ab2f93f7 166 set_leds_color(PURPLE);
drelliak 14:e8cd237c8639 167 initializeController();
drelliak 0:88faaa1afb83 168 break;
drelliak 20:7138ab2f93f7 169 case MAG_ANG_REF:
drelliak 20:7138ab2f93f7 170 set_leds_color(BLUE);
drelliak 20:7138ab2f93f7 171 reference = rcv.get_mag_ang_ref() - processMagAngle();
drelliak 20:7138ab2f93f7 172 //printf("New reference: %f \n\r",reference*180/PI);
drelliak 14:e8cd237c8639 173 if(reference > PI)
drelliak 14:e8cd237c8639 174 reference = reference - 2*PI;
drelliak 18:c1cd11db47ed 175 else if(reference < -PI)
drelliak 14:e8cd237c8639 176 reference = reference + 2*PI;
drelliak 18:c1cd11db47ed 177 break;
drelliak 20:7138ab2f93f7 178 case ABS_ANG_REF:
drelliak 20:7138ab2f93f7 179 set_leds_color(GREEN);
drelliak 20:7138ab2f93f7 180 reference = rcv.get_abs_ang_ref();
drelliak 20:7138ab2f93f7 181 //printf("New reference: %f \n\r",reference*180/PI);
drelliak 20:7138ab2f93f7 182 if(reference > PI)
drelliak 20:7138ab2f93f7 183 reference = reference - 2*PI;
drelliak 20:7138ab2f93f7 184 else if(reference < -PI)
drelliak 20:7138ab2f93f7 185 reference = reference + 2*PI;
drelliak 20:7138ab2f93f7 186 break;
drelliak 20:7138ab2f93f7 187 case REL_ANG_REF:
drelliak 20:7138ab2f93f7 188 set_leds_color(RED);
drelliak 20:7138ab2f93f7 189 reference = rcv.get_rel_ang_ref() + gyro.get_angle()*PI/180;
drelliak 20:7138ab2f93f7 190 //printf("New reference: %f \n\r",reference*180/PI);
drelliak 20:7138ab2f93f7 191 if(reference > PI)
drelliak 20:7138ab2f93f7 192 reference = reference - 2*PI;
drelliak 20:7138ab2f93f7 193 else if(reference < -PI)
drelliak 20:7138ab2f93f7 194 reference = reference + 2*PI;
drelliak 0:88faaa1afb83 195 break;
drelliak 14:e8cd237c8639 196 case GND_VEL:
drelliak 20:7138ab2f93f7 197 set_leds_color(AQUA);
drelliak 20:7138ab2f93f7 198 saved_velocity = rcv.get_gnd_vel();
drelliak 20:7138ab2f93f7 199 //printf("GND_VEL");
drelliak 20:7138ab2f93f7 200 if(saved_velocity > 0){
drelliak 20:7138ab2f93f7 201 motor.setVelocity(saved_velocity);
drelliak 20:7138ab2f93f7 202 if(abs(saved_velocity) > MINIMUM_VELOCITY)
drelliak 20:7138ab2f93f7 203 brake = false;
drelliak 20:7138ab2f93f7 204 //printf("GND_VEL = %f\r\n", saved_velocity);
drelliak 20:7138ab2f93f7 205 }
drelliak 14:e8cd237c8639 206 break;
drelliak 14:e8cd237c8639 207 case JOG_VEL:
drelliak 20:7138ab2f93f7 208 set_leds_color(WHITE);
drelliak 14:e8cd237c8639 209 float p, r;
drelliak 14:e8cd237c8639 210 rcv.get_jog_vel(&p,&r);
drelliak 20:7138ab2f93f7 211 //printf("Joggin with period %f and duty cycle %f\r\n",p,r);
drelliak 14:e8cd237c8639 212 if(p == 0 || r == 0)
drelliak 14:e8cd237c8639 213 motor.stopJogging();
drelliak 14:e8cd237c8639 214 else
drelliak 14:e8cd237c8639 215 motor.startJogging(r,p);
drelliak 0:88faaa1afb83 216 break;
drelliak 0:88faaa1afb83 217 case PID_PARAMS:
drelliak 20:7138ab2f93f7 218 set_leds_color(WHITE);
drelliak 20:7138ab2f93f7 219 float arr[4];
drelliak 20:7138ab2f93f7 220 rcv.get_pid_params(arr);
drelliak 20:7138ab2f93f7 221 P = arr[0];
drelliak 20:7138ab2f93f7 222 I = arr[1];
drelliak 20:7138ab2f93f7 223 D = arr[2];
drelliak 20:7138ab2f93f7 224 N = arr[3];
drelliak 20:7138ab2f93f7 225 // printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n",
drelliak 20:7138ab2f93f7 226 // arr[0], arr[1], arr[2], arr[3]);
drelliak 14:e8cd237c8639 227
drelliak 0:88faaa1afb83 228 break;
drelliak 16:a9e0eb97557f 229 case MAG_CALIB:
drelliak 20:7138ab2f93f7 230 set_leds_color(BLUE);
drelliak 18:c1cd11db47ed 231 printf("MAG_CALIB\r\n");
drelliak 16:a9e0eb97557f 232 float mag[4];
drelliak 16:a9e0eb97557f 233 rcv.get_mag_calib(mag);
drelliak 16:a9e0eb97557f 234 max_x=mag[1];
drelliak 16:a9e0eb97557f 235 max_y=mag[3];
drelliak 16:a9e0eb97557f 236 min_x=mag[0];
drelliak 16:a9e0eb97557f 237 min_y=mag[2];
drelliak 20:7138ab2f93f7 238 //printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
drelliak 20:7138ab2f93f7 239 break;
drelliak 20:7138ab2f93f7 240 case LED_ON:
drelliak 20:7138ab2f93f7 241 set_leds_color(BLACK);
drelliak 20:7138ab2f93f7 242 main_led = 1;
drelliak 20:7138ab2f93f7 243 break;
drelliak 20:7138ab2f93f7 244 case LED_OFF:
drelliak 20:7138ab2f93f7 245 set_leds_color(BLACK);
drelliak 20:7138ab2f93f7 246 main_led = 0;
drelliak 16:a9e0eb97557f 247 break;
drelliak 0:88faaa1afb83 248 default:
drelliak 14:e8cd237c8639 249 //ser.printf("unknown command!\r\n");
drelliak 0:88faaa1afb83 250 }
drelliak 0:88faaa1afb83 251 }
drelliak 0:88faaa1afb83 252 /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */
drelliak 0:88faaa1afb83 253 void initializeController(){
drelliak 0:88faaa1afb83 254 for(int i =0; i<2; i++){
drelliak 0:88faaa1afb83 255 e[i] = 0;
drelliak 0:88faaa1afb83 256 ui[i] = 0;
drelliak 0:88faaa1afb83 257 ud[i] = 0;
drelliak 0:88faaa1afb83 258 }
drelliak 0:88faaa1afb83 259 P= INITIAL_P;
drelliak 0:88faaa1afb83 260 I= INITIAL_I;
drelliak 0:88faaa1afb83 261 D= INITIAL_D;
drelliak 0:88faaa1afb83 262 N= INITIAL_N;
drelliak 0:88faaa1afb83 263 }
drelliak 0:88faaa1afb83 264
drelliak 0:88faaa1afb83 265 /* PID controller for angular position */
drelliak 0:88faaa1afb83 266 void controlAnglePID(float P, float I, float D, float N){
drelliak 0:88faaa1afb83 267 /* Getting error */
drelliak 20:7138ab2f93f7 268 float feedback = gyro.get_angle() - gyro_reference;
drelliak 0:88faaa1afb83 269 e[1] = e[0];
drelliak 5:b0af0cfb678e 270 e[0] = reference - (feedback*PI/180);
drelliak 0:88faaa1afb83 271 if(e[0] > PI)
drelliak 0:88faaa1afb83 272 e[0]= e[0] - 2*PI;
drelliak 0:88faaa1afb83 273 if(e[0] < -PI)
drelliak 0:88faaa1afb83 274 e[0] = e[0] + 2*PI;
drelliak 0:88faaa1afb83 275 /* Proportinal Part */
drelliak 0:88faaa1afb83 276 up[0] = e[0]*P;
drelliak 0:88faaa1afb83 277 /* Integral Part */
drelliak 0:88faaa1afb83 278 ui[1] = ui[0];
drelliak 0:88faaa1afb83 279 if(abs(u) < PI/8){
drelliak 0:88faaa1afb83 280 ui[0] = (P*I*Ts)*e[1] + ui[1];
drelliak 0:88faaa1afb83 281 }
drelliak 0:88faaa1afb83 282 else if(u > 0)
drelliak 0:88faaa1afb83 283 ui[0] = PI/8 - up[0];
drelliak 0:88faaa1afb83 284 else if(u < 0)
drelliak 0:88faaa1afb83 285 ui[0] = -PI/8 - up[0];
drelliak 0:88faaa1afb83 286 /* Derivative Part */
drelliak 0:88faaa1afb83 287 ud[1] = ud[0];
drelliak 0:88faaa1afb83 288 ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
drelliak 0:88faaa1afb83 289 /** Controller **/
drelliak 0:88faaa1afb83 290 u = up[0] + ud[0] + ui[0];
drelliak 0:88faaa1afb83 291 setServoPWM(u*100/(PI/8), servo);
drelliak 20:7138ab2f93f7 292 if(abs(e[0]) >= ERROR_THRESH && motor.getVelocity() > MINIMUM_CURVE_VELOCITY){
drelliak 20:7138ab2f93f7 293 saved_velocity = motor.getVelocity();
drelliak 20:7138ab2f93f7 294 motor.setVelocity(MINIMUM_CURVE_VELOCITY);
drelliak 20:7138ab2f93f7 295 }
drelliak 20:7138ab2f93f7 296 else if(abs(e[0]) < ERROR_THRESH && motor.getVelocity() != saved_velocity){
drelliak 20:7138ab2f93f7 297 motor.setVelocity(saved_velocity);
drelliak 20:7138ab2f93f7 298 }
drelliak 0:88faaa1afb83 299 }
drelliak 0:88faaa1afb83 300 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
drelliak 0:88faaa1afb83 301 /* Function to normalize the magnetometer reading */
drelliak 0:88faaa1afb83 302 void magCal(){
drelliak 14:e8cd237c8639 303 //red_led = 0;
drelliak 0:88faaa1afb83 304 printf("Starting Calibration");
drelliak 0:88faaa1afb83 305 mag.enable();
drelliak 0:88faaa1afb83 306 wait(0.01);
drelliak 0:88faaa1afb83 307 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 308 float x0 = max_x = min_y = mag_data.x;
drelliak 0:88faaa1afb83 309 float y0 = max_y = min_y = mag_data.y;
drelliak 0:88faaa1afb83 310 bool began = false;
drelliak 0:88faaa1afb83 311 while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
drelliak 0:88faaa1afb83 312 mag.getAxis(mag_data);
drelliak 0:88faaa1afb83 313 if(mag_data.x > max_x)
drelliak 0:88faaa1afb83 314 max_x = mag_data.x;
drelliak 0:88faaa1afb83 315 if(mag_data.y > max_y)
drelliak 0:88faaa1afb83 316 max_y = mag_data.y;
drelliak 0:88faaa1afb83 317 if(mag_data.y < min_y)
drelliak 0:88faaa1afb83 318 min_y = mag_data.y;
drelliak 0:88faaa1afb83 319 if(mag_data.x < min_x)
drelliak 0:88faaa1afb83 320 min_x = mag_data.x;
drelliak 0:88faaa1afb83 321 if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
drelliak 0:88faaa1afb83 322 began = true;
drelliak 0:88faaa1afb83 323 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 324 }
drelliak 0:88faaa1afb83 325 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 326 }
drelliak 0:88faaa1afb83 327
drelliak 0:88faaa1afb83 328 /* Function to transform the magnetometer reading in angle(rad/s).*/
drelliak 0:88faaa1afb83 329 float processMagAngle(){
drelliak 20:7138ab2f93f7 330 // printf("starting ProcessMagAngle()\n\r");
drelliak 18:c1cd11db47ed 331 float mag_lpf = 0;
drelliak 18:c1cd11db47ed 332 Timer t1;
drelliak 18:c1cd11db47ed 333 for(int i = 0; i<20; i++){
drelliak 18:c1cd11db47ed 334 //printf("\r\n");
drelliak 18:c1cd11db47ed 335 //wait(0.1);
drelliak 18:c1cd11db47ed 336 t1.start();
drelliak 18:c1cd11db47ed 337 __disable_irq();
drelliak 18:c1cd11db47ed 338 mag.getAxis(mag_data);
drelliak 18:c1cd11db47ed 339 __enable_irq();
drelliak 18:c1cd11db47ed 340 t1.stop();
drelliak 18:c1cd11db47ed 341 x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
drelliak 18:c1cd11db47ed 342 y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
drelliak 18:c1cd11db47ed 343 mag_lpf = mag_lpf + (-atan2(y,x));
drelliak 18:c1cd11db47ed 344 wait(0.015);
drelliak 18:c1cd11db47ed 345 }
drelliak 18:c1cd11db47ed 346 // wait(20*0.01);
drelliak 20:7138ab2f93f7 347 // printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
drelliak 18:c1cd11db47ed 348 return mag_lpf/20;
drelliak 20:7138ab2f93f7 349 }
drelliak 20:7138ab2f93f7 350 void turn_leds_off()
drelliak 20:7138ab2f93f7 351 {
drelliak 20:7138ab2f93f7 352 red_led = RGB_LED_OFF;
drelliak 20:7138ab2f93f7 353 green_led = RGB_LED_OFF;
drelliak 20:7138ab2f93f7 354 blue_led = RGB_LED_OFF;
drelliak 20:7138ab2f93f7 355 }
drelliak 20:7138ab2f93f7 356
drelliak 20:7138ab2f93f7 357 void set_leds_color(int color)
drelliak 20:7138ab2f93f7 358 {
drelliak 20:7138ab2f93f7 359 turn_leds_off();
drelliak 20:7138ab2f93f7 360
drelliak 20:7138ab2f93f7 361 switch(color)
drelliak 20:7138ab2f93f7 362 {
drelliak 20:7138ab2f93f7 363 case RED:
starling 22:b7cca3089dfe 364 red_led = RGB_LED_ON;
starling 22:b7cca3089dfe 365 red_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 366 break;
drelliak 20:7138ab2f93f7 367 case GREEN:
drelliak 20:7138ab2f93f7 368 green_led = RGB_LED_ON;
starling 22:b7cca3089dfe 369 green_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 370 break;
drelliak 20:7138ab2f93f7 371 case BLUE:
drelliak 20:7138ab2f93f7 372 blue_led = RGB_LED_ON;
starling 22:b7cca3089dfe 373 blue_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 374 break;
drelliak 20:7138ab2f93f7 375 case PURPLE:
drelliak 20:7138ab2f93f7 376 red_led = RGB_LED_ON;
drelliak 20:7138ab2f93f7 377 blue_led = RGB_LED_ON;
starling 22:b7cca3089dfe 378 red_led_s = RGB_LED_ON;
starling 22:b7cca3089dfe 379 blue_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 380 break;
drelliak 20:7138ab2f93f7 381 case YELLOW:
drelliak 20:7138ab2f93f7 382 red_led = RGB_LED_ON;
drelliak 20:7138ab2f93f7 383 green_led = RGB_LED_ON;
starling 22:b7cca3089dfe 384 red_led_s = RGB_LED_ON;
starling 22:b7cca3089dfe 385 green_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 386 break;
drelliak 20:7138ab2f93f7 387 case AQUA:
drelliak 20:7138ab2f93f7 388 blue_led = RGB_LED_ON;
drelliak 20:7138ab2f93f7 389 green_led = RGB_LED_ON;
starling 22:b7cca3089dfe 390 blue_led_s = RGB_LED_ON;
starling 22:b7cca3089dfe 391 green_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 392 break;
drelliak 20:7138ab2f93f7 393 case WHITE:
drelliak 20:7138ab2f93f7 394 red_led = RGB_LED_ON;
drelliak 20:7138ab2f93f7 395 green_led = RGB_LED_ON;
drelliak 20:7138ab2f93f7 396 blue_led = RGB_LED_ON;
starling 22:b7cca3089dfe 397 red_led_s = RGB_LED_ON;
starling 22:b7cca3089dfe 398 green_led_s = RGB_LED_ON;
starling 22:b7cca3089dfe 399 blue_led_s = RGB_LED_ON;
drelliak 20:7138ab2f93f7 400 break;
drelliak 20:7138ab2f93f7 401 default:
drelliak 20:7138ab2f93f7 402 break;
drelliak 20:7138ab2f93f7 403 }
drelliak 0:88faaa1afb83 404 }