Cristian Castro
Proyecto final de semestre: Se desarrollo un vehículo que recibe comandos por el puerto serial y realiza las siguientes funciones según el comando sea el comando recibido: - Genera distintos tonos por un buzzer. - Controla el movimiento de un carro (con 2 motores) con comandos - Controla el movimiento de un carro (con 2 motores) con Joystick. - Lee y envía el color leido por el puerto serial al PC - Muestra los colores leídos por una pantalla FFT instalada en el vehículo.
corregido.txt@0:3a420fc22672, 2021-09-03 (annotated)
- Committer:
- CCastrop1012
- Date:
- Fri Sep 03 05:26:27 2021 +0000
- Revision:
- 0:3a420fc22672
Proyecto de final de semestre, finalizado y funcional.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| CCastrop1012 | 0:3a420fc22672 | 1 | #include "mbed.h" |
| CCastrop1012 | 0:3a420fc22672 | 2 | #include "Motor.h" |
| CCastrop1012 | 0:3a420fc22672 | 3 | #include "scolor_TCS3200.h" |
| CCastrop1012 | 0:3a420fc22672 | 4 | |
| CCastrop1012 | 0:3a420fc22672 | 5 | // Puerto de comunicacion Serial |
| CCastrop1012 | 0:3a420fc22672 | 6 | Serial CoolTerm(USBTX, USBRX); |
| CCastrop1012 | 0:3a420fc22672 | 7 | |
| CCastrop1012 | 0:3a420fc22672 | 8 | // Motores m1step |
| CCastrop1012 | 0:3a420fc22672 | 9 | TraccionD Motores(PB_5, PB_3, PB_10, PB_4, 200, 3.75, 15.5) ; |
| CCastrop1012 | 0:3a420fc22672 | 10 | |
| CCastrop1012 | 0:3a420fc22672 | 11 | /// PWM OUTPUTS |
| CCastrop1012 | 0:3a420fc22672 | 12 | PwmOut Buzzer(D10); // LED1 |
| CCastrop1012 | 0:3a420fc22672 | 13 | |
| CCastrop1012 | 0:3a420fc22672 | 14 | // Temporizadores |
| CCastrop1012 | 0:3a420fc22672 | 15 | Ticker MuestrearCOLOR; |
| CCastrop1012 | 0:3a420fc22672 | 16 | |
| CCastrop1012 | 0:3a420fc22672 | 17 | // SENSOR DE COLOR |
| CCastrop1012 | 0:3a420fc22672 | 18 | scolor_TCS3200 SENSOR_COLOR (PA_9, PC_7, PB_6, PA_7, PA_8); |
| CCastrop1012 | 0:3a420fc22672 | 19 | |
| CCastrop1012 | 0:3a420fc22672 | 20 | |
| CCastrop1012 | 0:3a420fc22672 | 21 | // Lecturas Analogas de Joystick |
| CCastrop1012 | 0:3a420fc22672 | 22 | AnalogIn JEjeX(A0); |
| CCastrop1012 | 0:3a420fc22672 | 23 | AnalogIn JEjeY(A1); |
| CCastrop1012 | 0:3a420fc22672 | 24 | |
| CCastrop1012 | 0:3a420fc22672 | 25 | |
| CCastrop1012 | 0:3a420fc22672 | 26 | // Salidas digitales |
| CCastrop1012 | 0:3a420fc22672 | 27 | DigitalOut LED(PA_5); |
| CCastrop1012 | 0:3a420fc22672 | 28 | |
| CCastrop1012 | 0:3a420fc22672 | 29 | |
| CCastrop1012 | 0:3a420fc22672 | 30 | //****************************************************************************** |
| CCastrop1012 | 0:3a420fc22672 | 31 | /// Declarar Variables Globales |
| CCastrop1012 | 0:3a420fc22672 | 32 | |
| CCastrop1012 | 0:3a420fc22672 | 33 | |
| CCastrop1012 | 0:3a420fc22672 | 34 | /// Variables sensor de color |
| CCastrop1012 | 0:3a420fc22672 | 35 | long red; //Almacenan el Tiempo que dura el CicloUtil de la frecuencia |
| CCastrop1012 | 0:3a420fc22672 | 36 | long blue; //Generada por el Sensor TSC3200, al leer el componente |
| CCastrop1012 | 0:3a420fc22672 | 37 | long green; //R, G, B o Clear del color que tenga en Frente |
| CCastrop1012 | 0:3a420fc22672 | 38 | long clear; |
| CCastrop1012 | 0:3a420fc22672 | 39 | |
| CCastrop1012 | 0:3a420fc22672 | 40 | |
| CCastrop1012 | 0:3a420fc22672 | 41 | /// Constantes Sensor de Color |
| CCastrop1012 | 0:3a420fc22672 | 42 | #define CMD_rojo 0x01 |
| CCastrop1012 | 0:3a420fc22672 | 43 | #define CMD_azul 0x02 |
| CCastrop1012 | 0:3a420fc22672 | 44 | #define CMD_verde 0x03 |
| CCastrop1012 | 0:3a420fc22672 | 45 | #define CMD_clear 0x04 |
| CCastrop1012 | 0:3a420fc22672 | 46 | #define ColorNoIdentificado 0x00 |
| CCastrop1012 | 0:3a420fc22672 | 47 | |
| CCastrop1012 | 0:3a420fc22672 | 48 | |
| CCastrop1012 | 0:3a420fc22672 | 49 | uint8_t color_identificado = ColorNoIdentificado; |
| CCastrop1012 | 0:3a420fc22672 | 50 | |
| CCastrop1012 | 0:3a420fc22672 | 51 | |
| CCastrop1012 | 0:3a420fc22672 | 52 | |
| CCastrop1012 | 0:3a420fc22672 | 53 | /// Varialbles Buzzer |
| CCastrop1012 | 0:3a420fc22672 | 54 | uint8_t duracion_tono = 1; // Variable que almacena el Tiempo que es ESCUCHARÁ el Tono |
| CCastrop1012 | 0:3a420fc22672 | 55 | uint8_t tipo_tono = 1; // Variable que almacena el Tono que se desee escuchar |
| CCastrop1012 | 0:3a420fc22672 | 56 | |
| CCastrop1012 | 0:3a420fc22672 | 57 | /// Constantes Buzzer |
| CCastrop1012 | 0:3a420fc22672 | 58 | #define TONO_DO 0x01 /// si tipo_tono == 0x01, se escuchará un DO |
| CCastrop1012 | 0:3a420fc22672 | 59 | #define TONO_RE 0x02 /// si tipo_tono == 0x02, se escuchará un RE |
| CCastrop1012 | 0:3a420fc22672 | 60 | #define TONO_MI 0x03 /// si tipo_tono == 0x03, se escuchará un MI |
| CCastrop1012 | 0:3a420fc22672 | 61 | #define TONO_SI 0x04 /// si tipo_tono == 0x04, se escuchará un SI |
| CCastrop1012 | 0:3a420fc22672 | 62 | |
| CCastrop1012 | 0:3a420fc22672 | 63 | #define DO 3.78 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN DO |
| CCastrop1012 | 0:3a420fc22672 | 64 | #define RE 3.36 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN RE |
| CCastrop1012 | 0:3a420fc22672 | 65 | #define MI 3.03 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN MI |
| CCastrop1012 | 0:3a420fc22672 | 66 | #define SI 2.82 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN SI |
| CCastrop1012 | 0:3a420fc22672 | 67 | |
| CCastrop1012 | 0:3a420fc22672 | 68 | |
| CCastrop1012 | 0:3a420fc22672 | 69 | |
| CCastrop1012 | 0:3a420fc22672 | 70 | |
| CCastrop1012 | 0:3a420fc22672 | 71 | #define limite 0.727 |
| CCastrop1012 | 0:3a420fc22672 | 72 | |
| CCastrop1012 | 0:3a420fc22672 | 73 | // variables de control y flujo de programa |
| CCastrop1012 | 0:3a420fc22672 | 74 | |
| CCastrop1012 | 0:3a420fc22672 | 75 | uint8_t programa_ejecutar = 0; // Variable que almacena la ORDEN (Telemetria ó Telecomando) |
| CCastrop1012 | 0:3a420fc22672 | 76 | // enviada desde el CoolTerm |
| CCastrop1012 | 0:3a420fc22672 | 77 | uint8_t coolterm_data; // Se almacenan Todos los Caracteres recividos por el CoolTerm. |
| CCastrop1012 | 0:3a420fc22672 | 78 | |
| CCastrop1012 | 0:3a420fc22672 | 79 | |
| CCastrop1012 | 0:3a420fc22672 | 80 | // Constantes de velocidad |
| CCastrop1012 | 0:3a420fc22672 | 81 | |
| CCastrop1012 | 0:3a420fc22672 | 82 | #define VelAlta 300 |
| CCastrop1012 | 0:3a420fc22672 | 83 | #define VelMedia 200 |
| CCastrop1012 | 0:3a420fc22672 | 84 | #define VelBaja 100 |
| CCastrop1012 | 0:3a420fc22672 | 85 | |
| CCastrop1012 | 0:3a420fc22672 | 86 | |
| CCastrop1012 | 0:3a420fc22672 | 87 | //****************************************************************************** |
| CCastrop1012 | 0:3a420fc22672 | 88 | // COMANDOS |
| CCastrop1012 | 0:3a420fc22672 | 89 | |
| CCastrop1012 | 0:3a420fc22672 | 90 | #define iniciar_telemetria 0xFE |
| CCastrop1012 | 0:3a420fc22672 | 91 | #define iniciar_telecomando 0xFF |
| CCastrop1012 | 0:3a420fc22672 | 92 | |
| CCastrop1012 | 0:3a420fc22672 | 93 | #define telemetria_1 0x01 // |
| CCastrop1012 | 0:3a420fc22672 | 94 | #define telemcomando_1 0x01 |
| CCastrop1012 | 0:3a420fc22672 | 95 | |
| CCastrop1012 | 0:3a420fc22672 | 96 | #define C_LeerColor 0x00 |
| CCastrop1012 | 0:3a420fc22672 | 97 | #define C_Sonido1 0x01 |
| CCastrop1012 | 0:3a420fc22672 | 98 | #define C_Sonido2 0x02 |
| CCastrop1012 | 0:3a420fc22672 | 99 | #define C_Sonido3 0x03 |
| CCastrop1012 | 0:3a420fc22672 | 100 | #define C_Sonido4 0x04 |
| CCastrop1012 | 0:3a420fc22672 | 101 | #define C_Adelante 0x05 |
| CCastrop1012 | 0:3a420fc22672 | 102 | #define C_Atras 0x06 |
| CCastrop1012 | 0:3a420fc22672 | 103 | #define C_Izquierda 0x07 |
| CCastrop1012 | 0:3a420fc22672 | 104 | #define C_Derecha 0x08 |
| CCastrop1012 | 0:3a420fc22672 | 105 | #define C_Velocidad 0x09 |
| CCastrop1012 | 0:3a420fc22672 | 106 | #define C_Joistck 0x0A |
| CCastrop1012 | 0:3a420fc22672 | 107 | int comando_joystick = 0; |
| CCastrop1012 | 0:3a420fc22672 | 108 | |
| CCastrop1012 | 0:3a420fc22672 | 109 | |
| CCastrop1012 | 0:3a420fc22672 | 110 | // variables y constantes del Joystick |
| CCastrop1012 | 0:3a420fc22672 | 111 | |
| CCastrop1012 | 0:3a420fc22672 | 112 | uint8_t estado_x; |
| CCastrop1012 | 0:3a420fc22672 | 113 | uint8_t estado_y; |
| CCastrop1012 | 0:3a420fc22672 | 114 | #define E_Derecha 1 |
| CCastrop1012 | 0:3a420fc22672 | 115 | #define E_Izquier 0 |
| CCastrop1012 | 0:3a420fc22672 | 116 | #define E_Adelante 1 |
| CCastrop1012 | 0:3a420fc22672 | 117 | #define E_Atras 0 |
| CCastrop1012 | 0:3a420fc22672 | 118 | #define E_Centro 3 |
| CCastrop1012 | 0:3a420fc22672 | 119 | #define Lim_JKIn 150 |
| CCastrop1012 | 0:3a420fc22672 | 120 | #define Lim_JKSup 180 |
| CCastrop1012 | 0:3a420fc22672 | 121 | |
| CCastrop1012 | 0:3a420fc22672 | 122 | //**************************************************************************** |
| CCastrop1012 | 0:3a420fc22672 | 123 | // Prototipo de funciones |
| CCastrop1012 | 0:3a420fc22672 | 124 | |
| CCastrop1012 | 0:3a420fc22672 | 125 | void ReadPort(void); // Lee el puerto Serial |
| CCastrop1012 | 0:3a420fc22672 | 126 | void MainConfig(void); // Configuracion Inicial de los Perifericos del uC |
| CCastrop1012 | 0:3a420fc22672 | 127 | void Buzzer_Tone(uint8_t tipo_tono, uint8_t duracion_tono); // configura el tono y la duracion escuchada a travez del Buzzer |
| CCastrop1012 | 0:3a420fc22672 | 128 | void leer_color(void); // funcion que retorna los componentes |
| CCastrop1012 | 0:3a420fc22672 | 129 | // RGB y Clear del color leido |
| CCastrop1012 | 0:3a420fc22672 | 130 | void leer_Joystick(); // Ejerce control sobre el Movimiento del carro desde el Joistick |
| CCastrop1012 | 0:3a420fc22672 | 131 | |
| CCastrop1012 | 0:3a420fc22672 | 132 | //**************************************************************************** |
| CCastrop1012 | 0:3a420fc22672 | 133 | // |
| CCastrop1012 | 0:3a420fc22672 | 134 | #define valorInicial 0xAA |
| CCastrop1012 | 0:3a420fc22672 | 135 | uint8_t n_interrupcion = 0; |
| CCastrop1012 | 0:3a420fc22672 | 136 | uint8_t ComandoRecivido = valorInicial, Parametro = valorInicial; |
| CCastrop1012 | 0:3a420fc22672 | 137 | |
| CCastrop1012 | 0:3a420fc22672 | 138 | bool comm_pendi=false; |
| CCastrop1012 | 0:3a420fc22672 | 139 | void ReadPort() |
| CCastrop1012 | 0:3a420fc22672 | 140 | { |
| CCastrop1012 | 0:3a420fc22672 | 141 | uint8_t tm=CoolTerm.getc(); |
| CCastrop1012 | 0:3a420fc22672 | 142 | //if(CoolTerm.writable()) CoolTerm.abort_write(); // ELIMINA LO QUE ESTEMOS ESCRIBIENDO AL COOLTERM |
| CCastrop1012 | 0:3a420fc22672 | 143 | if (comm_pendi==false){ |
| CCastrop1012 | 0:3a420fc22672 | 144 | switch(n_interrupcion){ |
| CCastrop1012 | 0:3a420fc22672 | 145 | case 00: |
| CCastrop1012 | 0:3a420fc22672 | 146 | coolterm_data = tm; |
| CCastrop1012 | 0:3a420fc22672 | 147 | if (coolterm_data == iniciar_telecomando) |
| CCastrop1012 | 0:3a420fc22672 | 148 | n_interrupcion=1; |
| CCastrop1012 | 0:3a420fc22672 | 149 | break; |
| CCastrop1012 | 0:3a420fc22672 | 150 | case 01:ComandoRecivido = tm; n_interrupcion=2; break; |
| CCastrop1012 | 0:3a420fc22672 | 151 | case 02: |
| CCastrop1012 | 0:3a420fc22672 | 152 | Parametro = tm; |
| CCastrop1012 | 0:3a420fc22672 | 153 | n_interrupcion = 0; |
| CCastrop1012 | 0:3a420fc22672 | 154 | comm_pendi=true; |
| CCastrop1012 | 0:3a420fc22672 | 155 | break; |
| CCastrop1012 | 0:3a420fc22672 | 156 | } |
| CCastrop1012 | 0:3a420fc22672 | 157 | |
| CCastrop1012 | 0:3a420fc22672 | 158 | } |
| CCastrop1012 | 0:3a420fc22672 | 159 | // printf("%d",tm); |
| CCastrop1012 | 0:3a420fc22672 | 160 | } |
| CCastrop1012 | 0:3a420fc22672 | 161 | |
| CCastrop1012 | 0:3a420fc22672 | 162 | ///******************************************+ |
| CCastrop1012 | 0:3a420fc22672 | 163 | |
| CCastrop1012 | 0:3a420fc22672 | 164 | |
| CCastrop1012 | 0:3a420fc22672 | 165 | void leer_Joystick() |
| CCastrop1012 | 0:3a420fc22672 | 166 | { |
| CCastrop1012 | 0:3a420fc22672 | 167 | |
| CCastrop1012 | 0:3a420fc22672 | 168 | /// Variables Joystick |
| CCastrop1012 | 0:3a420fc22672 | 169 | float EjeX; |
| CCastrop1012 | 0:3a420fc22672 | 170 | float EjeY; |
| CCastrop1012 | 0:3a420fc22672 | 171 | float Vx; |
| CCastrop1012 | 0:3a420fc22672 | 172 | float Vy; |
| CCastrop1012 | 0:3a420fc22672 | 173 | |
| CCastrop1012 | 0:3a420fc22672 | 174 | while (comando_joystick == 1) // |
| CCastrop1012 | 0:3a420fc22672 | 175 | { |
| CCastrop1012 | 0:3a420fc22672 | 176 | |
| CCastrop1012 | 0:3a420fc22672 | 177 | EjeX = JEjeX.read(); |
| CCastrop1012 | 0:3a420fc22672 | 178 | Vx = EjeX * 3300; |
| CCastrop1012 | 0:3a420fc22672 | 179 | wait (0.1); |
| CCastrop1012 | 0:3a420fc22672 | 180 | EjeY = JEjeY.read(); |
| CCastrop1012 | 0:3a420fc22672 | 181 | Vy = EjeY * 3300; |
| CCastrop1012 | 0:3a420fc22672 | 182 | |
| CCastrop1012 | 0:3a420fc22672 | 183 | // CoolTerm.printf ("ejex: %f ejey: %f Vx: %f Vy: %f \n ", EjeX, EjeY, Vx, Vy); |
| CCastrop1012 | 0:3a420fc22672 | 184 | |
| CCastrop1012 | 0:3a420fc22672 | 185 | if(int(Vx/10) > Lim_JKIn && int(Vx/10) < Lim_JKSup) {estado_x = E_Centro; }//(CoolTerm.printf ("Estado X Centro \n"); } |
| CCastrop1012 | 0:3a420fc22672 | 186 | if(int(Vy/10) > Lim_JKIn && int(Vy/10) < Lim_JKSup) {estado_y = E_Centro; }//CoolTerm.printf ("Estado Y Centro \n"); } |
| CCastrop1012 | 0:3a420fc22672 | 187 | |
| CCastrop1012 | 0:3a420fc22672 | 188 | if(int(Vx/10) > Lim_JKSup && estado_y == E_Centro){ estado_x = E_Izquier; }// CoolTerm.printf ("Estado X Izquierda\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 189 | if(int(Vy/10) > Lim_JKSup && estado_x == E_Centro){ estado_y = E_Atras; }// CoolTerm.printf ("Estado Y Adelante\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 190 | |
| CCastrop1012 | 0:3a420fc22672 | 191 | if(int(Vx/10) < Lim_JKIn && estado_y == E_Centro){ estado_x = E_Derecha; } //CoolTerm.printf ("Estado X Derecha\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 192 | if(int(Vy/10) < Lim_JKIn && estado_x == E_Centro){ estado_y = E_Adelante; } //CoolTerm.printf ("Estado Y Atras\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 193 | |
| CCastrop1012 | 0:3a420fc22672 | 194 | |
| CCastrop1012 | 0:3a420fc22672 | 195 | // CoolTerm.printf ("\n\n X = %d Y = %d \n",estado_x , estado_y); |
| CCastrop1012 | 0:3a420fc22672 | 196 | // wait(2); |
| CCastrop1012 | 0:3a420fc22672 | 197 | // Combinacion de estados para STOP |
| CCastrop1012 | 0:3a420fc22672 | 198 | if( estado_x == E_Centro && estado_y == E_Centro){ Motores.Stop(); CoolTerm.printf ("MOTORES STOP\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 199 | |
| CCastrop1012 | 0:3a420fc22672 | 200 | // Combinacion de estados para ADELANTE |
| CCastrop1012 | 0:3a420fc22672 | 201 | if(estado_x == E_Centro && estado_y == E_Adelante) { Motores.Back(); Motores.Run(1);CoolTerm.printf ("MOTORES BACK\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 202 | |
| CCastrop1012 | 0:3a420fc22672 | 203 | // Combinacion de estados para ATRAS |
| CCastrop1012 | 0:3a420fc22672 | 204 | if(estado_x == E_Centro && estado_y == E_Atras) { Motores.Forward(); Motores.Run(1);CoolTerm.printf ("MOTORES FORWARD\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 205 | |
| CCastrop1012 | 0:3a420fc22672 | 206 | |
| CCastrop1012 | 0:3a420fc22672 | 207 | // Combinacion de estados para DERECHA |
| CCastrop1012 | 0:3a420fc22672 | 208 | if(estado_y == E_Centro && estado_x == E_Derecha) { Motores.Giro(15, false); Motores.Run(1); CoolTerm.printf ("MOTORES DERECHA\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 209 | |
| CCastrop1012 | 0:3a420fc22672 | 210 | // Combinacion de estados para IZQUIERDA |
| CCastrop1012 | 0:3a420fc22672 | 211 | if(estado_y == E_Centro && estado_x == E_Izquier) { Motores.Giro(15, true); Motores.Run(1); CoolTerm.printf ("MOTORES IZQUIERDA\n"); } |
| CCastrop1012 | 0:3a420fc22672 | 212 | //wait(1.5); |
| CCastrop1012 | 0:3a420fc22672 | 213 | } |
| CCastrop1012 | 0:3a420fc22672 | 214 | comando_joystick = 0; |
| CCastrop1012 | 0:3a420fc22672 | 215 | |
| CCastrop1012 | 0:3a420fc22672 | 216 | } |
| CCastrop1012 | 0:3a420fc22672 | 217 | |
| CCastrop1012 | 0:3a420fc22672 | 218 | |
| CCastrop1012 | 0:3a420fc22672 | 219 | int main() { |
| CCastrop1012 | 0:3a420fc22672 | 220 | |
| CCastrop1012 | 0:3a420fc22672 | 221 | CoolTerm.printf("inicio"); |
| CCastrop1012 | 0:3a420fc22672 | 222 | Buzzer.write(0); ///configura el ciclo util |
| CCastrop1012 | 0:3a420fc22672 | 223 | Motores.StepFreq(VelMedia); |
| CCastrop1012 | 0:3a420fc22672 | 224 | CoolTerm.attach(&ReadPort, Serial::RxIrq); //// se Habilita la interrupcion serial o recepcion de datos |
| CCastrop1012 | 0:3a420fc22672 | 225 | MuestrearCOLOR.attach(&leer_color, 0.6); |
| CCastrop1012 | 0:3a420fc22672 | 226 | |
| CCastrop1012 | 0:3a420fc22672 | 227 | //CoolTerm.printf("Hello World, System Run !!\n"); // mensaje inicial |
| CCastrop1012 | 0:3a420fc22672 | 228 | //leer_Joystick (); |
| CCastrop1012 | 0:3a420fc22672 | 229 | |
| CCastrop1012 | 0:3a420fc22672 | 230 | while(1) |
| CCastrop1012 | 0:3a420fc22672 | 231 | { |
| CCastrop1012 | 0:3a420fc22672 | 232 | |
| CCastrop1012 | 0:3a420fc22672 | 233 | /// Espera hasta recivir OxFF |
| CCastrop1012 | 0:3a420fc22672 | 234 | // while(programa_ejecutar != iniciar_telecomando); |
| CCastrop1012 | 0:3a420fc22672 | 235 | |
| CCastrop1012 | 0:3a420fc22672 | 236 | /// Espera hasta que reciva algo diferente de 0xAA |
| CCastrop1012 | 0:3a420fc22672 | 237 | // while(ComandoRecivido == valorInicial); |
| CCastrop1012 | 0:3a420fc22672 | 238 | |
| CCastrop1012 | 0:3a420fc22672 | 239 | /// Espera hasta que reciva algo diferente de 0xAA |
| CCastrop1012 | 0:3a420fc22672 | 240 | // while(Parametro == valorInicial); |
| CCastrop1012 | 0:3a420fc22672 | 241 | |
| CCastrop1012 | 0:3a420fc22672 | 242 | while(comm_pendi ==false); |
| CCastrop1012 | 0:3a420fc22672 | 243 | |
| CCastrop1012 | 0:3a420fc22672 | 244 | printf("okey comando :%d %d ", ComandoRecivido, Parametro); |
| CCastrop1012 | 0:3a420fc22672 | 245 | |
| CCastrop1012 | 0:3a420fc22672 | 246 | //// Desactivamos la interrupcion serial o recepcion de datos PORQUE NO NECESITAMOS recibir mas datos por AHORA |
| CCastrop1012 | 0:3a420fc22672 | 247 | // CoolTerm.attach(NULL, Serial::RxIrq); |
| CCastrop1012 | 0:3a420fc22672 | 248 | comm_pendi= false; |
| CCastrop1012 | 0:3a420fc22672 | 249 | |
| CCastrop1012 | 0:3a420fc22672 | 250 | switch(ComandoRecivido) |
| CCastrop1012 | 0:3a420fc22672 | 251 | { |
| CCastrop1012 | 0:3a420fc22672 | 252 | |
| CCastrop1012 | 0:3a420fc22672 | 253 | //case C_LeerColor: // Ejecutamos la Funcion LeerColor(); |
| CCastrop1012 | 0:3a420fc22672 | 254 | // leer_color(); |
| CCastrop1012 | 0:3a420fc22672 | 255 | //break; |
| CCastrop1012 | 0:3a420fc22672 | 256 | case C_Sonido1: //CoolTerm.printf("SONIDO 1\n"); |
| CCastrop1012 | 0:3a420fc22672 | 257 | duracion_tono = Parametro; // lo almacenamos en: duracion_tono |
| CCastrop1012 | 0:3a420fc22672 | 258 | tipo_tono = TONO_DO; |
| CCastrop1012 | 0:3a420fc22672 | 259 | Buzzer_Tone(tipo_tono, duracion_tono); |
| CCastrop1012 | 0:3a420fc22672 | 260 | break; |
| CCastrop1012 | 0:3a420fc22672 | 261 | case C_Sonido2: //CoolTerm.printf("SONIDO 2\n"); |
| CCastrop1012 | 0:3a420fc22672 | 262 | duracion_tono = Parametro; // lo almacenamos en: duracion_tono |
| CCastrop1012 | 0:3a420fc22672 | 263 | tipo_tono = TONO_RE; |
| CCastrop1012 | 0:3a420fc22672 | 264 | Buzzer_Tone(tipo_tono, duracion_tono); |
| CCastrop1012 | 0:3a420fc22672 | 265 | break; |
| CCastrop1012 | 0:3a420fc22672 | 266 | case C_Sonido3: //CoolTerm.printf("SONIDO 3\n"); |
| CCastrop1012 | 0:3a420fc22672 | 267 | duracion_tono = Parametro; // lo almacenamos en: duracion_tono |
| CCastrop1012 | 0:3a420fc22672 | 268 | tipo_tono = TONO_MI; |
| CCastrop1012 | 0:3a420fc22672 | 269 | Buzzer_Tone(tipo_tono, duracion_tono); |
| CCastrop1012 | 0:3a420fc22672 | 270 | break; |
| CCastrop1012 | 0:3a420fc22672 | 271 | case C_Sonido4: //CoolTerm.printf("SONIDO 4\n"); |
| CCastrop1012 | 0:3a420fc22672 | 272 | duracion_tono = Parametro; // lo almacenamos en: duracion_tono |
| CCastrop1012 | 0:3a420fc22672 | 273 | tipo_tono = TONO_SI; |
| CCastrop1012 | 0:3a420fc22672 | 274 | Buzzer_Tone(tipo_tono, duracion_tono); |
| CCastrop1012 | 0:3a420fc22672 | 275 | break; |
| CCastrop1012 | 0:3a420fc22672 | 276 | case C_Adelante: Motores.Forward(); Motores.RunRound(Parametro); |
| CCastrop1012 | 0:3a420fc22672 | 277 | break; |
| CCastrop1012 | 0:3a420fc22672 | 278 | case C_Atras: Motores.Back(); Motores.RunRound(Parametro); |
| CCastrop1012 | 0:3a420fc22672 | 279 | break; |
| CCastrop1012 | 0:3a420fc22672 | 280 | case C_Izquierda: Motores.Giro(65, true); |
| CCastrop1012 | 0:3a420fc22672 | 281 | break; |
| CCastrop1012 | 0:3a420fc22672 | 282 | case C_Derecha: Motores.Giro(65, false); |
| CCastrop1012 | 0:3a420fc22672 | 283 | break; |
| CCastrop1012 | 0:3a420fc22672 | 284 | case C_Velocidad: if(Parametro == 0x01)Motores.StepFreq(VelBaja); |
| CCastrop1012 | 0:3a420fc22672 | 285 | if(Parametro == 0x02)Motores.StepFreq(VelMedia); |
| CCastrop1012 | 0:3a420fc22672 | 286 | if(Parametro == 0x03)Motores.StepFreq(VelAlta); |
| CCastrop1012 | 0:3a420fc22672 | 287 | break; |
| CCastrop1012 | 0:3a420fc22672 | 288 | case C_Joistck: comando_joystick = 1; leer_Joystick(); |
| CCastrop1012 | 0:3a420fc22672 | 289 | break; |
| CCastrop1012 | 0:3a420fc22672 | 290 | default: break; |
| CCastrop1012 | 0:3a420fc22672 | 291 | |
| CCastrop1012 | 0:3a420fc22672 | 292 | } |
| CCastrop1012 | 0:3a420fc22672 | 293 | comm_pendi= false; |
| CCastrop1012 | 0:3a420fc22672 | 294 | //CoolTerm.printf("ProgramaFinalizado!!\n"); |
| CCastrop1012 | 0:3a420fc22672 | 295 | |
| CCastrop1012 | 0:3a420fc22672 | 296 | // Re inicializamos nuestras variables de control a sus valores iniciales |
| CCastrop1012 | 0:3a420fc22672 | 297 | // Para no seguir entrando a las sentencias IF |
| CCastrop1012 | 0:3a420fc22672 | 298 | // programa_ejecutar = 0; coolterm_data = 0; |
| CCastrop1012 | 0:3a420fc22672 | 299 | // ComandoRecivido = valorInicial; Parametro = valorInicial; |
| CCastrop1012 | 0:3a420fc22672 | 300 | //// HABILITAMOS NUEVAMENTE la interrupcion serial o recepcion de datos |
| CCastrop1012 | 0:3a420fc22672 | 301 | // CoolTerm.attach(&ReadPort, Serial::RxIrq); |
| CCastrop1012 | 0:3a420fc22672 | 302 | |
| CCastrop1012 | 0:3a420fc22672 | 303 | } |
| CCastrop1012 | 0:3a420fc22672 | 304 | |
| CCastrop1012 | 0:3a420fc22672 | 305 | } |
| CCastrop1012 | 0:3a420fc22672 | 306 | |
| CCastrop1012 | 0:3a420fc22672 | 307 | |
| CCastrop1012 | 0:3a420fc22672 | 308 | |
| CCastrop1012 | 0:3a420fc22672 | 309 | void Buzzer_Tone(uint8_t tipo_tono, uint8_t duracion_tono) |
| CCastrop1012 | 0:3a420fc22672 | 310 | { |
| CCastrop1012 | 0:3a420fc22672 | 311 | |
| CCastrop1012 | 0:3a420fc22672 | 312 | |
| CCastrop1012 | 0:3a420fc22672 | 313 | switch (tipo_tono) |
| CCastrop1012 | 0:3a420fc22672 | 314 | { |
| CCastrop1012 | 0:3a420fc22672 | 315 | |
| CCastrop1012 | 0:3a420fc22672 | 316 | case TONO_DO: Buzzer.period_ms(DO); |
| CCastrop1012 | 0:3a420fc22672 | 317 | //CoolTerm.printf("Tono Seleccionado DO!!\n"); |
| CCastrop1012 | 0:3a420fc22672 | 318 | |
| CCastrop1012 | 0:3a420fc22672 | 319 | break; // salimos del SWITCH |
| CCastrop1012 | 0:3a420fc22672 | 320 | |
| CCastrop1012 | 0:3a420fc22672 | 321 | case TONO_RE: Buzzer.period_ms(RE); |
| CCastrop1012 | 0:3a420fc22672 | 322 | //CoolTerm.printf("Tono Seleccionado RE!!\n"); |
| CCastrop1012 | 0:3a420fc22672 | 323 | |
| CCastrop1012 | 0:3a420fc22672 | 324 | break; // salimos del SWITCH |
| CCastrop1012 | 0:3a420fc22672 | 325 | |
| CCastrop1012 | 0:3a420fc22672 | 326 | case TONO_MI: Buzzer.period_ms(MI); |
| CCastrop1012 | 0:3a420fc22672 | 327 | //CoolTerm.printf("Tono Seleccionado MI!!\n"); |
| CCastrop1012 | 0:3a420fc22672 | 328 | |
| CCastrop1012 | 0:3a420fc22672 | 329 | break; // salimos del SWITCH |
| CCastrop1012 | 0:3a420fc22672 | 330 | |
| CCastrop1012 | 0:3a420fc22672 | 331 | case TONO_SI: Buzzer.period_ms(SI); |
| CCastrop1012 | 0:3a420fc22672 | 332 | //CoolTerm.printf("Tono Seleccionado SI!!\n"); |
| CCastrop1012 | 0:3a420fc22672 | 333 | |
| CCastrop1012 | 0:3a420fc22672 | 334 | break; // salimos del SWITCH |
| CCastrop1012 | 0:3a420fc22672 | 335 | |
| CCastrop1012 | 0:3a420fc22672 | 336 | // si no fue ninguno de los valores anteriores entonces: |
| CCastrop1012 | 0:3a420fc22672 | 337 | default: //CoolTerm.printf("teleComando desconocido, inicie nuevamente !!\n"); |
| CCastrop1012 | 0:3a420fc22672 | 338 | |
| CCastrop1012 | 0:3a420fc22672 | 339 | break; // salimos del SWITCH |
| CCastrop1012 | 0:3a420fc22672 | 340 | |
| CCastrop1012 | 0:3a420fc22672 | 341 | } |
| CCastrop1012 | 0:3a420fc22672 | 342 | // COMO EL CICLO UTIL DEL BUZZER ESTABA EN 0, POR LO CUAL NO SONABA |
| CCastrop1012 | 0:3a420fc22672 | 343 | // SE PONE AL 50% DEL PERIODO |
| CCastrop1012 | 0:3a420fc22672 | 344 | Buzzer.write(0.5); |
| CCastrop1012 | 0:3a420fc22672 | 345 | // SE ESPERA DURANTE EN TIEMPO INGRESADO (EN SEGUNDOS ) |
| CCastrop1012 | 0:3a420fc22672 | 346 | wait(duracion_tono); |
| CCastrop1012 | 0:3a420fc22672 | 347 | |
| CCastrop1012 | 0:3a420fc22672 | 348 | // Se Reinicializa el Periodo y el Ciclo útil de la señal PWM |
| CCastrop1012 | 0:3a420fc22672 | 349 | // que va al Buzzer |
| CCastrop1012 | 0:3a420fc22672 | 350 | Buzzer.period_ms(1); |
| CCastrop1012 | 0:3a420fc22672 | 351 | Buzzer.write(0); |
| CCastrop1012 | 0:3a420fc22672 | 352 | |
| CCastrop1012 | 0:3a420fc22672 | 353 | |
| CCastrop1012 | 0:3a420fc22672 | 354 | |
| CCastrop1012 | 0:3a420fc22672 | 355 | } |
| CCastrop1012 | 0:3a420fc22672 | 356 | |
| CCastrop1012 | 0:3a420fc22672 | 357 | |
| CCastrop1012 | 0:3a420fc22672 | 358 | |
| CCastrop1012 | 0:3a420fc22672 | 359 | void leer_color() |
| CCastrop1012 | 0:3a420fc22672 | 360 | { |
| CCastrop1012 | 0:3a420fc22672 | 361 | |
| CCastrop1012 | 0:3a420fc22672 | 362 | red = SENSOR_COLOR.ReadRed(); // OBTENEMOS EL TIEMPO DEL CICLO UTIL DE LA FRECUENCIA DE SALIDA |
| CCastrop1012 | 0:3a420fc22672 | 363 | green = SENSOR_COLOR.ReadGreen(); |
| CCastrop1012 | 0:3a420fc22672 | 364 | blue = SENSOR_COLOR.ReadBlue(); |
| CCastrop1012 | 0:3a420fc22672 | 365 | clear = SENSOR_COLOR.ReadClear(); |
| CCastrop1012 | 0:3a420fc22672 | 366 | |
| CCastrop1012 | 0:3a420fc22672 | 367 | //printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear); |
| CCastrop1012 | 0:3a420fc22672 | 368 | |
| CCastrop1012 | 0:3a420fc22672 | 369 | red *= 2; // Calculamos EL PERIODO de la frecuencia generada por la lectura del fotodiodo rojo |
| CCastrop1012 | 0:3a420fc22672 | 370 | blue *= 2; // Calculamos EL PERIODO de la frecuencia generada por la lectura del fotodiodo rojo |
| CCastrop1012 | 0:3a420fc22672 | 371 | green *= 2; // Calculamos EL PERIODO de la frecuencia generada por la lectura del fotodiodo rojo |
| CCastrop1012 | 0:3a420fc22672 | 372 | clear *= 2; // Calculamos EL PERIODO de la frecuencia generada por la lectura del fotodiodo rojo |
| CCastrop1012 | 0:3a420fc22672 | 373 | |
| CCastrop1012 | 0:3a420fc22672 | 374 | //printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear); |
| CCastrop1012 | 0:3a420fc22672 | 375 | |
| CCastrop1012 | 0:3a420fc22672 | 376 | |
| CCastrop1012 | 0:3a420fc22672 | 377 | ////////////////////////////////////////////////////////////// |
| CCastrop1012 | 0:3a420fc22672 | 378 | //// identificar azul |
| CCastrop1012 | 0:3a420fc22672 | 379 | |
| CCastrop1012 | 0:3a420fc22672 | 380 | |
| CCastrop1012 | 0:3a420fc22672 | 381 | if(red <=42 && red >=24) |
| CCastrop1012 | 0:3a420fc22672 | 382 | { |
| CCastrop1012 | 0:3a420fc22672 | 383 | if(green >= 20 && green <= 28 ) |
| CCastrop1012 | 0:3a420fc22672 | 384 | { |
| CCastrop1012 | 0:3a420fc22672 | 385 | if(blue >= 10 && blue <= 16) |
| CCastrop1012 | 0:3a420fc22672 | 386 | { |
| CCastrop1012 | 0:3a420fc22672 | 387 | color_identificado = CMD_azul; |
| CCastrop1012 | 0:3a420fc22672 | 388 | CoolTerm.putc( iniciar_telemetria); |
| CCastrop1012 | 0:3a420fc22672 | 389 | CoolTerm.putc( CMD_azul ); |
| CCastrop1012 | 0:3a420fc22672 | 390 | //Buzzer.period_ms(DO); |
| CCastrop1012 | 0:3a420fc22672 | 391 | //Buzzer.write(0.5); |
| CCastrop1012 | 0:3a420fc22672 | 392 | //wait(4); |
| CCastrop1012 | 0:3a420fc22672 | 393 | //Buzzer.write(0); |
| CCastrop1012 | 0:3a420fc22672 | 394 | |
| CCastrop1012 | 0:3a420fc22672 | 395 | } |
| CCastrop1012 | 0:3a420fc22672 | 396 | } |
| CCastrop1012 | 0:3a420fc22672 | 397 | } |
| CCastrop1012 | 0:3a420fc22672 | 398 | |
| CCastrop1012 | 0:3a420fc22672 | 399 | |
| CCastrop1012 | 0:3a420fc22672 | 400 | |
| CCastrop1012 | 0:3a420fc22672 | 401 | |
| CCastrop1012 | 0:3a420fc22672 | 402 | ///////////////////////////////////////////////////////////// |
| CCastrop1012 | 0:3a420fc22672 | 403 | /// identificar rojo |
| CCastrop1012 | 0:3a420fc22672 | 404 | if(red <= 12 ) |
| CCastrop1012 | 0:3a420fc22672 | 405 | { |
| CCastrop1012 | 0:3a420fc22672 | 406 | if(green >= 10 && green <= 28 ) |
| CCastrop1012 | 0:3a420fc22672 | 407 | { |
| CCastrop1012 | 0:3a420fc22672 | 408 | if(blue >= 18 && blue <= 24) |
| CCastrop1012 | 0:3a420fc22672 | 409 | { |
| CCastrop1012 | 0:3a420fc22672 | 410 | color_identificado = CMD_rojo; |
| CCastrop1012 | 0:3a420fc22672 | 411 | CoolTerm.putc( iniciar_telemetria); |
| CCastrop1012 | 0:3a420fc22672 | 412 | CoolTerm.putc( CMD_rojo ); |
| CCastrop1012 | 0:3a420fc22672 | 413 | //Buzzer.period_ms(RE); |
| CCastrop1012 | 0:3a420fc22672 | 414 | //Buzzer.write(0.5); //PERIODO UTIL |
| CCastrop1012 | 0:3a420fc22672 | 415 | //wait(4); //TIEMPO ACTIVO DEL BUZZER |
| CCastrop1012 | 0:3a420fc22672 | 416 | //Buzzer.write(0.0); |
| CCastrop1012 | 0:3a420fc22672 | 417 | } |
| CCastrop1012 | 0:3a420fc22672 | 418 | } |
| CCastrop1012 | 0:3a420fc22672 | 419 | |
| CCastrop1012 | 0:3a420fc22672 | 420 | if(green < 10 && green >= 6 ) |
| CCastrop1012 | 0:3a420fc22672 | 421 | { |
| CCastrop1012 | 0:3a420fc22672 | 422 | if(blue <= 12 ) |
| CCastrop1012 | 0:3a420fc22672 | 423 | { |
| CCastrop1012 | 0:3a420fc22672 | 424 | color_identificado = CMD_clear; |
| CCastrop1012 | 0:3a420fc22672 | 425 | CoolTerm.putc( iniciar_telemetria); |
| CCastrop1012 | 0:3a420fc22672 | 426 | CoolTerm.putc( CMD_clear ); |
| CCastrop1012 | 0:3a420fc22672 | 427 | //Buzzer.period_ms(MI); |
| CCastrop1012 | 0:3a420fc22672 | 428 | //Buzzer.write(0.5); |
| CCastrop1012 | 0:3a420fc22672 | 429 | //wait(4); |
| CCastrop1012 | 0:3a420fc22672 | 430 | //Buzzer.write(0); |
| CCastrop1012 | 0:3a420fc22672 | 431 | } |
| CCastrop1012 | 0:3a420fc22672 | 432 | |
| CCastrop1012 | 0:3a420fc22672 | 433 | } |
| CCastrop1012 | 0:3a420fc22672 | 434 | |
| CCastrop1012 | 0:3a420fc22672 | 435 | } |
| CCastrop1012 | 0:3a420fc22672 | 436 | |
| CCastrop1012 | 0:3a420fc22672 | 437 | |
| CCastrop1012 | 0:3a420fc22672 | 438 | if(green >= 36 && green <= 44 ) |
| CCastrop1012 | 0:3a420fc22672 | 439 | { |
| CCastrop1012 | 0:3a420fc22672 | 440 | if(red >= 40 && red <= 50 ) |
| CCastrop1012 | 0:3a420fc22672 | 441 | |
| CCastrop1012 | 0:3a420fc22672 | 442 | { |
| CCastrop1012 | 0:3a420fc22672 | 443 | color_identificado = CMD_verde; |
| CCastrop1012 | 0:3a420fc22672 | 444 | CoolTerm.putc( iniciar_telemetria); |
| CCastrop1012 | 0:3a420fc22672 | 445 | CoolTerm.putc( CMD_verde ); |
| CCastrop1012 | 0:3a420fc22672 | 446 | //Buzzer.period_ms(SI); |
| CCastrop1012 | 0:3a420fc22672 | 447 | //Buzzer.write(0.5); |
| CCastrop1012 | 0:3a420fc22672 | 448 | //wait(4); |
| CCastrop1012 | 0:3a420fc22672 | 449 | //Buzzer.write(0); |
| CCastrop1012 | 0:3a420fc22672 | 450 | |
| CCastrop1012 | 0:3a420fc22672 | 451 | |
| CCastrop1012 | 0:3a420fc22672 | 452 | } |
| CCastrop1012 | 0:3a420fc22672 | 453 | } |
| CCastrop1012 | 0:3a420fc22672 | 454 | |
| CCastrop1012 | 0:3a420fc22672 | 455 | if (color_identificado == ColorNoIdentificado) |
| CCastrop1012 | 0:3a420fc22672 | 456 | { |
| CCastrop1012 | 0:3a420fc22672 | 457 | |
| CCastrop1012 | 0:3a420fc22672 | 458 | |
| CCastrop1012 | 0:3a420fc22672 | 459 | CoolTerm.putc( iniciar_telemetria); |
| CCastrop1012 | 0:3a420fc22672 | 460 | CoolTerm.putc( ColorNoIdentificado); |
| CCastrop1012 | 0:3a420fc22672 | 461 | //Buzzer.period_ms(10); |
| CCastrop1012 | 0:3a420fc22672 | 462 | //Buzzer.write(0.5); |
| CCastrop1012 | 0:3a420fc22672 | 463 | //wait(4); |
| CCastrop1012 | 0:3a420fc22672 | 464 | //Buzzer.write(0); |
| CCastrop1012 | 0:3a420fc22672 | 465 | |
| CCastrop1012 | 0:3a420fc22672 | 466 | |
| CCastrop1012 | 0:3a420fc22672 | 467 | } |
| CCastrop1012 | 0:3a420fc22672 | 468 | |
| CCastrop1012 | 0:3a420fc22672 | 469 | color_identificado = ColorNoIdentificado; |
| CCastrop1012 | 0:3a420fc22672 | 470 | } |
| CCastrop1012 | 0:3a420fc22672 | 471 | |
| CCastrop1012 | 0:3a420fc22672 | 472 |