Rafael Caro / Mbed 2 deprecated RobotCompletoPublico

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of RobotCompleto by Rafael Caro

main.cpp@0:bd4170087cf1, 2015-05-04 (annotated)

Committer:
Racafe
Date:
Mon May 04 08:39:07 2015 +0000
Revision:
0:bd4170087cf1
Todo a?adido;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Racafe 0:bd4170087cf1 1 #include "main.h"
Racafe 0:bd4170087cf1 2 #include "mbed.h"
Racafe 0:bd4170087cf1 3 #include "calculaVelocidad.h"
Racafe 0:bd4170087cf1 4 #include "mantieneVelocidad.h"
Racafe 0:bd4170087cf1 5 #include "frenar.h"
Racafe 0:bd4170087cf1 6 Serial device(PA_11, PA_12);
Racafe 0:bd4170087cf1 7 //Para enviar datos al pc como debug
Racafe 0:bd4170087cf1 8 Serial pc(USBTX, USBRX);
Racafe 0:bd4170087cf1 9
Racafe 0:bd4170087cf1 10 //PwmOut pinRuedaDerechaAdelante(PB_13);
Racafe 0:bd4170087cf1 11 PwmOut pinRuedaDerechaAdelante(PC_8);
Racafe 0:bd4170087cf1 12 PwmOut pinRuedaDerechaAtras(PA_7);
Racafe 0:bd4170087cf1 13 //PwmOut pinRuedaIzquierdaAdelante(PC_8);
Racafe 0:bd4170087cf1 14 PwmOut pinRuedaIzquierdaAdelante(PB_15);
Racafe 0:bd4170087cf1 15 PwmOut pinRuedaIzquierdaAtras(PB_13);
Racafe 0:bd4170087cf1 16 int pinControl = 3;
Racafe 0:bd4170087cf1 17 double potenciaMotoresFrameAnterior = 0;
Racafe 0:bd4170087cf1 18 double potenciaMotores = 0;
Racafe 0:bd4170087cf1 19 int acumulador = 0;
Racafe 0:bd4170087cf1 20
Racafe 0:bd4170087cf1 21 double velocidadDeseada = 1.0;
Racafe 0:bd4170087cf1 22 double velocidadCalculada=0.0;
Racafe 0:bd4170087cf1 23
Racafe 0:bd4170087cf1 24 int tiempoUltimoControlFrenado;
Racafe 0:bd4170087cf1 25 int ping = 50;
Racafe 0:bd4170087cf1 26 int periodo = 0;
Racafe 0:bd4170087cf1 27
Racafe 0:bd4170087cf1 28 int modo = 0; //0 Frenar
Racafe 0:bd4170087cf1 29 //1 VelocidadConstante
Racafe 0:bd4170087cf1 30 //2 SigueLinea
Racafe 0:bd4170087cf1 31
Racafe 0:bd4170087cf1 32 //Sensores de color
Racafe 0:bd4170087cf1 33 AnalogIn derecha(PC_3);
Racafe 0:bd4170087cf1 34 AnalogIn izquierda(PC_2);
Racafe 0:bd4170087cf1 35
Racafe 0:bd4170087cf1 36 float velocidad = 0.25f;
Racafe 0:bd4170087cf1 37 float velocidadBaja = 0.0f;
Racafe 0:bd4170087cf1 38 float velocidadSube = 0.25f;
Racafe 0:bd4170087cf1 39 float medidaDerecha;
Racafe 0:bd4170087cf1 40 float medidaIzquierda;
Racafe 0:bd4170087cf1 41 float velocidadIzquierda;
Racafe 0:bd4170087cf1 42 float velocidadDerecha;
Racafe 0:bd4170087cf1 43 bool izquierdaNegro = false;
Racafe 0:bd4170087cf1 44 bool derechaNegro = false;
Racafe 0:bd4170087cf1 45 char estado = 'p';
Racafe 0:bd4170087cf1 46 bool estadoCambiado = false;
Racafe 0:bd4170087cf1 47
Racafe 0:bd4170087cf1 48
Racafe 0:bd4170087cf1 49
Racafe 0:bd4170087cf1 50 //Esto es exclusivo de la implementación en mbed
Racafe 0:bd4170087cf1 51 //Interrupcion para el calculo de velocidad
Racafe 0:bd4170087cf1 52 InterruptIn sensorRuedaDerecha(A5);
Racafe 0:bd4170087cf1 53 //Timer para el ping
Racafe 0:bd4170087cf1 54 Timer timerPing;
Racafe 0:bd4170087cf1 55 Timer timerFreno;
Racafe 0:bd4170087cf1 56
Racafe 0:bd4170087cf1 57 void interrupcion()
Racafe 0:bd4170087cf1 58 {
Racafe 0:bd4170087cf1 59 //El acumulador muestra el número de muescas que avanza la rueda.
Racafe 0:bd4170087cf1 60 //No se pueden poner prints en las interrupciones
Racafe 0:bd4170087cf1 61 acumulador++;
Racafe 0:bd4170087cf1 62 }
Racafe 0:bd4170087cf1 63
Racafe 0:bd4170087cf1 64
Racafe 0:bd4170087cf1 65
Racafe 0:bd4170087cf1 66 int main()
Racafe 0:bd4170087cf1 67 {
Racafe 0:bd4170087cf1 68
Racafe 0:bd4170087cf1 69 device.baud(9600);
Racafe 0:bd4170087cf1 70 pc.printf("Conexion establecida\n");
Racafe 0:bd4170087cf1 71 char rx;
Racafe 0:bd4170087cf1 72
Racafe 0:bd4170087cf1 73 //Inicializamos cosas (como el setup de arduino)
Racafe 0:bd4170087cf1 74 sensorRuedaDerecha.rise(&interrupcion);
Racafe 0:bd4170087cf1 75 timerPing.start();
Racafe 0:bd4170087cf1 76 timerFreno.start();
Racafe 0:bd4170087cf1 77 while(1) {
Racafe 0:bd4170087cf1 78
Racafe 0:bd4170087cf1 79 //Comprobar por bluetooth
Racafe 0:bd4170087cf1 80 if(device.readable()) {
Racafe 0:bd4170087cf1 81 pc.printf("\nRecibido dato\n");
Racafe 0:bd4170087cf1 82 rx =device.getc();
Racafe 0:bd4170087cf1 83 pc.printf("\n Recibido %c \n",rx);
Racafe 0:bd4170087cf1 84 if(rx == 'a') {
Racafe 0:bd4170087cf1 85 modo = 1;
Racafe 0:bd4170087cf1 86 pc.printf("acelera");
Racafe 0:bd4170087cf1 87 }
Racafe 0:bd4170087cf1 88 if(rx == 's') {
Racafe 0:bd4170087cf1 89 modo = 0;
Racafe 0:bd4170087cf1 90 pc.printf("frena");
Racafe 0:bd4170087cf1 91 }
Racafe 0:bd4170087cf1 92 if(rx == 'l') {
Racafe 0:bd4170087cf1 93 modo = 2;
Racafe 0:bd4170087cf1 94 pc.printf("linea");
Racafe 0:bd4170087cf1 95 }
Racafe 0:bd4170087cf1 96 }
Racafe 0:bd4170087cf1 97
Racafe 0:bd4170087cf1 98
Racafe 0:bd4170087cf1 99
Racafe 0:bd4170087cf1 100
Racafe 0:bd4170087cf1 101
Racafe 0:bd4170087cf1 102 if(modo == 2) {
Racafe 0:bd4170087cf1 103 //SEGUIR LINEA
Racafe 0:bd4170087cf1 104
Racafe 0:bd4170087cf1 105
Racafe 0:bd4170087cf1 106 medidaDerecha = derecha.read();
Racafe 0:bd4170087cf1 107 medidaIzquierda = izquierda.read();
Racafe 0:bd4170087cf1 108 medidaDerecha = medidaDerecha * 3300;
Racafe 0:bd4170087cf1 109 medidaIzquierda = medidaIzquierda * 3300;
Racafe 0:bd4170087cf1 110 // printf("measure = %.0f mV\n", meas);
Racafe 0:bd4170087cf1 111 if (medidaDerecha > 2000) {
Racafe 0:bd4170087cf1 112 derechaNegro = true;
Racafe 0:bd4170087cf1 113 } else {
Racafe 0:bd4170087cf1 114 derechaNegro = false;
Racafe 0:bd4170087cf1 115 }
Racafe 0:bd4170087cf1 116 if (medidaIzquierda > 2000) {
Racafe 0:bd4170087cf1 117 izquierdaNegro = false;
Racafe 0:bd4170087cf1 118 } else {
Racafe 0:bd4170087cf1 119 izquierdaNegro = true;
Racafe 0:bd4170087cf1 120 }
Racafe 0:bd4170087cf1 121 //pc.printf("Izquierda %d y derecha %d", izquierdaNegro, derechaNegro);
Racafe 0:bd4170087cf1 122
Racafe 0:bd4170087cf1 123 if(izquierdaNegro == 1 and derechaNegro == 1 and (estado != 'r')) {
Racafe 0:bd4170087cf1 124 pc.printf("RECTO ");
Racafe 0:bd4170087cf1 125 velocidadIzquierda = velocidad;
Racafe 0:bd4170087cf1 126 velocidadDerecha = velocidad;
Racafe 0:bd4170087cf1 127 estado = 'r';
Racafe 0:bd4170087cf1 128 estadoCambiado = true;
Racafe 0:bd4170087cf1 129
Racafe 0:bd4170087cf1 130 } else if (izquierdaNegro == 1 and derechaNegro == 0 and (estado != 'i')) {
Racafe 0:bd4170087cf1 131 velocidadIzquierda = velocidadBaja;
Racafe 0:bd4170087cf1 132 velocidadDerecha = velocidadSube;
Racafe 0:bd4170087cf1 133 pc.printf("giraIzquierda ");
Racafe 0:bd4170087cf1 134 estado = 'i';
Racafe 0:bd4170087cf1 135 estadoCambiado = true;
Racafe 0:bd4170087cf1 136
Racafe 0:bd4170087cf1 137 } else if (izquierdaNegro == 0 and derechaNegro == 1 and (estado != 'd')) {
Racafe 0:bd4170087cf1 138 velocidadIzquierda = velocidadSube;
Racafe 0:bd4170087cf1 139 velocidadDerecha = velocidadBaja;
Racafe 0:bd4170087cf1 140 pc.printf("giraDerecha ");
Racafe 0:bd4170087cf1 141 estado = 'd';
Racafe 0:bd4170087cf1 142 estadoCambiado = true;
Racafe 0:bd4170087cf1 143 } else if (izquierdaNegro == 0 and derechaNegro == 0 and (estado != 'p')) {
Racafe 0:bd4170087cf1 144 velocidadIzquierda = 0.0f;
Racafe 0:bd4170087cf1 145 velocidadDerecha = 0.0f;
Racafe 0:bd4170087cf1 146 pc.printf("parado ");
Racafe 0:bd4170087cf1 147 estado = 'p';
Racafe 0:bd4170087cf1 148 estadoCambiado = true;
Racafe 0:bd4170087cf1 149 } else {
Racafe 0:bd4170087cf1 150 estadoCambiado = false;
Racafe 0:bd4170087cf1 151 }
Racafe 0:bd4170087cf1 152
Racafe 0:bd4170087cf1 153 pc.printf(" VelocidadIzquierda %g VelocidadDerecha %g ", velocidadIzquierda, velocidadDerecha);
Racafe 0:bd4170087cf1 154 // if(estadoCambiado) {
Racafe 0:bd4170087cf1 155
Racafe 0:bd4170087cf1 156 pinRuedaIzquierdaAdelante.write(velocidadIzquierda);
Racafe 0:bd4170087cf1 157 pinRuedaDerechaAdelante.write(velocidadDerecha);
Racafe 0:bd4170087cf1 158 //}
Racafe 0:bd4170087cf1 159
Racafe 0:bd4170087cf1 160
Racafe 0:bd4170087cf1 161 } else {
Racafe 0:bd4170087cf1 162 periodo = timerPing.read_ms();
Racafe 0:bd4170087cf1 163 if(periodo > ping) {
Racafe 0:bd4170087cf1 164 //pc.printf("\n%d", periodo);
Racafe 0:bd4170087cf1 165 velocidadCalculada = calculaVelocidad(periodo, acumulador);
Racafe 0:bd4170087cf1 166
Racafe 0:bd4170087cf1 167 //Por si queremos mostrar la velocidad por pantalla
Racafe 0:bd4170087cf1 168 pc.printf("\n%f", velocidadCalculada);
Racafe 0:bd4170087cf1 169 acumulador = 0;
Racafe 0:bd4170087cf1 170 timerPing.reset();
Racafe 0:bd4170087cf1 171 }
Racafe 0:bd4170087cf1 172 //pc.printf("\n%d", acumulador);
Racafe 0:bd4170087cf1 173
Racafe 0:bd4170087cf1 174 //Mantener velocidad
Racafe 0:bd4170087cf1 175
Racafe 0:bd4170087cf1 176 if(modo == 1) {
Racafe 0:bd4170087cf1 177 mantenerVelocidad(velocidadDeseada, velocidadCalculada, potenciaMotoresFrameAnterior, potenciaMotores,pinRuedaDerechaAdelante, pinRuedaIzquierdaAdelante);
Racafe 0:bd4170087cf1 178
Racafe 0:bd4170087cf1 179
Racafe 0:bd4170087cf1 180 } else {
Racafe 0:bd4170087cf1 181 //Aquí va el código para frenado
Racafe 0:bd4170087cf1 182 periodo = timerFreno.read_ms();
Racafe 0:bd4170087cf1 183 if(modo == 0) {
Racafe 0:bd4170087cf1 184
Racafe 0:bd4170087cf1 185 if(periodo>ping) {
Racafe 0:bd4170087cf1 186 pc.printf("FRENA %f", velocidadCalculada);
Racafe 0:bd4170087cf1 187 frenar(velocidadCalculada, potenciaMotores, potenciaMotoresFrameAnterior, pinRuedaDerechaAdelante, pinRuedaDerechaAtras, pinRuedaIzquierdaAdelante, pinRuedaIzquierdaAtras);
Racafe 0:bd4170087cf1 188 timerFreno.reset();
Racafe 0:bd4170087cf1 189 }
Racafe 0:bd4170087cf1 190 }
Racafe 0:bd4170087cf1 191 }
Racafe 0:bd4170087cf1 192 }
Racafe 0:bd4170087cf1 193
Racafe 0:bd4170087cf1 194
Racafe 0:bd4170087cf1 195 }
Racafe 0:bd4170087cf1 196
Racafe 0:bd4170087cf1 197 }
Racafe 0:bd4170087cf1 198
Racafe 0:bd4170087cf1 199