Ryan Mauricio / Mbed 2 deprecated SAMUELFINAL_BACKUP

Dependencies:   mbed Servo DebounceIn

main.cpp@3:6e4ca952e920, 2019-08-27 (annotated)

Committer:
ryanzero
Date:
Tue Aug 27 22:51:39 2019 +0000
Revision:
3:6e4ca952e920
Parent:
2:7b7060835269 
Projeto final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manitou 0:31cc139b7d1e 1 #include "mbed.h"
manitou 0:31cc139b7d1e 2 #include "MPU9250.h"
ryanzero 3:6e4ca952e920 3 #include "Servo.h"
ryanzero 3:6e4ca952e920 4 #include "DebounceIn.h"
manitou 0:31cc139b7d1e 5
ryanzero 2:7b7060835269 6 AnalogIn pot(A0);
ryanzero 3:6e4ca952e920 7 PwmOut myservo(D6);
ryanzero 3:6e4ca952e920 8 DigitalOut LedNorma(D2), LedVolante(D3), LedAsa(D4), LedAcelera(D5), myled1(LED1);
ryanzero 3:6e4ca952e920 9 DebounceIn mybutton(USER_BUTTON);
manitou 0:31cc139b7d1e 10
manitou 0:31cc139b7d1e 11 float sum = 0;
manitou 0:31cc139b7d1e 12 uint32_t sumCount = 0;
manitou 0:31cc139b7d1e 13
ryanzero 3:6e4ca952e920 14 float axf=0, ayf=0, azf=0, soma=0, naxf=0, norma=0, norma2=0, g=0, pr[10];
ryanzero 3:6e4ca952e920 15 int Flag_botao=0, Flag_naxf=0,Flag_Norma=0, Flag_axf=0, Flag_DRS=0, Flag_vs=0, Flag_Volante=1, i;
ryanzero 2:7b7060835269 16
ryanzero 3:6e4ca952e920 17 char serial;
ryanzero 3:6e4ca952e920 18 bool inicio = true;
ryanzero 2:7b7060835269 19
manitou 1:0158e4d78423 20 Timer t;
manitou 1:0158e4d78423 21
ryanzero 3:6e4ca952e920 22 MPU9250 mpu9250;
ryanzero 3:6e4ca952e920 23
manitou 1:0158e4d78423 24 Serial pc(USBTX, USBRX); // tx, rx
manitou 0:31cc139b7d1e 25
manitou 0:31cc139b7d1e 26 volatile bool newData = false;
manitou 0:31cc139b7d1e 27
manitou 0:31cc139b7d1e 28 InterruptIn isrPin(D12); //k64 D12 dragon PD_0
manitou 0:31cc139b7d1e 29
ryanzero 3:6e4ca952e920 30 void mpuisr()
ryanzero 3:6e4ca952e920 31 {
ryanzero 3:6e4ca952e920 32 newData=true;
ryanzero 3:6e4ca952e920 33 }
ryanzero 2:7b7060835269 34
ryanzero 3:6e4ca952e920 35 int main(){
ryanzero 3:6e4ca952e920 36
manitou 1:0158e4d78423 37 pc.baud(9600);
manitou 1:0158e4d78423 38
manitou 1:0158e4d78423 39 //Set up I2C
ryanzero 3:6e4ca952e920 40 i2c.frequency(400000);
manitou 1:0158e4d78423 41 t.start();
manitou 1:0158e4d78423 42 isrPin.rise(&mpuisr);
ryanzero 3:6e4ca952e920 43
ryanzero 3:6e4ca952e920 44 myservo.period_ms(20);
ryanzero 3:6e4ca952e920 45 myservo.pulsewidth_ms(1.5);
ryanzero 3:6e4ca952e920 46
ryanzero 3:6e4ca952e920 47 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // numero registro I2C MPU9250
manitou 0:31cc139b7d1e 48
ryanzero 3:6e4ca952e920 49 if (whoami == 0x73){
ryanzero 3:6e4ca952e920 50 wait(1);
ryanzero 3:6e4ca952e920 51 mpu9250.resetMPU9250(); // Reset registradores
ryanzero 3:6e4ca952e920 52 mpu9250.calibrateMPU9250(gyroBias, accelBias); // calibração giroscopio e acelerometro
ryanzero 3:6e4ca952e920 53 wait(2);
ryanzero 3:6e4ca952e920 54 mpu9250.initMPU9250();
ryanzero 3:6e4ca952e920 55 mpu9250.initAK8963(magCalibration); // Calibração magnetometro
ryanzero 3:6e4ca952e920 56 wait(2);
ryanzero 3:6e4ca952e920 57 }
ryanzero 3:6e4ca952e920 58 else{
ryanzero 3:6e4ca952e920 59 while(1) ; // loop de não comunicação
ryanzero 3:6e4ca952e920 60 }
manitou 0:31cc139b7d1e 61
ryanzero 3:6e4ca952e920 62 mpu9250.getAres(); // valor setado de sensibilidade
ryanzero 3:6e4ca952e920 63 mpu9250.getGres();
ryanzero 3:6e4ca952e920 64 mpu9250.getMres();
ryanzero 3:6e4ca952e920 65 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
ryanzero 3:6e4ca952e920 66 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
ryanzero 3:6e4ca952e920 67 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
manitou 0:31cc139b7d1e 68
ryanzero 3:6e4ca952e920 69 while(1) {
ryanzero 2:7b7060835269 70 static int readycnt=0;
manitou 0:31cc139b7d1e 71 #if USE_ISR
ryanzero 3:6e4ca952e920 72 if(newData) {
ryanzero 2:7b7060835269 73 newData=false;
ryanzero 2:7b7060835269 74 mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS); //? need this with ISR
manitou 0:31cc139b7d1e 75 #else
ryanzero 3:6e4ca952e920 76 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
ryanzero 2:7b7060835269 77 #endif
ryanzero 2:7b7060835269 78 readycnt++;
ryanzero 3:6e4ca952e920 79 mpu9250.readAccelData(accelCount); // Read the x/y/z
ryanzero 3:6e4ca952e920 80 ax = (float)accelCount[0]*aRes - accelBias[0]; // Calculo da aceleração - mg
ryanzero 2:7b7060835269 81 ay = (float)accelCount[1]*aRes - accelBias[1];
ryanzero 2:7b7060835269 82 az = (float)accelCount[2]*aRes - accelBias[2];
ryanzero 2:7b7060835269 83
ryanzero 2:7b7060835269 84 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
ryanzero 3:6e4ca952e920 85 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // calculo do giroscopio - rad/s
ryanzero 2:7b7060835269 86 gy = (float)gyroCount[1]*gRes - gyroBias[1];
ryanzero 2:7b7060835269 87 gz = (float)gyroCount[2]*gRes - gyroBias[2];
ryanzero 2:7b7060835269 88
ryanzero 2:7b7060835269 89 mpu9250.readMagData(magCount); // Read the x/y/z adc values
ryanzero 3:6e4ca952e920 90 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; //calculo do magnetometro
ryanzero 2:7b7060835269 91 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
ryanzero 2:7b7060835269 92 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
ryanzero 3:6e4ca952e920 93 }
manitou 1:0158e4d78423 94
ryanzero 2:7b7060835269 95 Now = t.read_us();
ryanzero 2:7b7060835269 96 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
ryanzero 2:7b7060835269 97 lastUpdate = Now;
ryanzero 2:7b7060835269 98
ryanzero 2:7b7060835269 99 sum += deltat;
ryanzero 2:7b7060835269 100 sumCount++;
ryanzero 3:6e4ca952e920 101
ryanzero 2:7b7060835269 102 uint32_t us = t.read_us();
ryanzero 2:7b7060835269 103 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ryanzero 2:7b7060835269 104 us = t.read_us()-us;
ryanzero 2:7b7060835269 105
ryanzero 2:7b7060835269 106 // Serial print and/or display at 0.5 s rate independent of data rates
ryanzero 2:7b7060835269 107 delt_t = t.read_ms() - count;
ryanzero 3:6e4ca952e920 108
ryanzero 3:6e4ca952e920 109
ryanzero 3:6e4ca952e920 110 if (delt_t > 100) { //taxa de atualização calculos - ms
ryanzero 3:6e4ca952e920 111
manitou 1:0158e4d78423 112 readycnt=0;
manitou 1:0158e4d78423 113
manitou 1:0158e4d78423 114 tempCount = mpu9250.readTempData(); // Read the adc values
manitou 1:0158e4d78423 115 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
ryanzero 2:7b7060835269 116
manitou 1:0158e4d78423 117 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 118 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
manitou 1:0158e4d78423 119 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 120 pitch *= 180.0f / PI;
manitou 1:0158e4d78423 121 yaw *= 180.0f / PI;
ryanzero 3:6e4ca952e920 122 yaw -= 21.1f; // Declination em São carlos (27/06/19)
manitou 1:0158e4d78423 123 roll *= 180.0f / PI;
manitou 0:31cc139b7d1e 124
manitou 1:0158e4d78423 125 count = t.read_ms();
ryanzero 3:6e4ca952e920 126
manitou 1:0158e4d78423 127 sum = 0;
manitou 1:0158e4d78423 128 sumCount = 0;
ryanzero 3:6e4ca952e920 129 if(i==0){ //Cálculo da norma inicial para ajuste
ryanzero 3:6e4ca952e920 130 for(i=0;i<10;i++)
ryanzero 3:6e4ca952e920 131 {
ryanzero 3:6e4ca952e920 132 pr[i] = sqrt((1000000*ax*ax)+(1000000*ay*ay)+(1000000*az*az));
ryanzero 3:6e4ca952e920 133 soma = soma + pr[i];
ryanzero 3:6e4ca952e920 134 }
ryanzero 2:7b7060835269 135
ryanzero 3:6e4ca952e920 136 norma = soma/10000;
ryanzero 3:6e4ca952e920 137 }
ryanzero 3:6e4ca952e920 138 //Cálculo das acelerações normalizadas
ryanzero 3:6e4ca952e920 139 axf = (ax*1000)/norma;
ryanzero 3:6e4ca952e920 140 ayf = (ay*1000)/norma;
ryanzero 3:6e4ca952e920 141 azf = (az*1000)/norma;
ryanzero 2:7b7060835269 142
ryanzero 3:6e4ca952e920 143 //Print das acelerações
ryanzero 3:6e4ca952e920 144 // pc.printf("axf = %f\n", axf);
ryanzero 3:6e4ca952e920 145 // pc.printf("ayf = %f\n", ayf);
ryanzero 3:6e4ca952e920 146 // pc.printf("azf = %f\n", azf);
ryanzero 3:6e4ca952e920 147 //Cálculo da norma para saber se existe aceleração
ryanzero 3:6e4ca952e920 148 norma2=sqrt((axf*axf)+(ayf*ayf)+(azf*azf));
ryanzero 3:6e4ca952e920 149 g=1000;
ryanzero 3:6e4ca952e920 150
ryanzero 3:6e4ca952e920 151 //Verifica se o botão está pressionado
ryanzero 3:6e4ca952e920 152 if (mybutton==0){ // Button is pressed
ryanzero 3:6e4ca952e920 153 Flag_botao=1;
ryanzero 3:6e4ca952e920 154 }
ryanzero 3:6e4ca952e920 155 else{
ryanzero 3:6e4ca952e920 156 Flag_botao=0;
ryanzero 3:6e4ca952e920 157 }
ryanzero 3:6e4ca952e920 158 //Botão ativar DRS do visual studio
ryanzero 3:6e4ca952e920 159 if (0){ // comando abrir visual studio
ryanzero 3:6e4ca952e920 160 Flag_vs=1;
ryanzero 3:6e4ca952e920 161 }
ryanzero 3:6e4ca952e920 162 else{
ryanzero 3:6e4ca952e920 163 Flag_vs=0;
ryanzero 3:6e4ca952e920 164 }
ryanzero 3:6e4ca952e920 165
ryanzero 3:6e4ca952e920 166 //Verifica a norma2 para saber se existe aceleração
ryanzero 3:6e4ca952e920 167 if(norma2>950){
ryanzero 3:6e4ca952e920 168 Flag_Norma=1;
ryanzero 3:6e4ca952e920 169 LedNorma=1;
ryanzero 2:7b7060835269 170 }
ryanzero 3:6e4ca952e920 171 else{
ryanzero 3:6e4ca952e920 172 Flag_Norma=0;
ryanzero 3:6e4ca952e920 173 LedNorma=0;
ryanzero 3:6e4ca952e920 174 }
ryanzero 3:6e4ca952e920 175
ryanzero 3:6e4ca952e920 176 //Verifica a aceleração em x para saber se é constante ou positiva
ryanzero 3:6e4ca952e920 177 if(axf>-20){
ryanzero 3:6e4ca952e920 178 Flag_axf=1;
ryanzero 3:6e4ca952e920 179 LedAcelera=1;
ryanzero 2:7b7060835269 180 }
ryanzero 3:6e4ca952e920 181 else{
ryanzero 3:6e4ca952e920 182 LedAcelera=0;
ryanzero 3:6e4ca952e920 183 Flag_axf=0;
ryanzero 3:6e4ca952e920 184 }
ryanzero 3:6e4ca952e920 185 //Verifica se está desacelerando
ryanzero 3:6e4ca952e920 186 if(axf<-100){
ryanzero 3:6e4ca952e920 187 Flag_naxf=1;
ryanzero 3:6e4ca952e920 188 }
ryanzero 3:6e4ca952e920 189 else{
ryanzero 3:6e4ca952e920 190 Flag_naxf=0;
ryanzero 2:7b7060835269 191 }
ryanzero 2:7b7060835269 192
ryanzero 3:6e4ca952e920 193 //Cálculo do angulo do potenciometro\volante
ryanzero 3:6e4ca952e920 194 float angle = pot.read()*3600;
ryanzero 2:7b7060835269 195
ryanzero 3:6e4ca952e920 196 if ((angle < 1050) && (angle > 950)){
ryanzero 3:6e4ca952e920 197 LedVolante=1;
ryanzero 3:6e4ca952e920 198 Flag_Volante= 1;
ryanzero 3:6e4ca952e920 199 }
ryanzero 3:6e4ca952e920 200 else{
ryanzero 3:6e4ca952e920 201 LedVolante=0;
ryanzero 3:6e4ca952e920 202 Flag_Volante=0;
ryanzero 3:6e4ca952e920 203 }
ryanzero 3:6e4ca952e920 204 //Condições para acionar DRS
ryanzero 3:6e4ca952e920 205 if((Flag_Volante==1&&Flag_axf==1&&Flag_Norma==1&&Flag_DRS==0)&&(Flag_botao==1||Flag_vs==1)) {
ryanzero 3:6e4ca952e920 206 myled1 = 1;
ryanzero 3:6e4ca952e920 207 myservo.period_ms(20);
ryanzero 3:6e4ca952e920 208 myservo.pulsewidth_ms(2);
ryanzero 3:6e4ca952e920 209 // pc.printf("drs ativadando\n");
ryanzero 3:6e4ca952e920 210 Flag_DRS=1;
ryanzero 3:6e4ca952e920 211 LedAsa=1;
ryanzero 3:6e4ca952e920 212 }
ryanzero 3:6e4ca952e920 213
ryanzero 3:6e4ca952e920 214 //Mostra se depois de acionado o DRS continua ativado
ryanzero 3:6e4ca952e920 215 if(Flag_DRS==1){
ryanzero 3:6e4ca952e920 216 // pc.printf("drs ativo\n");
ryanzero 2:7b7060835269 217 }
ryanzero 3:6e4ca952e920 218 //Condições para desativar DRS
ryanzero 3:6e4ca952e920 219 if ((Flag_Volante==0 || Flag_axf==0)&&Flag_DRS==1) {
ryanzero 3:6e4ca952e920 220 myled1 = 0;
ryanzero 3:6e4ca952e920 221 myservo.period_ms(20);
ryanzero 3:6e4ca952e920 222 myservo.pulsewidth_ms(1.5);
ryanzero 3:6e4ca952e920 223 Flag_DRS=0;
ryanzero 3:6e4ca952e920 224 LedAsa=0;
ryanzero 3:6e4ca952e920 225 }
ryanzero 3:6e4ca952e920 226 //Mostra se o DRS esta desativado
ryanzero 3:6e4ca952e920 227 if(Flag_DRS==0){
ryanzero 3:6e4ca952e920 228
ryanzero 3:6e4ca952e920 229 }
ryanzero 2:7b7060835269 230
ryanzero 3:6e4ca952e920 231 //comunicação visual studio
ryanzero 3:6e4ca952e920 232
ryanzero 3:6e4ca952e920 233 if (inicio){
ryanzero 3:6e4ca952e920 234 pc.puts("ready");
ryanzero 3:6e4ca952e920 235 inicio = false;
ryanzero 3:6e4ca952e920 236 }
ryanzero 3:6e4ca952e920 237 serial = pc.getc();
ryanzero 3:6e4ca952e920 238
ryanzero 3:6e4ca952e920 239 if(serial == 'x'){
ryanzero 3:6e4ca952e920 240 pc.printf("%f", (axf/norma2));
ryanzero 3:6e4ca952e920 241
ryanzero 3:6e4ca952e920 242
ryanzero 3:6e4ca952e920 243 }
ryanzero 3:6e4ca952e920 244 if(serial == 'y'){
ryanzero 3:6e4ca952e920 245 pc.printf("%f", (ayf/norma2));
ryanzero 3:6e4ca952e920 246
ryanzero 3:6e4ca952e920 247
ryanzero 3:6e4ca952e920 248 }
ryanzero 3:6e4ca952e920 249 if(serial == 'd'){
ryanzero 3:6e4ca952e920 250 if (Flag_DRS == 1) pc.printf("drson");
ryanzero 3:6e4ca952e920 251 else pc.printf("drsoff");
ryanzero 3:6e4ca952e920 252
ryanzero 3:6e4ca952e920 253 }
ryanzero 3:6e4ca952e920 254
ryanzero 3:6e4ca952e920 255 if(serial == 'f'){
ryanzero 3:6e4ca952e920 256 inicio = true;
ryanzero 3:6e4ca952e920 257 }
ryanzero 3:6e4ca952e920 258
ryanzero 3:6e4ca952e920 259
ryanzero 2:7b7060835269 260 }
ryanzero 2:7b7060835269 261
manitou 1:0158e4d78423 262 }
manitou 1:0158e4d78423 263
ryanzero 2:7b7060835269 264 }
ryanzero 2:7b7060835269 265
ryanzero 2:7b7060835269 266
ryanzero 2:7b7060835269 267