Josimar Hernandez / Mbed 2 deprecated CUADRUPEDO

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

main.cpp@3:8bc95fb7c59c, 2018-11-22 (annotated)

Committer:
Yances64
Date:
Thu Nov 22 02:15:39 2018 +0000
Revision:
3:8bc95fb7c59c
Parent:
2:ee9eb5f76979 
Ara?a linki sistemas embebidos

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yances64 3:8bc95fb7c59c 1 /*Sistemas Embebidos
Yances64 3:8bc95fb7c59c 2 Presentado a:
Yances64 3:8bc95fb7c59c 3 Ingeniero Ferney Beltran
Yances64 3:8bc95fb7c59c 4 presentado por:
Yances64 3:8bc95fb7c59c 5 John Vanegas
Yances64 3:8bc95fb7c59c 6 Josimar Hernandez
Yances64 3:8bc95fb7c59c 7 Sebastian Aldana
Yances64 3:8bc95fb7c59c 8 Sergio Fonseca
Yances64 3:8bc95fb7c59c 9 */
Yances64 3:8bc95fb7c59c 10
Yances64 0:9df42b937ac1 11 #include "mbed.h"
Yances64 1:7d7dabfcd7ae 12 #include "main.h"
Yances64 1:7d7dabfcd7ae 13
Yances64 2:ee9eb5f76979 14
Yances64 1:7d7dabfcd7ae 15 int Conf_Servos();
Yances64 1:7d7dabfcd7ae 16 int ConfTransmi();
Yances64 2:ee9eb5f76979 17 void RutColor();
Yances64 2:ee9eb5f76979 18 void Joy();
Yances64 1:7d7dabfcd7ae 19 void MoverMotor (uint8_t motor, uint8_t grados);
Yances64 1:7d7dabfcd7ae 20 void MoverConjunto(uint8_t motores, uint8_t posicion);
Yances64 1:7d7dabfcd7ae 21
Yances64 2:ee9eb5f76979 22
Yances64 1:7d7dabfcd7ae 23 DigitalIn entrada (PE_15);
Yances64 1:7d7dabfcd7ae 24 DigitalOut S3 (PB_0);
Yances64 1:7d7dabfcd7ae 25 DigitalOut S2 (PE_0);
Yances64 2:ee9eb5f76979 26 AnalogIn analog_1(A0);
Yances64 2:ee9eb5f76979 27 DigitalOut led(LED1);
Yances64 1:7d7dabfcd7ae 28
Yances64 1:7d7dabfcd7ae 29 PwmOut Servo1(PB_11);
Yances64 1:7d7dabfcd7ae 30 PwmOut Servo2(PB_10);
Yances64 1:7d7dabfcd7ae 31 PwmOut Servo3(PA_0);
Yances64 1:7d7dabfcd7ae 32 PwmOut Servo4(PE_14);
Yances64 1:7d7dabfcd7ae 33 PwmOut Servo5(PE_12);
Yances64 1:7d7dabfcd7ae 34 PwmOut Servo6(PE_10);
Yances64 1:7d7dabfcd7ae 35 PwmOut Servo7(PD_12);
Yances64 1:7d7dabfcd7ae 36 PwmOut Servo8(PD_13);
Yances64 1:7d7dabfcd7ae 37
Yances64 1:7d7dabfcd7ae 38
Yances64 1:7d7dabfcd7ae 39 Serial Transmi(USBTX, USBRX);
Yances64 1:7d7dabfcd7ae 40 Timer tiempo;
Yances64 0:9df42b937ac1 41
Yances64 1:7d7dabfcd7ae 42 int inicio;
Yances64 1:7d7dabfcd7ae 43 char funcion;
Yances64 1:7d7dabfcd7ae 44 uint8_t motor_es;
Yances64 1:7d7dabfcd7ae 45 uint8_t grados_posicion;
Yances64 1:7d7dabfcd7ae 46 int cierre;
Yances64 1:7d7dabfcd7ae 47 float AnchoPulso;
Yances64 1:7d7dabfcd7ae 48 int valor;
Yances64 1:7d7dabfcd7ae 49 int color=0;
Yances64 1:7d7dabfcd7ae 50 int rojo=0;
Yances64 1:7d7dabfcd7ae 51 int azul=0;
Yances64 1:7d7dabfcd7ae 52 int verde=0;
Yances64 1:7d7dabfcd7ae 53 int Detectar();
Yances64 1:7d7dabfcd7ae 54 int tme=0;
Yances64 1:7d7dabfcd7ae 55
Yances64 1:7d7dabfcd7ae 56
Yances64 1:7d7dabfcd7ae 57
Yances64 1:7d7dabfcd7ae 58 int main() {
Yances64 1:7d7dabfcd7ae 59
Yances64 1:7d7dabfcd7ae 60 while (1){
Yances64 1:7d7dabfcd7ae 61
Yances64 1:7d7dabfcd7ae 62 Conf_Servos();
Yances64 1:7d7dabfcd7ae 63 ConfTransmi();
Yances64 1:7d7dabfcd7ae 64
Yances64 1:7d7dabfcd7ae 65 Transmi.printf("\n Inicio. \n");
Yances64 0:9df42b937ac1 66
Yances64 1:7d7dabfcd7ae 67 inicio=Transmi.getc();
Yances64 1:7d7dabfcd7ae 68 if (inicio==0xFF)
Yances64 1:7d7dabfcd7ae 69 {
Yances64 1:7d7dabfcd7ae 70 funcion=Transmi.getc();
Yances64 1:7d7dabfcd7ae 71 motor_es=Transmi.getc();
Yances64 1:7d7dabfcd7ae 72 grados_posicion=Transmi.getc();
Yances64 1:7d7dabfcd7ae 73 cierre=Transmi.getc();
Yances64 1:7d7dabfcd7ae 74
Yances64 1:7d7dabfcd7ae 75 if (cierre==0xFD){
Yances64 1:7d7dabfcd7ae 76
Yances64 1:7d7dabfcd7ae 77 switch (funcion){
Yances64 1:7d7dabfcd7ae 78 case 0x01:{
Yances64 1:7d7dabfcd7ae 79
Yances64 1:7d7dabfcd7ae 80 MoverMotor(motor_es,grados_posicion);
Yances64 1:7d7dabfcd7ae 81 break;
Yances64 1:7d7dabfcd7ae 82 }
Yances64 1:7d7dabfcd7ae 83 case 0x02:{
Yances64 1:7d7dabfcd7ae 84 MoverConjunto(motor_es,grados_posicion);
Yances64 1:7d7dabfcd7ae 85 break;
Yances64 1:7d7dabfcd7ae 86 }
Yances64 1:7d7dabfcd7ae 87 case 0x03:{
Yances64 2:ee9eb5f76979 88
Yances64 1:7d7dabfcd7ae 89 RutColor();
Yances64 2:ee9eb5f76979 90 break;
Yances64 2:ee9eb5f76979 91 }
Yances64 2:ee9eb5f76979 92
Yances64 2:ee9eb5f76979 93 case 0x04:{
Yances64 2:ee9eb5f76979 94 Joy();
Yances64 1:7d7dabfcd7ae 95 }
Yances64 2:ee9eb5f76979 96
Yances64 2:ee9eb5f76979 97
Yances64 2:ee9eb5f76979 98
Yances64 1:7d7dabfcd7ae 99 default:{
Yances64 1:7d7dabfcd7ae 100 Transmi.printf("Funcion no definida. \n");
Yances64 1:7d7dabfcd7ae 101 }
Yances64 1:7d7dabfcd7ae 102 }
Yances64 1:7d7dabfcd7ae 103 }
Yances64 1:7d7dabfcd7ae 104
Yances64 1:7d7dabfcd7ae 105 else {
Yances64 1:7d7dabfcd7ae 106 Transmi.printf("Error de codigo de cierre \n");
Yances64 1:7d7dabfcd7ae 107 }
Yances64 1:7d7dabfcd7ae 108 }
Yances64 1:7d7dabfcd7ae 109
Yances64 1:7d7dabfcd7ae 110 else {
Yances64 1:7d7dabfcd7ae 111 Transmi.printf("Error de codigo de inicio. \n");
Yances64 1:7d7dabfcd7ae 112 }
Yances64 1:7d7dabfcd7ae 113 }
Yances64 1:7d7dabfcd7ae 114
Yances64 1:7d7dabfcd7ae 115 }
Yances64 1:7d7dabfcd7ae 116
Yances64 1:7d7dabfcd7ae 117
Yances64 1:7d7dabfcd7ae 118 void MoverMotor(uint8_t motor, uint8_t grados){
Yances64 1:7d7dabfcd7ae 119
Yances64 1:7d7dabfcd7ae 120 Transmi.printf("Se movera solo un motor. \n");
Yances64 1:7d7dabfcd7ae 121 if (grados <= 180){
Yances64 1:7d7dabfcd7ae 122
Yances64 1:7d7dabfcd7ae 123 AnchoPulso=((grados*9.8)+535);
Yances64 1:7d7dabfcd7ae 124
Yances64 1:7d7dabfcd7ae 125
Yances64 1:7d7dabfcd7ae 126 switch (motor){
Yances64 1:7d7dabfcd7ae 127
Yances64 1:7d7dabfcd7ae 128 case 0x01:{
Yances64 1:7d7dabfcd7ae 129 Servo1.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 130 Transmi.printf("Se mueve el motor 1.\n");
Yances64 1:7d7dabfcd7ae 131 break;
Yances64 1:7d7dabfcd7ae 132
Yances64 1:7d7dabfcd7ae 133 }
Yances64 1:7d7dabfcd7ae 134
Yances64 1:7d7dabfcd7ae 135 case 0x02:{
Yances64 1:7d7dabfcd7ae 136 Servo2.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 137 Transmi.printf("Se mueve el motor 2.\n");
Yances64 1:7d7dabfcd7ae 138 break;
Yances64 1:7d7dabfcd7ae 139 }
Yances64 1:7d7dabfcd7ae 140
Yances64 1:7d7dabfcd7ae 141 case 0x03:{
Yances64 1:7d7dabfcd7ae 142 Servo3.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 143 Transmi.printf("Se mueve el motor 3.\n");
Yances64 1:7d7dabfcd7ae 144 break;
Yances64 1:7d7dabfcd7ae 145 }
Yances64 1:7d7dabfcd7ae 146
Yances64 1:7d7dabfcd7ae 147 case 0x04:{
Yances64 1:7d7dabfcd7ae 148 Servo4.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 149 Transmi.printf("Se mueve el motor 4.\n");
Yances64 1:7d7dabfcd7ae 150 break;
Yances64 1:7d7dabfcd7ae 151 }
Yances64 1:7d7dabfcd7ae 152
Yances64 1:7d7dabfcd7ae 153 case 0x05:{
Yances64 1:7d7dabfcd7ae 154 Servo5.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 155 Transmi.printf("Se mueve el motor 5.\n");
Yances64 1:7d7dabfcd7ae 156 break;
Yances64 1:7d7dabfcd7ae 157 }
Yances64 1:7d7dabfcd7ae 158 case 0x06:{
Yances64 1:7d7dabfcd7ae 159 Servo6.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 160 Transmi.printf("Se mueve el motor 6.\n");
Yances64 1:7d7dabfcd7ae 161 break;
Yances64 1:7d7dabfcd7ae 162 }
Yances64 1:7d7dabfcd7ae 163
Yances64 1:7d7dabfcd7ae 164 case 0x07:{
Yances64 1:7d7dabfcd7ae 165 Servo7.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 166 Transmi.printf("Se mueve el motor 7.\n");
Yances64 1:7d7dabfcd7ae 167 break;
Yances64 1:7d7dabfcd7ae 168 }
Yances64 1:7d7dabfcd7ae 169
Yances64 1:7d7dabfcd7ae 170 case 0x08:{
Yances64 1:7d7dabfcd7ae 171 Servo8.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 172 Transmi.printf("Se mueve el motor 8.\n");
Yances64 1:7d7dabfcd7ae 173 break;
Yances64 1:7d7dabfcd7ae 174 }
Yances64 1:7d7dabfcd7ae 175
Yances64 1:7d7dabfcd7ae 176 default:{
Yances64 1:7d7dabfcd7ae 177 Transmi.printf("Motor no cargado. \n");
Yances64 1:7d7dabfcd7ae 178 }
Yances64 1:7d7dabfcd7ae 179 }
Yances64 1:7d7dabfcd7ae 180
Yances64 1:7d7dabfcd7ae 181 }
Yances64 1:7d7dabfcd7ae 182
Yances64 1:7d7dabfcd7ae 183 else {
Yances64 1:7d7dabfcd7ae 184 Transmi.printf("Valor de grados fuera de rango. \n");
Yances64 1:7d7dabfcd7ae 185 }
Yances64 1:7d7dabfcd7ae 186
Yances64 1:7d7dabfcd7ae 187 }
Yances64 1:7d7dabfcd7ae 188
Yances64 1:7d7dabfcd7ae 189
Yances64 1:7d7dabfcd7ae 190 void MoverConjunto(uint8_t motores,uint8_t posicion){
Yances64 1:7d7dabfcd7ae 191 Transmi.printf("Se movera un conjunto de motores. \n");
Yances64 1:7d7dabfcd7ae 192
Yances64 1:7d7dabfcd7ae 193 if (posicion==0xDD){
Yances64 0:9df42b937ac1 194
Yances64 1:7d7dabfcd7ae 195 switch(motores){
Yances64 1:7d7dabfcd7ae 196 case 1:{
Yances64 1:7d7dabfcd7ae 197 Servo1.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 198 Servo2.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 199 Transmi.printf("Mover primer conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 200 break;
Yances64 1:7d7dabfcd7ae 201 }
Yances64 1:7d7dabfcd7ae 202
Yances64 1:7d7dabfcd7ae 203 case 2:{
Yances64 1:7d7dabfcd7ae 204 Servo3.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 205 Servo4.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 206 Transmi.printf("Mover segundo conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 207 break;
Yances64 1:7d7dabfcd7ae 208 }
Yances64 0:9df42b937ac1 209
Yances64 1:7d7dabfcd7ae 210 case 3:{
Yances64 1:7d7dabfcd7ae 211 Servo5.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 212 Servo6.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 213 Transmi.printf("Mover tercer conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 214 break;
Yances64 1:7d7dabfcd7ae 215 }
Yances64 0:9df42b937ac1 216
Yances64 1:7d7dabfcd7ae 217 case 4:{
Yances64 1:7d7dabfcd7ae 218 Servo7.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 219 Servo8.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 220 Transmi.printf("Mover cuarto conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 221 break;
Yances64 1:7d7dabfcd7ae 222 }
Yances64 1:7d7dabfcd7ae 223
Yances64 1:7d7dabfcd7ae 224 default: {
Yances64 1:7d7dabfcd7ae 225 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 226 }
Yances64 1:7d7dabfcd7ae 227 }
Yances64 1:7d7dabfcd7ae 228 }
Yances64 1:7d7dabfcd7ae 229
Yances64 1:7d7dabfcd7ae 230 else{
Yances64 1:7d7dabfcd7ae 231 if (posicion==0xDA){
Yances64 1:7d7dabfcd7ae 232
Yances64 1:7d7dabfcd7ae 233 switch(motores){
Yances64 1:7d7dabfcd7ae 234 case 1:{
Yances64 1:7d7dabfcd7ae 235 Servo1.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 236 Servo2.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 237 Transmi.printf("Mover primer conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 238 break;
Yances64 1:7d7dabfcd7ae 239 }
Yances64 1:7d7dabfcd7ae 240
Yances64 1:7d7dabfcd7ae 241 case 2:{
Yances64 1:7d7dabfcd7ae 242 Servo3.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 243 Servo4.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 244 Transmi.printf("Mover segundo conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 245 break;
Yances64 1:7d7dabfcd7ae 246 }
Yances64 1:7d7dabfcd7ae 247
Yances64 1:7d7dabfcd7ae 248 case 3:{
Yances64 1:7d7dabfcd7ae 249 Servo5.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 250 Servo6.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 251 Transmi.printf("Mover tercer conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 252 break;
Yances64 1:7d7dabfcd7ae 253 }
Yances64 0:9df42b937ac1 254
Yances64 1:7d7dabfcd7ae 255 case 4:{
Yances64 1:7d7dabfcd7ae 256 Servo7.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 257 Servo8.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 258 Transmi.printf("Mover cuarto conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 259 break;
Yances64 1:7d7dabfcd7ae 260 }
Yances64 1:7d7dabfcd7ae 261
Yances64 1:7d7dabfcd7ae 262 default: {
Yances64 1:7d7dabfcd7ae 263 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 264 }
Yances64 1:7d7dabfcd7ae 265 }
Yances64 1:7d7dabfcd7ae 266 }
Yances64 1:7d7dabfcd7ae 267
Yances64 1:7d7dabfcd7ae 268
Yances64 1:7d7dabfcd7ae 269 else{
Yances64 1:7d7dabfcd7ae 270
Yances64 1:7d7dabfcd7ae 271 if (posicion==0x1D){
Yances64 1:7d7dabfcd7ae 272
Yances64 1:7d7dabfcd7ae 273 switch(motores){
Yances64 1:7d7dabfcd7ae 274 case 1:{
Yances64 1:7d7dabfcd7ae 275 Servo1.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 276 Servo2.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 277 Transmi.printf("Mover primer conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 278 break;
Yances64 1:7d7dabfcd7ae 279 }
Yances64 1:7d7dabfcd7ae 280
Yances64 1:7d7dabfcd7ae 281 case 2:{
Yances64 1:7d7dabfcd7ae 282 Servo3.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 283 Servo4.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 284 Transmi.printf("Mover segundo conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 285 break;
Yances64 1:7d7dabfcd7ae 286 }
Yances64 1:7d7dabfcd7ae 287
Yances64 1:7d7dabfcd7ae 288 case 3:{
Yances64 1:7d7dabfcd7ae 289 Servo5.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 290 Servo6.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 291 Transmi.printf("Mover tercer conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 292 break;
Yances64 1:7d7dabfcd7ae 293 }
Yances64 1:7d7dabfcd7ae 294
Yances64 1:7d7dabfcd7ae 295 case 4:{
Yances64 1:7d7dabfcd7ae 296 Servo7.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 297 Servo8.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 298 Transmi.printf("Mover cuarto conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 299 break;
Yances64 1:7d7dabfcd7ae 300 }
Yances64 0:9df42b937ac1 301
Yances64 1:7d7dabfcd7ae 302 default: {
Yances64 1:7d7dabfcd7ae 303 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 304 }
Yances64 1:7d7dabfcd7ae 305 }
Yances64 1:7d7dabfcd7ae 306 }
Yances64 1:7d7dabfcd7ae 307 else{
Yances64 1:7d7dabfcd7ae 308 if (posicion==0x1A){
Yances64 1:7d7dabfcd7ae 309
Yances64 1:7d7dabfcd7ae 310 switch(motores){
Yances64 1:7d7dabfcd7ae 311 case 1:{
Yances64 1:7d7dabfcd7ae 312 Servo1.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 313 Servo2.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 314 Transmi.printf("Mover primer conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 315 break;
Yances64 1:7d7dabfcd7ae 316 }
Yances64 1:7d7dabfcd7ae 317
Yances64 1:7d7dabfcd7ae 318 case 2:{
Yances64 1:7d7dabfcd7ae 319 Servo3.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 320 Servo4.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 321 Transmi.printf("Mover segundo conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 322 break;
Yances64 1:7d7dabfcd7ae 323 }
Yances64 1:7d7dabfcd7ae 324
Yances64 1:7d7dabfcd7ae 325 case 3:{
Yances64 1:7d7dabfcd7ae 326 Servo5.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 327 Servo6.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 328 Transmi.printf("Mover tercer conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 329 break;
Yances64 1:7d7dabfcd7ae 330 }
Yances64 1:7d7dabfcd7ae 331
Yances64 1:7d7dabfcd7ae 332 case 4:{
Yances64 1:7d7dabfcd7ae 333 Servo7.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 334 Servo8.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 335 Transmi.printf("Mover tercer conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 336 break;
Yances64 1:7d7dabfcd7ae 337 }
Yances64 0:9df42b937ac1 338
Yances64 1:7d7dabfcd7ae 339 default: {
Yances64 1:7d7dabfcd7ae 340 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 341 }
Yances64 1:7d7dabfcd7ae 342 }
Yances64 1:7d7dabfcd7ae 343 }
Yances64 1:7d7dabfcd7ae 344
Yances64 1:7d7dabfcd7ae 345 else{
Yances64 1:7d7dabfcd7ae 346 Transmi.printf("Movimiento no definido. \n");
Yances64 1:7d7dabfcd7ae 347
Yances64 1:7d7dabfcd7ae 348 }
Yances64 0:9df42b937ac1 349 }
Yances64 1:7d7dabfcd7ae 350 }
Yances64 1:7d7dabfcd7ae 351 }
Yances64 1:7d7dabfcd7ae 352
Yances64 1:7d7dabfcd7ae 353
Yances64 1:7d7dabfcd7ae 354 }
Yances64 1:7d7dabfcd7ae 355
Yances64 1:7d7dabfcd7ae 356
Yances64 1:7d7dabfcd7ae 357
Yances64 1:7d7dabfcd7ae 358 int Conf_Servos(){
Yances64 1:7d7dabfcd7ae 359 Servo1.period_ms(20);
Yances64 1:7d7dabfcd7ae 360 Servo2.period_ms(20);
Yances64 1:7d7dabfcd7ae 361 Servo3.period_ms(20);
Yances64 1:7d7dabfcd7ae 362 Servo4.period_ms(20);
Yances64 1:7d7dabfcd7ae 363 Servo5.period_ms(20);
Yances64 1:7d7dabfcd7ae 364 Servo6.period_ms(20);
Yances64 1:7d7dabfcd7ae 365 Servo7.period_ms(20);
Yances64 1:7d7dabfcd7ae 366 Servo8.period_ms(20);
Yances64 1:7d7dabfcd7ae 367 return (0);
Yances64 1:7d7dabfcd7ae 368 }
Yances64 1:7d7dabfcd7ae 369
Yances64 1:7d7dabfcd7ae 370
Yances64 1:7d7dabfcd7ae 371 int ConfTransmi(){
Yances64 1:7d7dabfcd7ae 372 Transmi.baud(9600);
Yances64 1:7d7dabfcd7ae 373 return(0);
Yances64 1:7d7dabfcd7ae 374 }
Yances64 1:7d7dabfcd7ae 375
Yances64 2:ee9eb5f76979 376 void RutColor(){
Yances64 1:7d7dabfcd7ae 377
Yances64 1:7d7dabfcd7ae 378 S2=0;
Yances64 1:7d7dabfcd7ae 379 S3=0;
Yances64 1:7d7dabfcd7ae 380 rojo=Detectar();
Yances64 1:7d7dabfcd7ae 381
Yances64 1:7d7dabfcd7ae 382 S2=0;
Yances64 1:7d7dabfcd7ae 383 S3=1;
Yances64 1:7d7dabfcd7ae 384 azul=Detectar();
Yances64 1:7d7dabfcd7ae 385
Yances64 1:7d7dabfcd7ae 386 S2=1;
Yances64 1:7d7dabfcd7ae 387 S3=1;
Yances64 1:7d7dabfcd7ae 388 verde=Detectar();
Yances64 1:7d7dabfcd7ae 389
Yances64 3:8bc95fb7c59c 390 if ((rojo>300)&(rojo<460) & (azul>100)&(azul<160) & (verde>200)&(verde<310)){
Yances64 3:8bc95fb7c59c 391
Yances64 3:8bc95fb7c59c 392 Transmi.printf("AZUL \n");
Yances64 2:ee9eb5f76979 393 Servo1.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 394 wait (0.5);
Yances64 2:ee9eb5f76979 395 Servo2.pulsewidth_us(1319);
Yances64 2:ee9eb5f76979 396 wait (0.5);
Yances64 2:ee9eb5f76979 397 Servo1.pulsewidth_us(1412);
Yances64 2:ee9eb5f76979 398 wait (0.5);
Yances64 2:ee9eb5f76979 399 Servo7.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 400 wait (0.5);
Yances64 2:ee9eb5f76979 401 Servo8.pulsewidth_us(1809);
Yances64 2:ee9eb5f76979 402 wait (0.5);
Yances64 2:ee9eb5f76979 403 Servo7.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 404 wait (0.5);
Yances64 2:ee9eb5f76979 405 Servo2.pulsewidth_us(1515);
Yances64 2:ee9eb5f76979 406 Servo8.pulsewidth_us(1515);
Yances64 2:ee9eb5f76979 407 wait (0.5);
Yances64 2:ee9eb5f76979 408 Servo3.pulsewidth_us(1221);
Yances64 2:ee9eb5f76979 409 wait (0.5);
Yances64 2:ee9eb5f76979 410 Servo4.pulsewidth_us(1613);
Yances64 2:ee9eb5f76979 411 wait (0.5);
Yances64 2:ee9eb5f76979 412 Servo3.pulsewidth_us(1613);
Yances64 2:ee9eb5f76979 413 wait (0.5);
Yances64 2:ee9eb5f76979 414 Servo5.pulsewidth_us(731);
Yances64 2:ee9eb5f76979 415 wait (0.5);
Yances64 2:ee9eb5f76979 416 Servo6.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 417 wait (0.5);
Yances64 2:ee9eb5f76979 418 Servo5.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 419 wait (0.5);
Yances64 2:ee9eb5f76979 420 Servo4.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 421 Servo6.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 422 wait (0.5);
Yances64 1:7d7dabfcd7ae 423 }
Yances64 1:7d7dabfcd7ae 424
Yances64 1:7d7dabfcd7ae 425 else{
Yances64 3:8bc95fb7c59c 426 if ((rojo>70)&(rojo<80) & (azul>170)&(azul<220) & (verde>250)&(verde<300)){
Yances64 3:8bc95fb7c59c 427
Yances64 3:8bc95fb7c59c 428 Transmi.printf("ROJO\n");
Yances64 2:ee9eb5f76979 429 Servo1.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 430 wait (0.5);
Yances64 2:ee9eb5f76979 431 Servo2.pulsewidth_us(1711);
Yances64 2:ee9eb5f76979 432 wait (0.5);
Yances64 2:ee9eb5f76979 433 Servo1.pulsewidth_us(1412);
Yances64 2:ee9eb5f76979 434 wait (0.5);
Yances64 2:ee9eb5f76979 435 Servo7.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 436 wait (0.5);
Yances64 2:ee9eb5f76979 437 Servo8.pulsewidth_us(1319);
Yances64 2:ee9eb5f76979 438 wait (0.5);
Yances64 2:ee9eb5f76979 439 Servo7.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 440 wait (0.5);
Yances64 2:ee9eb5f76979 441 Servo2.pulsewidth_us(1515);
Yances64 2:ee9eb5f76979 442 Servo8.pulsewidth_us(1515);
Yances64 2:ee9eb5f76979 443 wait (0.5);
Yances64 2:ee9eb5f76979 444 Servo3.pulsewidth_us(1221);
Yances64 2:ee9eb5f76979 445 wait (0.5);
Yances64 2:ee9eb5f76979 446 Servo4.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 447 wait (0.5);
Yances64 2:ee9eb5f76979 448 Servo3.pulsewidth_us(1613);
Yances64 2:ee9eb5f76979 449 wait (0.5);
Yances64 2:ee9eb5f76979 450 Servo5.pulsewidth_us(731);
Yances64 2:ee9eb5f76979 451 wait (0.5);
Yances64 2:ee9eb5f76979 452 Servo6.pulsewidth_us(1613);
Yances64 2:ee9eb5f76979 453 wait (0.5);
Yances64 2:ee9eb5f76979 454 Servo5.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 455 wait (0.5);
Yances64 2:ee9eb5f76979 456 Servo4.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 457 Servo6.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 458 wait (0.5);
Yances64 2:ee9eb5f76979 459 }
Yances64 1:7d7dabfcd7ae 460
Yances64 1:7d7dabfcd7ae 461 else{
Yances64 2:ee9eb5f76979 462
Yances64 3:8bc95fb7c59c 463 if ((rojo>90)&(rojo<120) & (azul>121)&(azul<170) & (verde>100)&(verde<110)){
Yances64 3:8bc95fb7c59c 464
Yances64 3:8bc95fb7c59c 465 Transmi.printf("VERDE\n");
Yances64 2:ee9eb5f76979 466 Servo1.pulsewidth_us(700);
Yances64 2:ee9eb5f76979 467 wait (1);
Yances64 2:ee9eb5f76979 468 Servo3.pulsewidth_us(700);
Yances64 2:ee9eb5f76979 469 wait (1);
Yances64 2:ee9eb5f76979 470 Servo5.pulsewidth_us(700);
Yances64 2:ee9eb5f76979 471 wait (1);
Yances64 2:ee9eb5f76979 472 Servo7.pulsewidth_us(700);
Yances64 2:ee9eb5f76979 473 wait (1);
Yances64 2:ee9eb5f76979 474 Servo3.pulsewidth_us(700);
Yances64 2:ee9eb5f76979 475 wait (1);
Yances64 2:ee9eb5f76979 476 Servo1.pulsewidth_us(1500);
Yances64 2:ee9eb5f76979 477 wait (1);
Yances64 2:ee9eb5f76979 478 Servo3.pulsewidth_us(1500);
Yances64 2:ee9eb5f76979 479 wait (1);
Yances64 2:ee9eb5f76979 480 Servo5.pulsewidth_us(1500);
Yances64 2:ee9eb5f76979 481 wait (1);
Yances64 2:ee9eb5f76979 482 Servo7.pulsewidth_us(1500);
Yances64 2:ee9eb5f76979 483 wait (1);
Yances64 2:ee9eb5f76979 484
Yances64 2:ee9eb5f76979 485
Yances64 2:ee9eb5f76979 486 }
Yances64 2:ee9eb5f76979 487 else{
Yances64 2:ee9eb5f76979 488 Transmi.printf("color no valido. \n");}
Yances64 1:7d7dabfcd7ae 489 }
Yances64 1:7d7dabfcd7ae 490 }
Yances64 1:7d7dabfcd7ae 491 }
Yances64 1:7d7dabfcd7ae 492
Yances64 1:7d7dabfcd7ae 493 int Detectar(){
Yances64 1:7d7dabfcd7ae 494 tme=0;
Yances64 1:7d7dabfcd7ae 495 while (!entrada){}
Yances64 1:7d7dabfcd7ae 496 while (entrada) {}
Yances64 1:7d7dabfcd7ae 497 while (!entrada){}
Yances64 1:7d7dabfcd7ae 498 tiempo.start();
Yances64 1:7d7dabfcd7ae 499 while (entrada) {}
Yances64 1:7d7dabfcd7ae 500 while (!entrada){}
Yances64 1:7d7dabfcd7ae 501 while (entrada) {}
Yances64 1:7d7dabfcd7ae 502 while (!entrada){}
Yances64 1:7d7dabfcd7ae 503 while (entrada) {}
Yances64 1:7d7dabfcd7ae 504 tiempo.stop();
Yances64 1:7d7dabfcd7ae 505 tme=tiempo.read_us();
Yances64 1:7d7dabfcd7ae 506 tiempo.reset();
Yances64 1:7d7dabfcd7ae 507 return(tme);
Yances64 1:7d7dabfcd7ae 508
Yances64 2:ee9eb5f76979 509 }
Yances64 2:ee9eb5f76979 510
Yances64 2:ee9eb5f76979 511 void Joy(){
Yances64 0:9df42b937ac1 512
Yances64 2:ee9eb5f76979 513
Yances64 1:7d7dabfcd7ae 514
Yances64 1:7d7dabfcd7ae 515
Yances64 2:ee9eb5f76979 516 float medir_r;
Yances64 2:ee9eb5f76979 517 float medir_v;
Yances64 2:ee9eb5f76979 518
Yances64 1:7d7dabfcd7ae 519
Yances64 2:ee9eb5f76979 520 printf("MANDO\n");
Yances64 2:ee9eb5f76979 521
Yances64 2:ee9eb5f76979 522 while(funcion==4) {
Yances64 2:ee9eb5f76979 523
Yances64 3:8bc95fb7c59c 524 medir_r = analog_1.read();
Yances64 2:ee9eb5f76979 525
Yances64 3:8bc95fb7c59c 526 medir_v = medir_r * 3300;
Yances64 2:ee9eb5f76979 527
Yances64 2:ee9eb5f76979 528 if (medir_v < 1000) {
Yances64 2:ee9eb5f76979 529
Yances64 2:ee9eb5f76979 530 Servo1.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 531 Servo2.pulsewidth_us(1319);
Yances64 3:8bc95fb7c59c 532 wait (0.3);
Yances64 2:ee9eb5f76979 533 Servo1.pulsewidth_us(1412);
Yances64 3:8bc95fb7c59c 534 wait (0.3);
Yances64 2:ee9eb5f76979 535
Yances64 2:ee9eb5f76979 536 Servo7.pulsewidth_us(1025);
Yances64 2:ee9eb5f76979 537 Servo8.pulsewidth_us(1809);
Yances64 3:8bc95fb7c59c 538 wait (0.3);
Yances64 2:ee9eb5f76979 539 Servo7.pulsewidth_us(1417);
Yances64 3:8bc95fb7c59c 540 wait (0.3);
Yances64 2:ee9eb5f76979 541
Yances64 2:ee9eb5f76979 542 Servo2.pulsewidth_us(1515);
Yances64 2:ee9eb5f76979 543 Servo8.pulsewidth_us(1515);
Yances64 3:8bc95fb7c59c 544 wait (0.3);
Yances64 2:ee9eb5f76979 545 Servo3.pulsewidth_us(1221);
Yances64 2:ee9eb5f76979 546 Servo4.pulsewidth_us(1613);
Yances64 3:8bc95fb7c59c 547 wait (0.3);
Yances64 2:ee9eb5f76979 548 Servo3.pulsewidth_us(1613);
Yances64 3:8bc95fb7c59c 549 wait (0.3);
Yances64 2:ee9eb5f76979 550
Yances64 2:ee9eb5f76979 551 Servo5.pulsewidth_us(731);
Yances64 2:ee9eb5f76979 552 Servo6.pulsewidth_us(1025);
Yances64 3:8bc95fb7c59c 553 wait (0.3);
Yances64 2:ee9eb5f76979 554 Servo5.pulsewidth_us(1025);
Yances64 3:8bc95fb7c59c 555 wait (0.3);
Yances64 2:ee9eb5f76979 556
Yances64 2:ee9eb5f76979 557 Servo4.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 558 Servo6.pulsewidth_us(1417);
Yances64 3:8bc95fb7c59c 559 wait (0.3);
Yances64 3:8bc95fb7c59c 560
Yances64 2:ee9eb5f76979 561 } else {
Yances64 2:ee9eb5f76979 562 if (medir_v > 2000){
Yances64 3:8bc95fb7c59c 563 wait (0.3);
Yances64 2:ee9eb5f76979 564 Servo2.pulsewidth_us(1711);
Yances64 3:8bc95fb7c59c 565 wait (0.3);
Yances64 2:ee9eb5f76979 566 Servo1.pulsewidth_us(1412);
Yances64 3:8bc95fb7c59c 567 wait (0.3);
Yances64 2:ee9eb5f76979 568 Servo7.pulsewidth_us(1025);
Yances64 3:8bc95fb7c59c 569 wait (0.3);
Yances64 2:ee9eb5f76979 570 Servo8.pulsewidth_us(1319);
Yances64 3:8bc95fb7c59c 571 wait (0.3);
Yances64 2:ee9eb5f76979 572 Servo7.pulsewidth_us(1417);
Yances64 3:8bc95fb7c59c 573 wait (0.3);
Yances64 2:ee9eb5f76979 574 Servo2.pulsewidth_us(1515);
Yances64 2:ee9eb5f76979 575 Servo8.pulsewidth_us(1515);
Yances64 3:8bc95fb7c59c 576 wait (0.3);
Yances64 2:ee9eb5f76979 577 Servo3.pulsewidth_us(1221);
Yances64 3:8bc95fb7c59c 578 wait (0.3);
Yances64 2:ee9eb5f76979 579 Servo4.pulsewidth_us(1025);
Yances64 3:8bc95fb7c59c 580 wait (0.3);
Yances64 2:ee9eb5f76979 581 Servo3.pulsewidth_us(1613);
Yances64 3:8bc95fb7c59c 582 wait (0.3);
Yances64 2:ee9eb5f76979 583 Servo5.pulsewidth_us(731);
Yances64 3:8bc95fb7c59c 584 wait (0.3);
Yances64 2:ee9eb5f76979 585 Servo6.pulsewidth_us(1613);
Yances64 3:8bc95fb7c59c 586 wait (0.3);
Yances64 2:ee9eb5f76979 587 Servo5.pulsewidth_us(1025);
Yances64 3:8bc95fb7c59c 588 wait (0.3);
Yances64 2:ee9eb5f76979 589 Servo4.pulsewidth_us(1417);
Yances64 2:ee9eb5f76979 590 Servo6.pulsewidth_us(1417);
Yances64 3:8bc95fb7c59c 591 wait (0.3);
Yances64 2:ee9eb5f76979 592
Yances64 2:ee9eb5f76979 593 }
Yances64 3:8bc95fb7c59c 594 else{
Yances64 3:8bc95fb7c59c 595 wait (1.0);}
Yances64 2:ee9eb5f76979 596 }
Yances64 2:ee9eb5f76979 597 }
Yances64 2:ee9eb5f76979 598 }
Yances64 2:ee9eb5f76979 599
Yances64 2:ee9eb5f76979 600