Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
main.cpp@8:a1067fcde341, 2016-04-23 (annotated)
- Committer:
- drelliak
- Date:
- Sat Apr 23 03:55:39 2016 +0000
- Revision:
- 8:a1067fcde341
- Parent:
- 7:27516a2b504b
Updated what we did last weekend
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:88faaa1afb83 | 6 | |
drelliak | 0:88faaa1afb83 | 7 | |
drelliak | 0:88faaa1afb83 | 8 | #define PI 3.141592653589793238462 |
drelliak | 0:88faaa1afb83 | 9 | #define Ts 0.02 // Seconds |
drelliak | 6:c930555fd872 | 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 | 6:c930555fd872 | 15 | #define BRAKE_CONSTANT 40 |
drelliak | 8:a1067fcde341 | 16 | #define BRAKE_WAIT 2 |
drelliak | 0:88faaa1afb83 | 17 | #define GYRO_OFFSET 0.0152 |
drelliak | 0:88faaa1afb83 | 18 | #define END_THRESH 4 |
drelliak | 0:88faaa1afb83 | 19 | #define START_THRESH 10 |
drelliak | 0:88faaa1afb83 | 20 | #define MINIMUM_VELOCITY 15 |
drelliak | 6:c930555fd872 | 21 | |
drelliak | 8:a1067fcde341 | 22 | |
drelliak | 0:88faaa1afb83 | 23 | Serial ser(USBTX, USBRX); // Initialize Serial port |
drelliak | 0:88faaa1afb83 | 24 | PwmOut motor(PTD1); // Motor connected to pin PTD1 |
drelliak | 0:88faaa1afb83 | 25 | PwmOut servo(PTD3); // Servo connected to pin PTD3 |
drelliak | 0:88faaa1afb83 | 26 | |
drelliak | 0:88faaa1afb83 | 27 | FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1); |
drelliak | 0:88faaa1afb83 | 28 | FXAS21002 gyro(PTE25,PTE24); |
drelliak | 0:88faaa1afb83 | 29 | |
drelliak | 0:88faaa1afb83 | 30 | |
drelliak | 0:88faaa1afb83 | 31 | |
drelliak | 0:88faaa1afb83 | 32 | // PID controller parameters and functions |
drelliak | 0:88faaa1afb83 | 33 | 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 | 34 | float P, I, D, N, reference = 0; |
drelliak | 0:88faaa1afb83 | 35 | void controlAnglePID(float P, float I, float D, float N); |
drelliak | 0:88faaa1afb83 | 36 | void initializeController(); |
drelliak | 0:88faaa1afb83 | 37 | |
drelliak | 0:88faaa1afb83 | 38 | // Gyroscope variables and functions |
drelliak | 0:88faaa1afb83 | 39 | float gyro_data[3], gyro_angle; |
drelliak | 0:88faaa1afb83 | 40 | Timer t; |
drelliak | 0:88faaa1afb83 | 41 | void processGyroAngle(); |
drelliak | 0:88faaa1afb83 | 42 | void startGyro(); |
drelliak | 0:88faaa1afb83 | 43 | void stopGyro(); |
drelliak | 0:88faaa1afb83 | 44 | |
drelliak | 0:88faaa1afb83 | 45 | // Magnetometer variables and functions |
drelliak | 0:88faaa1afb83 | 46 | float max_x, max_y, min_x, min_y,x,y; |
drelliak | 0:88faaa1afb83 | 47 | MotionSensorDataUnits mag_data; |
drelliak | 0:88faaa1afb83 | 48 | float processMagAngle(); |
drelliak | 0:88faaa1afb83 | 49 | void magCal(); |
drelliak | 0:88faaa1afb83 | 50 | |
drelliak | 8:a1067fcde341 | 51 | // State variables and functions |
drelliak | 8:a1067fcde341 | 52 | float feedback, velocity = 0, jog_duty_cycle, jog_period; |
drelliak | 8:a1067fcde341 | 53 | bool alternate_motor = 0; |
drelliak | 0:88faaa1afb83 | 54 | void readProtocol(); |
drelliak | 0:88faaa1afb83 | 55 | void brakeMotor(); |
drelliak | 6:c930555fd872 | 56 | void reverseMotor(int speed); |
drelliak | 6:c930555fd872 | 57 | void setVelocity(int new_velocity); |
drelliak | 8:a1067fcde341 | 58 | void motorAlternation(); |
drelliak | 8:a1067fcde341 | 59 | Ticker interruption; |
drelliak | 6:c930555fd872 | 60 | |
drelliak | 0:88faaa1afb83 | 61 | // Test functions |
drelliak | 0:88faaa1afb83 | 62 | void debug(); |
drelliak | 0:88faaa1afb83 | 63 | |
drelliak | 0:88faaa1afb83 | 64 | int main(){ |
drelliak | 8:a1067fcde341 | 65 | //printf("Initializing controller....\r\n\r\n"); |
drelliak | 7:27516a2b504b | 66 | initializeController(); |
drelliak | 8:a1067fcde341 | 67 | //printf("Controller Initialized. \r\n"); |
drelliak | 8:a1067fcde341 | 68 | //magCal(); |
drelliak | 8:a1067fcde341 | 69 | gyro_angle = 0;//processMagAngle(); |
drelliak | 8:a1067fcde341 | 70 | ser.baud(9600); |
drelliak | 8:a1067fcde341 | 71 | ser.format(8,SerialBase::None,1); |
drelliak | 7:27516a2b504b | 72 | velocity = MINIMUM_VELOCITY; |
drelliak | 7:27516a2b504b | 73 | setMotorPWM(velocity,motor); |
drelliak | 7:27516a2b504b | 74 | startGyro(); |
drelliak | 8:a1067fcde341 | 75 | while (true){ |
drelliak | 7:27516a2b504b | 76 | processGyroAngle(); |
drelliak | 7:27516a2b504b | 77 | controlAnglePID(P,I,D,N); |
drelliak | 7:27516a2b504b | 78 | if(t.read_us() < Ts*1000000) |
drelliak | 7:27516a2b504b | 79 | wait_us(Ts*1000000 - t.read_us()); |
drelliak | 8:a1067fcde341 | 80 | if(ser.readable()) |
drelliak | 7:27516a2b504b | 81 | readProtocol(); |
drelliak | 7:27516a2b504b | 82 | } |
drelliak | 0:88faaa1afb83 | 83 | } |
drelliak | 8:a1067fcde341 | 84 | void motorAlternation() { |
drelliak | 8:a1067fcde341 | 85 | interruption.detach(); |
drelliak | 8:a1067fcde341 | 86 | if(!alternate_motor){ |
drelliak | 8:a1067fcde341 | 87 | setMotorPWM(velocity, motor); |
drelliak | 8:a1067fcde341 | 88 | interruption.attach(&motorAlternation, jog_duty_cycle*jog_period); |
drelliak | 8:a1067fcde341 | 89 | } |
drelliak | 8:a1067fcde341 | 90 | else{ |
drelliak | 8:a1067fcde341 | 91 | setMotorPWM(10, motor); |
drelliak | 8:a1067fcde341 | 92 | interruption.attach(&motorAlternation, (1-jog_duty_cycle)*jog_period); |
drelliak | 8:a1067fcde341 | 93 | } |
drelliak | 8:a1067fcde341 | 94 | alternate_motor = !alternate_motor; |
drelliak | 8:a1067fcde341 | 95 | } |
drelliak | 8:a1067fcde341 | 96 | |
drelliak | 0:88faaa1afb83 | 97 | void readProtocol(){ |
drelliak | 8:a1067fcde341 | 98 | printf("Entrei no protocolo!\n\r"); |
drelliak | 0:88faaa1afb83 | 99 | char msg = ser.getc(); |
drelliak | 8:a1067fcde341 | 100 | printf("%d \n\r", (int)msg); |
drelliak | 0:88faaa1afb83 | 101 | switch(msg) |
drelliak | 0:88faaa1afb83 | 102 | { |
drelliak | 0:88faaa1afb83 | 103 | case NONE: |
drelliak | 0:88faaa1afb83 | 104 | //ser.printf("sending red signal to led\r\n"); |
drelliak | 0:88faaa1afb83 | 105 | return; |
drelliak | 0:88faaa1afb83 | 106 | break; |
drelliak | 0:88faaa1afb83 | 107 | case BRAKE: |
drelliak | 8:a1067fcde341 | 108 | //ser.printf("BRAKE!r\n"); |
drelliak | 0:88faaa1afb83 | 109 | brakeMotor(); |
drelliak | 0:88faaa1afb83 | 110 | break; |
drelliak | 0:88faaa1afb83 | 111 | case ANG_RST: |
drelliak | 0:88faaa1afb83 | 112 | //ser.printf("sending blue signal to led\r\n"); |
drelliak | 0:88faaa1afb83 | 113 | stopGyro(); |
drelliak | 0:88faaa1afb83 | 114 | gyro_angle = 0; |
drelliak | 0:88faaa1afb83 | 115 | startGyro(); |
drelliak | 0:88faaa1afb83 | 116 | return; |
drelliak | 0:88faaa1afb83 | 117 | break; |
drelliak | 0:88faaa1afb83 | 118 | case ANG_REF: |
drelliak | 0:88faaa1afb83 | 119 | reference = get_ang_ref(ser); |
drelliak | 0:88faaa1afb83 | 120 | break; |
drelliak | 8:a1067fcde341 | 121 | case GND_VEL: |
drelliak | 8:a1067fcde341 | 122 | //ser.printf("GND_VEL\r\n"); |
drelliak | 8:a1067fcde341 | 123 | velocity = get_gnd_vel(ser); |
drelliak | 8:a1067fcde341 | 124 | //ser.printf("%f\r\n",velocity); |
drelliak | 8:a1067fcde341 | 125 | //interruption.detach(); |
drelliak | 0:88faaa1afb83 | 126 | break; |
drelliak | 0:88faaa1afb83 | 127 | case PID_PARAMS: |
drelliak | 0:88faaa1afb83 | 128 | get_pid_params(ser, &P, &I, &D, &N); |
drelliak | 0:88faaa1afb83 | 129 | break; |
drelliak | 8:a1067fcde341 | 130 | case JOG_VEL: |
drelliak | 8:a1067fcde341 | 131 | get_jog_vel(ser,&jog_period,&jog_duty_cycle); |
drelliak | 8:a1067fcde341 | 132 | interruption.attach(&motorAlternation, jog_duty_cycle*jog_period); |
drelliak | 8:a1067fcde341 | 133 | break; |
drelliak | 0:88faaa1afb83 | 134 | default: |
drelliak | 8:a1067fcde341 | 135 | ser.printf("GARBAGE\r\n"); |
drelliak | 0:88faaa1afb83 | 136 | // ser.flush(); |
drelliak | 0:88faaa1afb83 | 137 | |
drelliak | 0:88faaa1afb83 | 138 | } |
drelliak | 0:88faaa1afb83 | 139 | } |
drelliak | 0:88faaa1afb83 | 140 | /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */ |
drelliak | 0:88faaa1afb83 | 141 | void initializeController(){ |
drelliak | 0:88faaa1afb83 | 142 | for(int i =0; i<2; i++){ |
drelliak | 0:88faaa1afb83 | 143 | e[i] = 0; |
drelliak | 0:88faaa1afb83 | 144 | ui[i] = 0; |
drelliak | 0:88faaa1afb83 | 145 | ud[i] = 0; |
drelliak | 0:88faaa1afb83 | 146 | } |
drelliak | 0:88faaa1afb83 | 147 | P= INITIAL_P; |
drelliak | 0:88faaa1afb83 | 148 | I= INITIAL_I; |
drelliak | 0:88faaa1afb83 | 149 | D= INITIAL_D; |
drelliak | 0:88faaa1afb83 | 150 | N= INITIAL_N; |
drelliak | 0:88faaa1afb83 | 151 | } |
drelliak | 0:88faaa1afb83 | 152 | |
drelliak | 0:88faaa1afb83 | 153 | /* Start the Gyroscope timer and set the initial configuration */ |
drelliak | 0:88faaa1afb83 | 154 | void startGyro(){ |
drelliak | 0:88faaa1afb83 | 155 | gyro.gyro_config(); |
drelliak | 0:88faaa1afb83 | 156 | t.start(); |
drelliak | 0:88faaa1afb83 | 157 | } |
drelliak | 0:88faaa1afb83 | 158 | |
drelliak | 0:88faaa1afb83 | 159 | /* Stop and reset the Gyroscope */ |
drelliak | 0:88faaa1afb83 | 160 | void stopGyro(){ |
drelliak | 0:88faaa1afb83 | 161 | t.stop(); |
drelliak | 0:88faaa1afb83 | 162 | t.reset(); |
drelliak | 0:88faaa1afb83 | 163 | gyro_angle = 0; |
drelliak | 0:88faaa1afb83 | 164 | } |
drelliak | 0:88faaa1afb83 | 165 | |
drelliak | 0:88faaa1afb83 | 166 | /* Integrate the Gyroscope to get the angular position (Deegre/seconds) */ |
drelliak | 0:88faaa1afb83 | 167 | void processGyroAngle(){ |
drelliak | 0:88faaa1afb83 | 168 | gyro.acquire_gyro_data_dps(gyro_data); |
drelliak | 0:88faaa1afb83 | 169 | t.stop(); |
drelliak | 0:88faaa1afb83 | 170 | gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000; |
drelliak | 0:88faaa1afb83 | 171 | t.reset(); |
drelliak | 0:88faaa1afb83 | 172 | t.start(); |
drelliak | 4:8ead8ada8a8b | 173 | feedback = gyro_angle; |
drelliak | 4:8ead8ada8a8b | 174 | if(feedback > 180) |
drelliak | 4:8ead8ada8a8b | 175 | feedback = feedback - 360; |
drelliak | 4:8ead8ada8a8b | 176 | if(feedback < -180) |
drelliak | 4:8ead8ada8a8b | 177 | feedback = feedback + 360; |
drelliak | 0:88faaa1afb83 | 178 | } |
drelliak | 0:88faaa1afb83 | 179 | |
drelliak | 0:88faaa1afb83 | 180 | /* PID controller for angular position */ |
drelliak | 0:88faaa1afb83 | 181 | void controlAnglePID(float P, float I, float D, float N){ |
drelliak | 0:88faaa1afb83 | 182 | /* Getting error */ |
drelliak | 0:88faaa1afb83 | 183 | e[1] = e[0]; |
drelliak | 4:8ead8ada8a8b | 184 | e[0] = reference - (feedback*PI/180); |
drelliak | 0:88faaa1afb83 | 185 | if(e[0] > PI) |
drelliak | 0:88faaa1afb83 | 186 | e[0]= e[0] - 2*PI; |
drelliak | 0:88faaa1afb83 | 187 | if(e[0] < -PI) |
drelliak | 0:88faaa1afb83 | 188 | e[0] = e[0] + 2*PI; |
drelliak | 8:a1067fcde341 | 189 | if(abs(e[0]) > PI/6){ |
drelliak | 8:a1067fcde341 | 190 | setMotorPWM(MINIMUM_VELOCITY,motor); |
drelliak | 8:a1067fcde341 | 191 | } |
drelliak | 8:a1067fcde341 | 192 | else{ |
drelliak | 8:a1067fcde341 | 193 | setMotorPWM(velocity,motor); |
drelliak | 8:a1067fcde341 | 194 | } |
drelliak | 0:88faaa1afb83 | 195 | /* Proportinal Part */ |
drelliak | 0:88faaa1afb83 | 196 | up[0] = e[0]*P; |
drelliak | 0:88faaa1afb83 | 197 | /* Integral Part */ |
drelliak | 0:88faaa1afb83 | 198 | ui[1] = ui[0]; |
drelliak | 0:88faaa1afb83 | 199 | if(abs(u) < PI/8){ |
drelliak | 0:88faaa1afb83 | 200 | ui[0] = (P*I*Ts)*e[1] + ui[1]; |
drelliak | 0:88faaa1afb83 | 201 | } |
drelliak | 0:88faaa1afb83 | 202 | else if(u > 0) |
drelliak | 0:88faaa1afb83 | 203 | ui[0] = PI/8 - up[0]; |
drelliak | 0:88faaa1afb83 | 204 | else if(u < 0) |
drelliak | 0:88faaa1afb83 | 205 | ui[0] = -PI/8 - up[0]; |
drelliak | 0:88faaa1afb83 | 206 | /* Derivative Part */ |
drelliak | 0:88faaa1afb83 | 207 | ud[1] = ud[0]; |
drelliak | 0:88faaa1afb83 | 208 | ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); |
drelliak | 0:88faaa1afb83 | 209 | /** Controller **/ |
drelliak | 0:88faaa1afb83 | 210 | u = up[0] + ud[0] + ui[0]; |
drelliak | 0:88faaa1afb83 | 211 | setServoPWM(u*100/(PI/8), servo); |
drelliak | 0:88faaa1afb83 | 212 | } |
drelliak | 0:88faaa1afb83 | 213 | /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */ |
drelliak | 0:88faaa1afb83 | 214 | void brakeMotor(){ |
drelliak | 6:c930555fd872 | 215 | if(velocity >= 0){ |
drelliak | 3:e213c44a9f6c | 216 | setMotorPWM(-BRAKE_CONSTANT, motor); |
drelliak | 6:c930555fd872 | 217 | wait(BRAKE_WAIT); |
drelliak | 6:c930555fd872 | 218 | velocity = 0; |
drelliak | 6:c930555fd872 | 219 | setMotorPWM(velocity,motor); |
drelliak | 6:c930555fd872 | 220 | } |
drelliak | 6:c930555fd872 | 221 | else { |
drelliak | 6:c930555fd872 | 222 | setVelocity(0); |
drelliak | 6:c930555fd872 | 223 | } |
drelliak | 6:c930555fd872 | 224 | } |
drelliak | 6:c930555fd872 | 225 | void reverseMotor(int speed){ |
drelliak | 6:c930555fd872 | 226 | for(int i=0 ; i >= -speed; i--){ |
drelliak | 6:c930555fd872 | 227 | setMotorPWM((float)i,motor); |
drelliak | 6:c930555fd872 | 228 | wait_ms(13.5); |
drelliak | 6:c930555fd872 | 229 | } |
drelliak | 6:c930555fd872 | 230 | for(int i=-speed ; i <= 0; i++){ |
drelliak | 6:c930555fd872 | 231 | setMotorPWM((float)i,motor); |
drelliak | 6:c930555fd872 | 232 | wait_ms(13.5); |
drelliak | 6:c930555fd872 | 233 | } |
drelliak | 6:c930555fd872 | 234 | for(int i=0 ; i >= -speed; i--){ |
drelliak | 6:c930555fd872 | 235 | setMotorPWM((float)i,motor); |
drelliak | 6:c930555fd872 | 236 | wait_ms(13.5); |
drelliak | 6:c930555fd872 | 237 | } |
drelliak | 6:c930555fd872 | 238 | } |
drelliak | 6:c930555fd872 | 239 | void setVelocity(int new_velocity){ |
drelliak | 6:c930555fd872 | 240 | if( velocity > new_velocity){ |
drelliak | 6:c930555fd872 | 241 | for(; velocity >= new_velocity; velocity--){ |
drelliak | 6:c930555fd872 | 242 | setMotorPWM(velocity,motor); |
drelliak | 6:c930555fd872 | 243 | wait_ms(PWM_PERIOD); |
drelliak | 6:c930555fd872 | 244 | } |
drelliak | 6:c930555fd872 | 245 | velocity++; |
drelliak | 6:c930555fd872 | 246 | } |
drelliak | 6:c930555fd872 | 247 | else if(velocity < new_velocity){ |
drelliak | 6:c930555fd872 | 248 | for(; velocity <= new_velocity; velocity++){ |
drelliak | 6:c930555fd872 | 249 | setMotorPWM(velocity,motor); |
drelliak | 6:c930555fd872 | 250 | wait_ms(PWM_PERIOD); |
drelliak | 6:c930555fd872 | 251 | } |
drelliak | 6:c930555fd872 | 252 | velocity--; |
drelliak | 6:c930555fd872 | 253 | } |
drelliak | 0:88faaa1afb83 | 254 | } |
drelliak | 0:88faaa1afb83 | 255 | /* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used */ |
drelliak | 0:88faaa1afb83 | 256 | /* in the main loop or the controller performance may be affected. */ |
drelliak | 0:88faaa1afb83 | 257 | void debug(){ |
drelliak | 0:88faaa1afb83 | 258 | //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8)); |
drelliak | 0:88faaa1afb83 | 259 | //printf("Erro: %f Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI); |
drelliak | 4:8ead8ada8a8b | 260 | printf(" %f \r\n",feedback); |
drelliak | 0:88faaa1afb83 | 261 | } |
drelliak | 0:88faaa1afb83 | 262 | |
drelliak | 0:88faaa1afb83 | 263 | /* Function to normalize the magnetometer reading */ |
drelliak | 0:88faaa1afb83 | 264 | void magCal(){ |
drelliak | 0:88faaa1afb83 | 265 | printf("Starting Calibration"); |
drelliak | 0:88faaa1afb83 | 266 | mag.enable(); |
drelliak | 0:88faaa1afb83 | 267 | wait(0.01); |
drelliak | 0:88faaa1afb83 | 268 | mag.getAxis(mag_data); |
drelliak | 0:88faaa1afb83 | 269 | float x0 = max_x = min_y = mag_data.x; |
drelliak | 0:88faaa1afb83 | 270 | float y0 = max_y = min_y = mag_data.y; |
drelliak | 0:88faaa1afb83 | 271 | bool began = false; |
drelliak | 0:88faaa1afb83 | 272 | while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){ |
drelliak | 0:88faaa1afb83 | 273 | mag.getAxis(mag_data); |
drelliak | 0:88faaa1afb83 | 274 | if(mag_data.x > max_x) |
drelliak | 0:88faaa1afb83 | 275 | max_x = mag_data.x; |
drelliak | 0:88faaa1afb83 | 276 | if(mag_data.y > max_y) |
drelliak | 0:88faaa1afb83 | 277 | max_y = mag_data.y; |
drelliak | 0:88faaa1afb83 | 278 | if(mag_data.y < min_y) |
drelliak | 0:88faaa1afb83 | 279 | min_y = mag_data.y; |
drelliak | 0:88faaa1afb83 | 280 | if(mag_data.x < min_x) |
drelliak | 0:88faaa1afb83 | 281 | min_x = mag_data.x; |
drelliak | 0:88faaa1afb83 | 282 | if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH) |
drelliak | 0:88faaa1afb83 | 283 | began = true; |
drelliak | 0:88faaa1afb83 | 284 | 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 | 285 | } |
drelliak | 0:88faaa1afb83 | 286 | 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 | 287 | } |
drelliak | 0:88faaa1afb83 | 288 | |
drelliak | 0:88faaa1afb83 | 289 | /* Function to transform the magnetometer reading in angle(rad/s).*/ |
drelliak | 0:88faaa1afb83 | 290 | float processMagAngle(){ |
drelliak | 0:88faaa1afb83 | 291 | mag.getAxis(mag_data); |
drelliak | 0:88faaa1afb83 | 292 | x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1; |
drelliak | 0:88faaa1afb83 | 293 | y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1; |
drelliak | 0:88faaa1afb83 | 294 | return atan2(y,x); |
drelliak | 0:88faaa1afb83 | 295 | } |