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@2:ee9eb5f76979, 2018-11-20 (annotated)

Committer:
Yances64
Date:
Tue Nov 20 01:56:56 2018 +0000
Revision:
2:ee9eb5f76979
Parent:
1:7d7dabfcd7ae
Child:
3:8bc95fb7c59c
FINAL FALTANTE

Who changed what in which revision?

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