TRR 2018 / Mbed 2 deprecated test_hw_biniou

test hardware

Dependencies:   mbed MPU6050 PulseWidthCounter PulseCounter

main.cpp@1:b1f5ab5f08a2, 2019-09-28 (annotated)

Committer:
ohlimi2
Date:
Sat Sep 28 10:06:37 2019 +0000
Revision:
1:b1f5ab5f08a2
Parent:
0:0232ed5c62b6 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ohlimi2 0:0232ed5c62b6 1 #include "mbed.h"
ohlimi2 0:0232ed5c62b6 2 #include "MPU6050.h"
ohlimi2 0:0232ed5c62b6 3
ohlimi2 0:0232ed5c62b6 4 // declaration des entrées analogiques
ohlimi2 0:0232ed5c62b6 5 AnalogIn analog_value_PA4(PA_4);
ohlimi2 0:0232ed5c62b6 6 AnalogIn analog_value_PA5(PA_5);
ohlimi2 0:0232ed5c62b6 7 AnalogIn analog_value_PA6(PA_6);
ohlimi2 0:0232ed5c62b6 8 AnalogIn analog_value_PC0(PC_0);
ohlimi2 0:0232ed5c62b6 9
ohlimi2 0:0232ed5c62b6 10 AnalogIn analog_value_PF3(PF_3);
ohlimi2 0:0232ed5c62b6 11 AnalogIn analog_value_PF4(PF_4);
ohlimi2 0:0232ed5c62b6 12 AnalogIn analog_value_PF9(PF_9);
ohlimi2 0:0232ed5c62b6 13 AnalogIn analog_value_PF10(PF_10);
ohlimi2 0:0232ed5c62b6 14
ohlimi2 0:0232ed5c62b6 15 // declaration des sorties PWM
ohlimi2 0:0232ed5c62b6 16 PwmOut pwm_PE9(PE_9);
ohlimi2 0:0232ed5c62b6 17 PwmOut pwm_PF7(PF_7);
ohlimi2 0:0232ed5c62b6 18 PwmOut pwm_PF6(PF_6);
ohlimi2 0:0232ed5c62b6 19 PwmOut pwm_PC8(PC_8);
ohlimi2 0:0232ed5c62b6 20 PwmOut pwm_PD12(PD_12);
ohlimi2 0:0232ed5c62b6 21
ohlimi2 0:0232ed5c62b6 22 // declaration des IO
ohlimi2 0:0232ed5c62b6 23
ohlimi2 0:0232ed5c62b6 24 DigitalOut led(LED1);
ohlimi2 1:b1f5ab5f08a2 25 DigitalOut enable_esc(PG_0);
ohlimi2 1:b1f5ab5f08a2 26
ohlimi2 1:b1f5ab5f08a2 27 InterruptIn odoInput(PF_8);
ohlimi2 1:b1f5ab5f08a2 28
ohlimi2 1:b1f5ab5f08a2 29 #define ROUE_DISTANCE_TIC_2019 0.06
ohlimi2 1:b1f5ab5f08a2 30 #define ROUE_DISTANCE_TIC_2018 0.08
ohlimi2 1:b1f5ab5f08a2 31
ohlimi2 1:b1f5ab5f08a2 32 float roueDistanceTic = ROUE_DISTANCE_TIC_2019; //A changer en ROUE_DISTANCE_TIC_2018 pour chassis 2018
ohlimi2 1:b1f5ab5f08a2 33 float vitesseBiniou = 0.0; //En m/s
ohlimi2 1:b1f5ab5f08a2 34 float compteurDistance = 0.0;
ohlimi2 1:b1f5ab5f08a2 35
ohlimi2 1:b1f5ab5f08a2 36 Timer odoTick;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
ohlimi2 1:b1f5ab5f08a2 37
ohlimi2 1:b1f5ab5f08a2 38 void odoTickHandler()
ohlimi2 1:b1f5ab5f08a2 39 {
ohlimi2 1:b1f5ab5f08a2 40 unsigned odoMicroSec = 0;
ohlimi2 1:b1f5ab5f08a2 41 //Incremente compteur de distance
ohlimi2 1:b1f5ab5f08a2 42 compteurDistance++;
ohlimi2 1:b1f5ab5f08a2 43 //Calcule de la vitesse Biniou avec Odometre en fonction de horodatage du TIC odo precedent
ohlimi2 1:b1f5ab5f08a2 44 vitesseBiniou = roueDistanceTic / (odoTick.read_ms()*1000);
ohlimi2 1:b1f5ab5f08a2 45
ohlimi2 1:b1f5ab5f08a2 46 odoTick.reset();
ohlimi2 1:b1f5ab5f08a2 47 odoTick.start();
ohlimi2 1:b1f5ab5f08a2 48
ohlimi2 1:b1f5ab5f08a2 49 printf("ODO TICK!\n");
ohlimi2 1:b1f5ab5f08a2 50 printf("vitesseBiniou: %f\n", vitesseBiniou);
ohlimi2 1:b1f5ab5f08a2 51 printf("compteurDistance: %u\n", compteurDistance);
ohlimi2 1:b1f5ab5f08a2 52 }
ohlimi2 0:0232ed5c62b6 53
ohlimi2 0:0232ed5c62b6 54 // declaration d'une interruption sur le front de la pin d'odometre
ohlimi2 0:0232ed5c62b6 55 //InterruptRise my_interruptPin(PF_8);
ohlimi2 0:0232ed5c62b6 56
ohlimi2 0:0232ed5c62b6 57 // interruption
ohlimi2 0:0232ed5c62b6 58 //void pressed()
ohlimi2 0:0232ed5c62b6 59 //{
ohlimi2 0:0232ed5c62b6 60 // cntr_odometre++ ;
ohlimi2 0:0232ed5c62b6 61 //}
ohlimi2 0:0232ed5c62b6 62
ohlimi2 0:0232ed5c62b6 63 // initialisation du MPU6050
ohlimi2 1:b1f5ab5f08a2 64
ohlimi2 0:0232ed5c62b6 65 Serial pc(USBTX, USBRX,115200);
ohlimi2 0:0232ed5c62b6 66
ohlimi2 0:0232ed5c62b6 67 MPU6050 mpu;
ohlimi2 0:0232ed5c62b6 68
ohlimi2 0:0232ed5c62b6 69
ohlimi2 0:0232ed5c62b6 70 int16_t ax, ay, az;
ohlimi2 0:0232ed5c62b6 71 float ax_g,ay_g,az_g;
ohlimi2 0:0232ed5c62b6 72
ohlimi2 0:0232ed5c62b6 73 int16_t gx, gy, gz;
ohlimi2 0:0232ed5c62b6 74
ohlimi2 0:0232ed5c62b6 75 int main()
ohlimi2 0:0232ed5c62b6 76 {
ohlimi2 0:0232ed5c62b6 77 float meas_adc_PA4;
ohlimi2 0:0232ed5c62b6 78 float meas_adc_PA5;
ohlimi2 0:0232ed5c62b6 79 float meas_adc_PA6;
ohlimi2 0:0232ed5c62b6 80 float meas_adc_PC0;
ohlimi2 0:0232ed5c62b6 81
ohlimi2 0:0232ed5c62b6 82 float meas_adc_PF3;
ohlimi2 0:0232ed5c62b6 83 float meas_adc_PF4;
ohlimi2 0:0232ed5c62b6 84 float meas_adc_PF9;
ohlimi2 0:0232ed5c62b6 85 float meas_adc_PF10;
ohlimi2 0:0232ed5c62b6 86
ohlimi2 0:0232ed5c62b6 87 float meas_adc_PA4_V;
ohlimi2 0:0232ed5c62b6 88 float meas_adc_PA5_V;
ohlimi2 0:0232ed5c62b6 89 float meas_adc_PA6_V;
ohlimi2 0:0232ed5c62b6 90 float meas_adc_PC0_V;
ohlimi2 0:0232ed5c62b6 91
ohlimi2 0:0232ed5c62b6 92 float meas_adc_PF3_V;
ohlimi2 0:0232ed5c62b6 93 float meas_adc_PF4_V;
ohlimi2 0:0232ed5c62b6 94 float meas_adc_PF9_V;
ohlimi2 0:0232ed5c62b6 95 float meas_adc_PF10_V;
ohlimi2 0:0232ed5c62b6 96
ohlimi2 0:0232ed5c62b6 97 // configuration des PWMs
ohlimi2 0:0232ed5c62b6 98
ohlimi2 0:0232ed5c62b6 99 pwm_PE9.period_ms(10);
ohlimi2 0:0232ed5c62b6 100 pwm_PF7.period_ms(10);
ohlimi2 0:0232ed5c62b6 101 pwm_PF6.period_ms(10);
ohlimi2 0:0232ed5c62b6 102 pwm_PC8.period_ms(10);
ohlimi2 0:0232ed5c62b6 103 pwm_PD12.period_ms(10);
ohlimi2 0:0232ed5c62b6 104
ohlimi2 0:0232ed5c62b6 105 pwm_PE9.pulsewidth_us(1000);
ohlimi2 0:0232ed5c62b6 106 pwm_PF7.pulsewidth_us(1250);
ohlimi2 0:0232ed5c62b6 107 pwm_PF6.pulsewidth_us(1500);
ohlimi2 0:0232ed5c62b6 108 pwm_PC8.pulsewidth_us(1750);
ohlimi2 0:0232ed5c62b6 109 pwm_PD12.pulsewidth_us(2000);
ohlimi2 0:0232ed5c62b6 110
ohlimi2 0:0232ed5c62b6 111 //MPU6050
ohlimi2 0:0232ed5c62b6 112 pc.printf("MPU6050 test\n\n");
ohlimi2 0:0232ed5c62b6 113 pc.printf("MPU6050 initialize \n");
ohlimi2 0:0232ed5c62b6 114
ohlimi2 0:0232ed5c62b6 115 mpu.initialize();
ohlimi2 0:0232ed5c62b6 116 pc.printf("MPU6050 testConnection \n");
ohlimi2 0:0232ed5c62b6 117
ohlimi2 0:0232ed5c62b6 118 bool mpu6050TestResult = mpu.testConnection();
ohlimi2 0:0232ed5c62b6 119 if(mpu6050TestResult) {
ohlimi2 0:0232ed5c62b6 120 pc.printf("MPU6050 test passed \n");
ohlimi2 0:0232ed5c62b6 121 } else {
ohlimi2 0:0232ed5c62b6 122 pc.printf("MPU6050 test failed \n");
ohlimi2 0:0232ed5c62b6 123 }
ohlimi2 0:0232ed5c62b6 124
ohlimi2 1:b1f5ab5f08a2 125 // Assign functions to Odometre input
ohlimi2 1:b1f5ab5f08a2 126 odoInput.fall(&odoTickHandler);
ohlimi2 0:0232ed5c62b6 127
ohlimi2 0:0232ed5c62b6 128 while(1) {
ohlimi2 0:0232ed5c62b6 129 // recuperer la valeur en float entre 0.0 et 1.0
ohlimi2 0:0232ed5c62b6 130 meas_adc_PA4 = analog_value_PA4.read();
ohlimi2 0:0232ed5c62b6 131 meas_adc_PA5 = analog_value_PA5.read();
ohlimi2 0:0232ed5c62b6 132 meas_adc_PA6 = analog_value_PA6.read();
ohlimi2 0:0232ed5c62b6 133 meas_adc_PC0 = analog_value_PC0.read();
ohlimi2 0:0232ed5c62b6 134
ohlimi2 0:0232ed5c62b6 135 meas_adc_PF3 = analog_value_PF3.read();
ohlimi2 0:0232ed5c62b6 136 meas_adc_PF4 = analog_value_PF4.read();
ohlimi2 0:0232ed5c62b6 137 meas_adc_PF9 = analog_value_PF9.read();
ohlimi2 0:0232ed5c62b6 138 meas_adc_PF10 = analog_value_PF10.read();
ohlimi2 0:0232ed5c62b6 139
ohlimi2 0:0232ed5c62b6 140 // Multiplier par la tension de reference pour avoir la tension en volt
ohlimi2 0:0232ed5c62b6 141 meas_adc_PA4_V = meas_adc_PA4* 3300 ;
ohlimi2 0:0232ed5c62b6 142 meas_adc_PA5_V = meas_adc_PA5* 3300 ;
ohlimi2 0:0232ed5c62b6 143 meas_adc_PA6_V = meas_adc_PA6* 3300 ;
ohlimi2 0:0232ed5c62b6 144 meas_adc_PC0_V = meas_adc_PC0* 3300 ;
ohlimi2 0:0232ed5c62b6 145
ohlimi2 0:0232ed5c62b6 146 meas_adc_PF3_V = meas_adc_PF3* 3300 ;
ohlimi2 0:0232ed5c62b6 147 meas_adc_PF4_V = meas_adc_PF4* 3300 ;
ohlimi2 0:0232ed5c62b6 148 meas_adc_PF9_V = meas_adc_PF9* 3300 ;
ohlimi2 0:0232ed5c62b6 149 meas_adc_PF10_V = meas_adc_PF10* 3300 ;
ohlimi2 0:0232ed5c62b6 150
ohlimi2 0:0232ed5c62b6 151 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ohlimi2 0:0232ed5c62b6 152 //correction pour avoir une acceleration coef empirique
ohlimi2 0:0232ed5c62b6 153 ax_g = (float)ax /1658.0 ;
ohlimi2 0:0232ed5c62b6 154 ay_g = (float)ay /1658.0 ;
ohlimi2 0:0232ed5c62b6 155 az_g = (float)az /1658.0 ;
ohlimi2 0:0232ed5c62b6 156
ohlimi2 0:0232ed5c62b6 157 // affiche les valeurs sur la com usb
ohlimi2 0:0232ed5c62b6 158 printf("\f");
ohlimi2 1:b1f5ab5f08a2 159 printf("meas_adc_PA4_V = %.0f mV\n\r", meas_adc_PA4_V);
ohlimi2 1:b1f5ab5f08a2 160 printf("meas_adc_PA5_V = %.0f mV\n\r", meas_adc_PA5_V);
ohlimi2 1:b1f5ab5f08a2 161 printf("meas_adc_PA6_V = %.0f mV\n\r", meas_adc_PA6_V);
ohlimi2 1:b1f5ab5f08a2 162 printf("meas_adc_PC0_V = %.0f mV\n\r", meas_adc_PC0_V);
ohlimi2 0:0232ed5c62b6 163
ohlimi2 1:b1f5ab5f08a2 164 printf("meas_adc_PF3_V = %.0f mV\n\r", meas_adc_PF3_V);
ohlimi2 1:b1f5ab5f08a2 165 printf("meas_adc_PF4_V = %.0f mV\n\r", meas_adc_PF4_V);
ohlimi2 1:b1f5ab5f08a2 166 printf("meas_adc_PF9_V = %.0f mV\n\r", meas_adc_PF9_V);
ohlimi2 1:b1f5ab5f08a2 167 printf("meas_adc_PF10_V = %.0f mV\n\r", meas_adc_PF10_V);
ohlimi2 0:0232ed5c62b6 168
ohlimi2 0:0232ed5c62b6 169
ohlimi2 0:0232ed5c62b6 170 //writing current accelerometer and gyro position
ohlimi2 0:0232ed5c62b6 171
ohlimi2 0:0232ed5c62b6 172 printf("acc X =%.2f; acc Y =%.2f; acc Z=%.2f;\n\r",ax_g,ay_g,az_g);
ohlimi2 1:b1f5ab5f08a2 173
ohlimi2 0:0232ed5c62b6 174 //pc.printf("acc X =%d; acc Y =%d; acc Z=%d; Gyr X =%d; Gyr Y =%d; Gyr Z =%d\n",ax,ay,az,gx,gy,gz);
ohlimi2 0:0232ed5c62b6 175
ohlimi2 0:0232ed5c62b6 176 led = !led;
ohlimi2 1:b1f5ab5f08a2 177 enable_esc = !enable_esc ;
ohlimi2 1:b1f5ab5f08a2 178 wait(0.2); // delais
ohlimi2 0:0232ed5c62b6 179 }
ohlimi2 0:0232ed5c62b6 180 }