Ryan Mauricio / Mbed 2 deprecated SAMUELFINAL_BACKUP

Dependencies:   mbed Servo DebounceIn

main.cpp@2:7b7060835269, 2019-06-26 (annotated)

Committer:
ryanzero
Date:
Wed Jun 26 00:02:36 2019 +0000
Revision:
2:7b7060835269
Parent:
1:0158e4d78423
Child:
3:6e4ca952e920
Projeto Samuel;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manitou 0:31cc139b7d1e 1 /* MPU9250 Basic Example Code
manitou 0:31cc139b7d1e 2 by: Kris Winer
manitou 0:31cc139b7d1e 3 date: April 1, 2014
manitou 1:0158e4d78423 4 license: Beerware - Use this code however you'd like. If you
manitou 0:31cc139b7d1e 5 find it useful you can buy me a beer some time.
manitou 1:0158e4d78423 6
manitou 1:0158e4d78423 7 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
manitou 1:0158e4d78423 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
manitou 1:0158e4d78423 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
manitou 0:31cc139b7d1e 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
manitou 1:0158e4d78423 11
manitou 0:31cc139b7d1e 12 SDA and SCL should have external pull-up resistors (to 3.3V).
manitou 0:31cc139b7d1e 13 10k resistors are on the EMSENSR-9250 breakout board.
manitou 1:0158e4d78423 14
manitou 0:31cc139b7d1e 15 Hardware setup:
manitou 0:31cc139b7d1e 16 MPU9250 Breakout --------- Arduino
manitou 0:31cc139b7d1e 17 VDD ---------------------- 3.3V
manitou 0:31cc139b7d1e 18 VDDI --------------------- 3.3V
manitou 0:31cc139b7d1e 19 SDA ----------------------- A4
manitou 0:31cc139b7d1e 20 SCL ----------------------- A5
manitou 0:31cc139b7d1e 21 GND ---------------------- GND
manitou 1:0158e4d78423 22
manitou 1:0158e4d78423 23 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
manitou 0:31cc139b7d1e 24 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
manitou 0:31cc139b7d1e 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
manitou 0:31cc139b7d1e 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
manitou 0:31cc139b7d1e 27 */
manitou 1:0158e4d78423 28
manitou 1:0158e4d78423 29 //#include "ST_F401_84MHZ.h"
manitou 0:31cc139b7d1e 30 //F401_init84 myinit(0);
manitou 0:31cc139b7d1e 31 #include "mbed.h"
manitou 0:31cc139b7d1e 32 #include "MPU9250.h"
ryanzero 2:7b7060835269 33 #include "math.h"
manitou 0:31cc139b7d1e 34
ryanzero 2:7b7060835269 35 AnalogIn pot(A0);
manitou 0:31cc139b7d1e 36
manitou 0:31cc139b7d1e 37 float sum = 0;
manitou 0:31cc139b7d1e 38 uint32_t sumCount = 0;
manitou 0:31cc139b7d1e 39
manitou 1:0158e4d78423 40 MPU9250 mpu9250;
manitou 0:31cc139b7d1e 41
ryanzero 2:7b7060835269 42 int i;
ryanzero 2:7b7060835269 43
ryanzero 2:7b7060835269 44 DigitalOut myled(LED1);
ryanzero 2:7b7060835269 45
ryanzero 2:7b7060835269 46 float axf=0, ayf=0, azf=0, soma=0, naxf=0, Flag_botao=0, Flag_naxf=0, norma=0, norma2=0, g=0, Flag_Norma=0, Flag_axf=0, Flag_Volante=1;
ryanzero 2:7b7060835269 47 float pr[10];
ryanzero 2:7b7060835269 48
manitou 1:0158e4d78423 49 Timer t;
manitou 1:0158e4d78423 50
manitou 1:0158e4d78423 51 Serial pc(USBTX, USBRX); // tx, rx
manitou 0:31cc139b7d1e 52
manitou 0:31cc139b7d1e 53 volatile bool newData = false;
manitou 0:31cc139b7d1e 54
manitou 0:31cc139b7d1e 55 InterruptIn isrPin(D12); //k64 D12 dragon PD_0
manitou 0:31cc139b7d1e 56
ryanzero 2:7b7060835269 57 InterruptIn button1(USER_BUTTON);
ryanzero 2:7b7060835269 58 volatile bool button1_pressed = false; // Used in the main loop
ryanzero 2:7b7060835269 59 volatile bool button1_enabled = true; // Used for debouncing
ryanzero 2:7b7060835269 60 Timeout button1_timeout; // Used for debouncing
ryanzero 2:7b7060835269 61
ryanzero 2:7b7060835269 62 // Enables button when bouncing is over
ryanzero 2:7b7060835269 63 void button1_enabled_cb(void)
ryanzero 2:7b7060835269 64 {
ryanzero 2:7b7060835269 65 button1_enabled = true;
ryanzero 2:7b7060835269 66 }
ryanzero 2:7b7060835269 67
ryanzero 2:7b7060835269 68 // ISR handling button pressed event
ryanzero 2:7b7060835269 69 void button1_onpressed_cb(void)
ryanzero 2:7b7060835269 70 {
ryanzero 2:7b7060835269 71 if (button1_enabled) { // Disabled while the button is bouncing
ryanzero 2:7b7060835269 72 button1_enabled = false;
ryanzero 2:7b7060835269 73 button1_pressed = true; // To be read by the main loop
ryanzero 2:7b7060835269 74 button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
ryanzero 2:7b7060835269 75 }
ryanzero 2:7b7060835269 76 }
ryanzero 2:7b7060835269 77
manitou 1:0158e4d78423 78 void mpuisr()
manitou 1:0158e4d78423 79 {
manitou 0:31cc139b7d1e 80 newData=true;
manitou 0:31cc139b7d1e 81 }
manitou 1:0158e4d78423 82
manitou 0:31cc139b7d1e 83 int main()
manitou 0:31cc139b7d1e 84 {
manitou 1:0158e4d78423 85 pc.baud(9600);
manitou 1:0158e4d78423 86
manitou 1:0158e4d78423 87 //Set up I2C
manitou 1:0158e4d78423 88 i2c.frequency(400000); // use fast (400 kHz) I2C
manitou 1:0158e4d78423 89
ryanzero 2:7b7060835269 90 //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
manitou 1:0158e4d78423 91
manitou 1:0158e4d78423 92 t.start();
manitou 1:0158e4d78423 93 isrPin.rise(&mpuisr);
manitou 0:31cc139b7d1e 94
manitou 1:0158e4d78423 95 // Read the WHO_AM_I register, this is a good test of communication
manitou 1:0158e4d78423 96 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
ryanzero 2:7b7060835269 97 //pc.printf("I AM 0x%x\n\r", whoami);
ryanzero 2:7b7060835269 98 //pc.printf("I SHOULD BE 0x71\n\r");
manitou 1:0158e4d78423 99
ryanzero 2:7b7060835269 100 if (whoami == 0x73) { // WHO_AM_I should always be 0x68
ryanzero 2:7b7060835269 101 //pc.printf("MPU9250 is online...\n\r");
manitou 1:0158e4d78423 102 wait(1);
manitou 1:0158e4d78423 103
manitou 0:31cc139b7d1e 104
manitou 1:0158e4d78423 105 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
manitou 1:0158e4d78423 106 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ryanzero 2:7b7060835269 107 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
ryanzero 2:7b7060835269 108 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
ryanzero 2:7b7060835269 109 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
ryanzero 2:7b7060835269 110 //pc.printf("x accel bias = %f\n\r", accelBias[0]);
ryanzero 2:7b7060835269 111 //pc.printf("y accel bias = %f\n\r", accelBias[1]);
ryanzero 2:7b7060835269 112 //pc.printf("z accel bias = %f\n\r", accelBias[2]);
manitou 1:0158e4d78423 113 wait(2);
manitou 1:0158e4d78423 114 mpu9250.initMPU9250();
ryanzero 2:7b7060835269 115 //pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
manitou 1:0158e4d78423 116 mpu9250.initAK8963(magCalibration);
ryanzero 2:7b7060835269 117 //pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
ryanzero 2:7b7060835269 118 //pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
ryanzero 2:7b7060835269 119 //pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
ryanzero 2:7b7060835269 120 //if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
ryanzero 2:7b7060835269 121 //if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
ryanzero 2:7b7060835269 122 //if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
ryanzero 2:7b7060835269 123 //if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
manitou 1:0158e4d78423 124 wait(2);
ryanzero 2:7b7060835269 125 }
ryanzero 2:7b7060835269 126 else {
ryanzero 2:7b7060835269 127 //pc.printf("Could not connect to MPU9250: \n\r");
ryanzero 2:7b7060835269 128 //pc.printf("%#x \n", whoami);
manitou 0:31cc139b7d1e 129
manitou 1:0158e4d78423 130
manitou 1:0158e4d78423 131 while(1) ; // Loop forever if communication doesn't happen
manitou 0:31cc139b7d1e 132 }
manitou 0:31cc139b7d1e 133
manitou 0:31cc139b7d1e 134 mpu9250.getAres(); // Get accelerometer sensitivity
manitou 0:31cc139b7d1e 135 mpu9250.getGres(); // Get gyro sensitivity
manitou 0:31cc139b7d1e 136 mpu9250.getMres(); // Get magnetometer sensitivity
ryanzero 2:7b7060835269 137 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
ryanzero 2:7b7060835269 138 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
ryanzero 2:7b7060835269 139 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
manitou 0:31cc139b7d1e 140 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
manitou 0:31cc139b7d1e 141 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
manitou 0:31cc139b7d1e 142 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
manitou 0:31cc139b7d1e 143
ryanzero 2:7b7060835269 144 while(1) { //abertur do primeiro while
ryanzero 2:7b7060835269 145 static int readycnt=0;
ryanzero 2:7b7060835269 146 // If intPin goes high, all data registers have new data
manitou 1:0158e4d78423 147
manitou 0:31cc139b7d1e 148 #if USE_ISR
ryanzero 2:7b7060835269 149 if(newData) {//if num 1
ryanzero 2:7b7060835269 150 newData=false;
ryanzero 2:7b7060835269 151 mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS); //? need this with ISR
manitou 0:31cc139b7d1e 152 #else
ryanzero 2:7b7060835269 153 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { //if num2 // On interrupt, check if data ready interrupt
ryanzero 2:7b7060835269 154 #endif
ryanzero 2:7b7060835269 155 readycnt++;
ryanzero 2:7b7060835269 156 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
ryanzero 2:7b7060835269 157 // Now we'll calculate the accleration value into actual g's
ryanzero 2:7b7060835269 158 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ryanzero 2:7b7060835269 159 ay = (float)accelCount[1]*aRes - accelBias[1];
ryanzero 2:7b7060835269 160 az = (float)accelCount[2]*aRes - accelBias[2];
ryanzero 2:7b7060835269 161
ryanzero 2:7b7060835269 162 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
ryanzero 2:7b7060835269 163 // Calculate the gyro value into actual degrees per second
ryanzero 2:7b7060835269 164 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
ryanzero 2:7b7060835269 165 gy = (float)gyroCount[1]*gRes - gyroBias[1];
ryanzero 2:7b7060835269 166 gz = (float)gyroCount[2]*gRes - gyroBias[2];
ryanzero 2:7b7060835269 167
ryanzero 2:7b7060835269 168 mpu9250.readMagData(magCount); // Read the x/y/z adc values
ryanzero 2:7b7060835269 169 // Calculate the magnetometer values in milliGauss
ryanzero 2:7b7060835269 170 // Include factory calibration per data sheet and user environmental corrections
ryanzero 2:7b7060835269 171 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
ryanzero 2:7b7060835269 172 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
ryanzero 2:7b7060835269 173 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
ryanzero 2:7b7060835269 174 }//if num 2
manitou 1:0158e4d78423 175
ryanzero 2:7b7060835269 176 Now = t.read_us();
ryanzero 2:7b7060835269 177 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
ryanzero 2:7b7060835269 178 lastUpdate = Now;
ryanzero 2:7b7060835269 179
ryanzero 2:7b7060835269 180 sum += deltat;
ryanzero 2:7b7060835269 181 sumCount++;
manitou 1:0158e4d78423 182
manitou 1:0158e4d78423 183 // Pass gyro rate as rad/s
ryanzero 2:7b7060835269 184 uint32_t us = t.read_us();
ryanzero 2:7b7060835269 185 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ryanzero 2:7b7060835269 186 us = t.read_us()-us;
ryanzero 2:7b7060835269 187 // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ryanzero 2:7b7060835269 188
ryanzero 2:7b7060835269 189 // Serial print and/or display at 0.5 s rate independent of data rates
ryanzero 2:7b7060835269 190 delt_t = t.read_ms() - count;
ryanzero 2:7b7060835269 191 if (delt_t > 500) { // update LCD once per half-second independent of read rate //if num 3
ryanzero 2:7b7060835269 192 //pc.printf("readycnt %d us %d\n",readycnt,us);
manitou 1:0158e4d78423 193 readycnt=0;
ryanzero 2:7b7060835269 194 //pc.printf("ax = %f", 1000*ax);
ryanzero 2:7b7060835269 195 //pc.printf(" ay = %f", 1000*ay);
ryanzero 2:7b7060835269 196 //pc.printf(" az = %f mg\n\r", 1000*az);
manitou 1:0158e4d78423 197
ryanzero 2:7b7060835269 198 //pc.printf("gx = %f", gx);
ryanzero 2:7b7060835269 199 //pc.printf(" gy = %f", gy);
ryanzero 2:7b7060835269 200 //pc.printf(" gz = %f deg/s\n\r", gz);
manitou 1:0158e4d78423 201
ryanzero 2:7b7060835269 202 //pc.printf("gx = %f", mx);
ryanzero 2:7b7060835269 203 //pc.printf(" gy = %f", my);
ryanzero 2:7b7060835269 204 //pc.printf(" gz = %f mG\n\r", mz);
manitou 1:0158e4d78423 205
manitou 1:0158e4d78423 206 tempCount = mpu9250.readTempData(); // Read the adc values
manitou 1:0158e4d78423 207 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
ryanzero 2:7b7060835269 208 //pc.printf("temperature = %f C\n\r", temperature);
manitou 0:31cc139b7d1e 209
ryanzero 2:7b7060835269 210 //pc.printf("q0 = %f\n\r", q[0]);
ryanzero 2:7b7060835269 211 //pc.printf("q1 = %f\n\r", q[1]);
ryanzero 2:7b7060835269 212 //pc.printf("q2 = %f\n\r", q[2]);
ryanzero 2:7b7060835269 213 //pc.printf("q3 = %f\n\r", q[3]);
ryanzero 2:7b7060835269 214
manitou 1:0158e4d78423 215 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
manitou 1:0158e4d78423 216 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
manitou 1:0158e4d78423 217 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
manitou 1:0158e4d78423 218 pitch *= 180.0f / PI;
manitou 1:0158e4d78423 219 yaw *= 180.0f / PI;
manitou 1:0158e4d78423 220 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
manitou 1:0158e4d78423 221 roll *= 180.0f / PI;
manitou 0:31cc139b7d1e 222
ryanzero 2:7b7060835269 223 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
ryanzero 2:7b7060835269 224 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
manitou 1:0158e4d78423 225
ryanzero 2:7b7060835269 226 //myled= !myled;
manitou 1:0158e4d78423 227 count = t.read_ms();
manitou 1:0158e4d78423 228 sum = 0;
manitou 1:0158e4d78423 229 sumCount = 0;
ryanzero 2:7b7060835269 230 if(i==0){
ryanzero 2:7b7060835269 231 for(i=0;i<10;i++)
ryanzero 2:7b7060835269 232 {
ryanzero 2:7b7060835269 233 pr[i] = sqrt((1000000*ax*ax)+(1000000*ay*ay)+(1000000*az*az));
ryanzero 2:7b7060835269 234 soma = soma + pr[i];
ryanzero 2:7b7060835269 235 }
ryanzero 2:7b7060835269 236
ryanzero 2:7b7060835269 237 norma = soma/(10000);
manitou 1:0158e4d78423 238 }
ryanzero 2:7b7060835269 239 axf = (ax*1000)/norma;
ryanzero 2:7b7060835269 240 ayf = (ay*1000)/norma;
ryanzero 2:7b7060835269 241 azf = (az*1000)/norma;
ryanzero 2:7b7060835269 242
ryanzero 2:7b7060835269 243 //pc.printf("norma = %f\n", norma);
ryanzero 2:7b7060835269 244 pc.printf("axf = %f\n", axf);
ryanzero 2:7b7060835269 245 pc.printf("ayf = %f\n", ayf);
ryanzero 2:7b7060835269 246 pc.printf("azf = %f\n", azf);
ryanzero 2:7b7060835269 247
ryanzero 2:7b7060835269 248 norma2=sqrt((axf*axf)+(ayf*ayf)+(azf*azf));
ryanzero 2:7b7060835269 249 g=1000;
ryanzero 2:7b7060835269 250
ryanzero 2:7b7060835269 251 button1.fall(callback(button1_onpressed_cb));
ryanzero 2:7b7060835269 252
ryanzero 2:7b7060835269 253 if (button1_pressed) {
ryanzero 2:7b7060835269 254 button1_pressed = false;
ryanzero 2:7b7060835269 255 Flag_botao=1;
ryanzero 2:7b7060835269 256 };
ryanzero 2:7b7060835269 257
ryanzero 2:7b7060835269 258 if(norma2>1000){
ryanzero 2:7b7060835269 259 Flag_Norma=1;
ryanzero 2:7b7060835269 260 }else{
ryanzero 2:7b7060835269 261 Flag_Norma=0;
ryanzero 2:7b7060835269 262 }
ryanzero 2:7b7060835269 263
ryanzero 2:7b7060835269 264 if(axf>-10){
ryanzero 2:7b7060835269 265 Flag_axf=1;
ryanzero 2:7b7060835269 266 }else{
ryanzero 2:7b7060835269 267 Flag_axf=0;
ryanzero 2:7b7060835269 268 }
ryanzero 2:7b7060835269 269
ryanzero 2:7b7060835269 270 if(naxf<-100){
ryanzero 2:7b7060835269 271 Flag_naxf=1;
ryanzero 2:7b7060835269 272 }else{
ryanzero 2:7b7060835269 273 Flag_naxf=0;
ryanzero 2:7b7060835269 274 }
ryanzero 2:7b7060835269 275
ryanzero 2:7b7060835269 276
ryanzero 2:7b7060835269 277 float angle = pot.read()*3600;
ryanzero 2:7b7060835269 278
ryanzero 2:7b7060835269 279 if ((angle < 1050) && (angle > 950) )
ryanzero 2:7b7060835269 280 {
ryanzero 2:7b7060835269 281 Flag_Volante= 1;
ryanzero 2:7b7060835269 282 }else{
ryanzero 2:7b7060835269 283 Flag_Volante=0;
ryanzero 2:7b7060835269 284 }
ryanzero 2:7b7060835269 285
ryanzero 2:7b7060835269 286 if(Flag_Volante==1&&Flag_axf==1&&Flag_botao==1&&Flag_DRS=0) {
ryanzero 2:7b7060835269 287 myled = 0;
ryanzero 2:7b7060835269 288 Flag_botao=0;
ryanzero 2:7b7060835269 289 pc.printf("drs disponivel\n");
ryanzero 2:7b7060835269 290 Flag_DRS=1;
ryanzero 2:7b7060835269 291 } else{
ryanzero 2:7b7060835269 292 pc.printf("drs nao disponivel\n");
ryanzero 2:7b7060835269 293 }
ryanzero 2:7b7060835269 294
ryanzero 2:7b7060835269 295 if (Flag_Volante==0 || Flag_naxf==1) {
ryanzero 2:7b7060835269 296 myled = 1;
ryanzero 2:7b7060835269 297 pc.printf("desativando drs\n");
ryanzero 2:7b7060835269 298 Flag_DRS=0;
ryanzero 2:7b7060835269 299 };
ryanzero 2:7b7060835269 300
ryanzero 2:7b7060835269 301
ryanzero 2:7b7060835269 302 pc.printf("angulo: %f\n", angle);
ryanzero 2:7b7060835269 303 pc.printf("percentage: %3.3f%%\n", pot.read()*100.0f);
ryanzero 2:7b7060835269 304 pc.printf("Flag Volante %f\n", Flag_Volante);
ryanzero 2:7b7060835269 305 pc.printf("Flag axf %f\n", Flag_axf);
ryanzero 2:7b7060835269 306 pc.printf("Flag Norma %f\n", Flag_Norma);
ryanzero 2:7b7060835269 307 pc.printf("Norma %f\n\n", norma2);
ryanzero 2:7b7060835269 308 wait(0.2);
ryanzero 2:7b7060835269 309
ryanzero 2:7b7060835269 310
ryanzero 2:7b7060835269 311
ryanzero 2:7b7060835269 312 }
ryanzero 2:7b7060835269 313
manitou 1:0158e4d78423 314 }
manitou 1:0158e4d78423 315
ryanzero 2:7b7060835269 316 }
ryanzero 2:7b7060835269 317
ryanzero 2:7b7060835269 318
ryanzero 2:7b7060835269 319