TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

main.cpp@20:7138ab2f93f7, 2016-07-16 (annotated)

Committer:
drelliak
Date:
Sat Jul 16 19:17:08 2016 +0000
Revision:
20:7138ab2f93f7
Parent:
18:c1cd11db47ed
Child:
22:b7cca3089dfe
Winter Challenge 2016 Trekking controller

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