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@1:7d7dabfcd7ae, 2018-10-08 (annotated)

Committer:
Yances64
Date:
Mon Oct 08 23:25:59 2018 +0000
Revision:
1:7d7dabfcd7ae
Parent:
0:9df42b937ac1
Child:
2:ee9eb5f76979
PRESENTACI?N SEGUNDO CORTE; ;

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 1:7d7dabfcd7ae 4 int Conf_Servos();
Yances64 1:7d7dabfcd7ae 5 int ConfTransmi();
Yances64 1:7d7dabfcd7ae 6 int RutColor();
Yances64 1:7d7dabfcd7ae 7 void MoverMotor (uint8_t motor, uint8_t grados);
Yances64 1:7d7dabfcd7ae 8 void MoverConjunto(uint8_t motores, uint8_t posicion);
Yances64 1:7d7dabfcd7ae 9
Yances64 1:7d7dabfcd7ae 10 DigitalIn entrada (PE_15);
Yances64 1:7d7dabfcd7ae 11 DigitalOut S3 (PB_0);
Yances64 1:7d7dabfcd7ae 12 DigitalOut S2 (PE_0);
Yances64 1:7d7dabfcd7ae 13
Yances64 1:7d7dabfcd7ae 14 PwmOut Servo1(PB_11);
Yances64 1:7d7dabfcd7ae 15 PwmOut Servo2(PB_10);
Yances64 1:7d7dabfcd7ae 16 PwmOut Servo3(PA_0);
Yances64 1:7d7dabfcd7ae 17 PwmOut Servo4(PE_14);
Yances64 1:7d7dabfcd7ae 18 PwmOut Servo5(PE_12);
Yances64 1:7d7dabfcd7ae 19 PwmOut Servo6(PE_10);
Yances64 1:7d7dabfcd7ae 20 PwmOut Servo7(PD_12);
Yances64 1:7d7dabfcd7ae 21 PwmOut Servo8(PD_13);
Yances64 1:7d7dabfcd7ae 22
Yances64 1:7d7dabfcd7ae 23
Yances64 1:7d7dabfcd7ae 24 Serial Transmi(USBTX, USBRX);
Yances64 1:7d7dabfcd7ae 25 Timer tiempo;
Yances64 0:9df42b937ac1 26
Yances64 1:7d7dabfcd7ae 27 int inicio;
Yances64 1:7d7dabfcd7ae 28 char funcion;
Yances64 1:7d7dabfcd7ae 29 uint8_t motor_es;
Yances64 1:7d7dabfcd7ae 30 uint8_t grados_posicion;
Yances64 1:7d7dabfcd7ae 31 int cierre;
Yances64 1:7d7dabfcd7ae 32 float AnchoPulso;
Yances64 1:7d7dabfcd7ae 33 int valor;
Yances64 1:7d7dabfcd7ae 34 int color=0;
Yances64 1:7d7dabfcd7ae 35 int rojo=0;
Yances64 1:7d7dabfcd7ae 36 int azul=0;
Yances64 1:7d7dabfcd7ae 37 int verde=0;
Yances64 1:7d7dabfcd7ae 38 int Detectar();
Yances64 1:7d7dabfcd7ae 39 int tme=0;
Yances64 1:7d7dabfcd7ae 40
Yances64 1:7d7dabfcd7ae 41
Yances64 1:7d7dabfcd7ae 42
Yances64 1:7d7dabfcd7ae 43 int main() {
Yances64 1:7d7dabfcd7ae 44
Yances64 1:7d7dabfcd7ae 45 while (1){
Yances64 1:7d7dabfcd7ae 46
Yances64 1:7d7dabfcd7ae 47 Conf_Servos();
Yances64 1:7d7dabfcd7ae 48 ConfTransmi();
Yances64 1:7d7dabfcd7ae 49
Yances64 1:7d7dabfcd7ae 50 Transmi.printf("\n Inicio. \n");
Yances64 0:9df42b937ac1 51
Yances64 1:7d7dabfcd7ae 52 inicio=Transmi.getc();
Yances64 1:7d7dabfcd7ae 53 if (inicio==0xFF)
Yances64 1:7d7dabfcd7ae 54 {
Yances64 1:7d7dabfcd7ae 55 funcion=Transmi.getc();
Yances64 1:7d7dabfcd7ae 56 motor_es=Transmi.getc();
Yances64 1:7d7dabfcd7ae 57 grados_posicion=Transmi.getc();
Yances64 1:7d7dabfcd7ae 58 cierre=Transmi.getc();
Yances64 1:7d7dabfcd7ae 59
Yances64 1:7d7dabfcd7ae 60 if (cierre==0xFD){
Yances64 1:7d7dabfcd7ae 61
Yances64 1:7d7dabfcd7ae 62 switch (funcion){
Yances64 1:7d7dabfcd7ae 63 case 0x01:{
Yances64 1:7d7dabfcd7ae 64
Yances64 1:7d7dabfcd7ae 65 MoverMotor(motor_es,grados_posicion);
Yances64 1:7d7dabfcd7ae 66 break;
Yances64 1:7d7dabfcd7ae 67 }
Yances64 1:7d7dabfcd7ae 68 case 0x02:{
Yances64 1:7d7dabfcd7ae 69 MoverConjunto(motor_es,grados_posicion);
Yances64 1:7d7dabfcd7ae 70 break;
Yances64 1:7d7dabfcd7ae 71 }
Yances64 1:7d7dabfcd7ae 72 case 0x03:{
Yances64 1:7d7dabfcd7ae 73 while (funcion==0x03){
Yances64 1:7d7dabfcd7ae 74 RutColor();
Yances64 1:7d7dabfcd7ae 75 }
Yances64 1:7d7dabfcd7ae 76 }
Yances64 0:9df42b937ac1 77
Yances64 1:7d7dabfcd7ae 78 default:{
Yances64 1:7d7dabfcd7ae 79 Transmi.printf("Funcion no definida. \n");
Yances64 1:7d7dabfcd7ae 80 }
Yances64 1:7d7dabfcd7ae 81 }
Yances64 1:7d7dabfcd7ae 82 }
Yances64 1:7d7dabfcd7ae 83
Yances64 1:7d7dabfcd7ae 84 else {
Yances64 1:7d7dabfcd7ae 85 Transmi.printf("Error de codigo de cierre \n");
Yances64 1:7d7dabfcd7ae 86 }
Yances64 1:7d7dabfcd7ae 87 }
Yances64 1:7d7dabfcd7ae 88
Yances64 1:7d7dabfcd7ae 89 else {
Yances64 1:7d7dabfcd7ae 90 Transmi.printf("Error de codigo de inicio. \n");
Yances64 1:7d7dabfcd7ae 91 }
Yances64 1:7d7dabfcd7ae 92 }
Yances64 1:7d7dabfcd7ae 93
Yances64 1:7d7dabfcd7ae 94 }
Yances64 1:7d7dabfcd7ae 95
Yances64 1:7d7dabfcd7ae 96
Yances64 1:7d7dabfcd7ae 97 void MoverMotor(uint8_t motor, uint8_t grados){
Yances64 1:7d7dabfcd7ae 98
Yances64 1:7d7dabfcd7ae 99 Transmi.printf("Se movera solo un motor. \n");
Yances64 1:7d7dabfcd7ae 100 if (grados <= 180){
Yances64 1:7d7dabfcd7ae 101
Yances64 1:7d7dabfcd7ae 102 AnchoPulso=((grados*9.8)+535);
Yances64 1:7d7dabfcd7ae 103
Yances64 1:7d7dabfcd7ae 104
Yances64 1:7d7dabfcd7ae 105 switch (motor){
Yances64 1:7d7dabfcd7ae 106
Yances64 1:7d7dabfcd7ae 107 case 0x01:{
Yances64 1:7d7dabfcd7ae 108 Servo1.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 109 Transmi.printf("Se mueve el motor 1.\n");
Yances64 1:7d7dabfcd7ae 110 break;
Yances64 1:7d7dabfcd7ae 111
Yances64 1:7d7dabfcd7ae 112 }
Yances64 1:7d7dabfcd7ae 113
Yances64 1:7d7dabfcd7ae 114 case 0x02:{
Yances64 1:7d7dabfcd7ae 115 Servo2.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 116 Transmi.printf("Se mueve el motor 2.\n");
Yances64 1:7d7dabfcd7ae 117 break;
Yances64 1:7d7dabfcd7ae 118 }
Yances64 1:7d7dabfcd7ae 119
Yances64 1:7d7dabfcd7ae 120 case 0x03:{
Yances64 1:7d7dabfcd7ae 121 Servo3.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 122 Transmi.printf("Se mueve el motor 3.\n");
Yances64 1:7d7dabfcd7ae 123 break;
Yances64 1:7d7dabfcd7ae 124 }
Yances64 1:7d7dabfcd7ae 125
Yances64 1:7d7dabfcd7ae 126 case 0x04:{
Yances64 1:7d7dabfcd7ae 127 Servo4.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 128 Transmi.printf("Se mueve el motor 4.\n");
Yances64 1:7d7dabfcd7ae 129 break;
Yances64 1:7d7dabfcd7ae 130 }
Yances64 1:7d7dabfcd7ae 131
Yances64 1:7d7dabfcd7ae 132 case 0x05:{
Yances64 1:7d7dabfcd7ae 133 Servo5.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 134 Transmi.printf("Se mueve el motor 5.\n");
Yances64 1:7d7dabfcd7ae 135 break;
Yances64 1:7d7dabfcd7ae 136 }
Yances64 1:7d7dabfcd7ae 137 case 0x06:{
Yances64 1:7d7dabfcd7ae 138 Servo6.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 139 Transmi.printf("Se mueve el motor 6.\n");
Yances64 1:7d7dabfcd7ae 140 break;
Yances64 1:7d7dabfcd7ae 141 }
Yances64 1:7d7dabfcd7ae 142
Yances64 1:7d7dabfcd7ae 143 case 0x07:{
Yances64 1:7d7dabfcd7ae 144 Servo7.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 145 Transmi.printf("Se mueve el motor 7.\n");
Yances64 1:7d7dabfcd7ae 146 break;
Yances64 1:7d7dabfcd7ae 147 }
Yances64 1:7d7dabfcd7ae 148
Yances64 1:7d7dabfcd7ae 149 case 0x08:{
Yances64 1:7d7dabfcd7ae 150 Servo8.pulsewidth_us(AnchoPulso);
Yances64 1:7d7dabfcd7ae 151 Transmi.printf("Se mueve el motor 8.\n");
Yances64 1:7d7dabfcd7ae 152 break;
Yances64 1:7d7dabfcd7ae 153 }
Yances64 1:7d7dabfcd7ae 154
Yances64 1:7d7dabfcd7ae 155 default:{
Yances64 1:7d7dabfcd7ae 156 Transmi.printf("Motor no cargado. \n");
Yances64 1:7d7dabfcd7ae 157 }
Yances64 1:7d7dabfcd7ae 158 }
Yances64 1:7d7dabfcd7ae 159
Yances64 1:7d7dabfcd7ae 160 }
Yances64 1:7d7dabfcd7ae 161
Yances64 1:7d7dabfcd7ae 162 else {
Yances64 1:7d7dabfcd7ae 163 Transmi.printf("Valor de grados fuera de rango. \n");
Yances64 1:7d7dabfcd7ae 164 }
Yances64 1:7d7dabfcd7ae 165
Yances64 1:7d7dabfcd7ae 166 }
Yances64 1:7d7dabfcd7ae 167
Yances64 1:7d7dabfcd7ae 168
Yances64 1:7d7dabfcd7ae 169 void MoverConjunto(uint8_t motores,uint8_t posicion){
Yances64 1:7d7dabfcd7ae 170 Transmi.printf("Se movera un conjunto de motores. \n");
Yances64 1:7d7dabfcd7ae 171
Yances64 1:7d7dabfcd7ae 172 if (posicion==0xDD){
Yances64 0:9df42b937ac1 173
Yances64 1:7d7dabfcd7ae 174 switch(motores){
Yances64 1:7d7dabfcd7ae 175 case 1:{
Yances64 1:7d7dabfcd7ae 176 Servo1.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 177 Servo2.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 178 Transmi.printf("Mover primer conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 179 break;
Yances64 1:7d7dabfcd7ae 180 }
Yances64 1:7d7dabfcd7ae 181
Yances64 1:7d7dabfcd7ae 182 case 2:{
Yances64 1:7d7dabfcd7ae 183 Servo3.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 184 Servo4.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 185 Transmi.printf("Mover segundo conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 186 break;
Yances64 1:7d7dabfcd7ae 187 }
Yances64 0:9df42b937ac1 188
Yances64 1:7d7dabfcd7ae 189 case 3:{
Yances64 1:7d7dabfcd7ae 190 Servo5.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 191 Servo6.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 192 Transmi.printf("Mover tercer conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 193 break;
Yances64 1:7d7dabfcd7ae 194 }
Yances64 0:9df42b937ac1 195
Yances64 1:7d7dabfcd7ae 196 case 4:{
Yances64 1:7d7dabfcd7ae 197 Servo7.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 198 Servo8.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 199 Transmi.printf("Mover cuarto conjunto, ambos cerrados. \n");
Yances64 1:7d7dabfcd7ae 200 break;
Yances64 1:7d7dabfcd7ae 201 }
Yances64 1:7d7dabfcd7ae 202
Yances64 1:7d7dabfcd7ae 203 default: {
Yances64 1:7d7dabfcd7ae 204 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 205 }
Yances64 1:7d7dabfcd7ae 206 }
Yances64 1:7d7dabfcd7ae 207 }
Yances64 1:7d7dabfcd7ae 208
Yances64 1:7d7dabfcd7ae 209 else{
Yances64 1:7d7dabfcd7ae 210 if (posicion==0xDA){
Yances64 1:7d7dabfcd7ae 211
Yances64 1:7d7dabfcd7ae 212 switch(motores){
Yances64 1:7d7dabfcd7ae 213 case 1:{
Yances64 1:7d7dabfcd7ae 214 Servo1.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 215 Servo2.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 216 Transmi.printf("Mover primer conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 217 break;
Yances64 1:7d7dabfcd7ae 218 }
Yances64 1:7d7dabfcd7ae 219
Yances64 1:7d7dabfcd7ae 220 case 2:{
Yances64 1:7d7dabfcd7ae 221 Servo3.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 222 Servo4.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 223 Transmi.printf("Mover segundo conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 224 break;
Yances64 1:7d7dabfcd7ae 225 }
Yances64 1:7d7dabfcd7ae 226
Yances64 1:7d7dabfcd7ae 227 case 3:{
Yances64 1:7d7dabfcd7ae 228 Servo5.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 229 Servo6.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 230 Transmi.printf("Mover tercer conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 231 break;
Yances64 1:7d7dabfcd7ae 232 }
Yances64 0:9df42b937ac1 233
Yances64 1:7d7dabfcd7ae 234 case 4:{
Yances64 1:7d7dabfcd7ae 235 Servo7.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 236 Servo8.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 237 Transmi.printf("Mover cuarto conjunto, uno abierto, dos cerrado. \n");
Yances64 1:7d7dabfcd7ae 238 break;
Yances64 1:7d7dabfcd7ae 239 }
Yances64 1:7d7dabfcd7ae 240
Yances64 1:7d7dabfcd7ae 241 default: {
Yances64 1:7d7dabfcd7ae 242 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 243 }
Yances64 1:7d7dabfcd7ae 244 }
Yances64 1:7d7dabfcd7ae 245 }
Yances64 1:7d7dabfcd7ae 246
Yances64 1:7d7dabfcd7ae 247
Yances64 1:7d7dabfcd7ae 248 else{
Yances64 1:7d7dabfcd7ae 249
Yances64 1:7d7dabfcd7ae 250 if (posicion==0x1D){
Yances64 1:7d7dabfcd7ae 251
Yances64 1:7d7dabfcd7ae 252 switch(motores){
Yances64 1:7d7dabfcd7ae 253 case 1:{
Yances64 1:7d7dabfcd7ae 254 Servo1.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 255 Servo2.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 256 Transmi.printf("Mover primer conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 257 break;
Yances64 1:7d7dabfcd7ae 258 }
Yances64 1:7d7dabfcd7ae 259
Yances64 1:7d7dabfcd7ae 260 case 2:{
Yances64 1:7d7dabfcd7ae 261 Servo3.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 262 Servo4.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 263 Transmi.printf("Mover segundo conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 264 break;
Yances64 1:7d7dabfcd7ae 265 }
Yances64 1:7d7dabfcd7ae 266
Yances64 1:7d7dabfcd7ae 267 case 3:{
Yances64 1:7d7dabfcd7ae 268 Servo5.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 269 Servo6.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 270 Transmi.printf("Mover tercer conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 271 break;
Yances64 1:7d7dabfcd7ae 272 }
Yances64 1:7d7dabfcd7ae 273
Yances64 1:7d7dabfcd7ae 274 case 4:{
Yances64 1:7d7dabfcd7ae 275 Servo7.pulsewidth_us(550);
Yances64 1:7d7dabfcd7ae 276 Servo8.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 277 Transmi.printf("Mover cuarto conjunto, uno cerrado, dos abierto. \n");
Yances64 1:7d7dabfcd7ae 278 break;
Yances64 1:7d7dabfcd7ae 279 }
Yances64 0:9df42b937ac1 280
Yances64 1:7d7dabfcd7ae 281 default: {
Yances64 1:7d7dabfcd7ae 282 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 283 }
Yances64 1:7d7dabfcd7ae 284 }
Yances64 1:7d7dabfcd7ae 285 }
Yances64 1:7d7dabfcd7ae 286 else{
Yances64 1:7d7dabfcd7ae 287 if (posicion==0x1A){
Yances64 1:7d7dabfcd7ae 288
Yances64 1:7d7dabfcd7ae 289 switch(motores){
Yances64 1:7d7dabfcd7ae 290 case 1:{
Yances64 1:7d7dabfcd7ae 291 Servo1.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 292 Servo2.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 293 Transmi.printf("Mover primer conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 294 break;
Yances64 1:7d7dabfcd7ae 295 }
Yances64 1:7d7dabfcd7ae 296
Yances64 1:7d7dabfcd7ae 297 case 2:{
Yances64 1:7d7dabfcd7ae 298 Servo3.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 299 Servo4.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 300 Transmi.printf("Mover segundo conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 301 break;
Yances64 1:7d7dabfcd7ae 302 }
Yances64 1:7d7dabfcd7ae 303
Yances64 1:7d7dabfcd7ae 304 case 3:{
Yances64 1:7d7dabfcd7ae 305 Servo5.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 306 Servo6.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 307 Transmi.printf("Mover tercer conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 308 break;
Yances64 1:7d7dabfcd7ae 309 }
Yances64 1:7d7dabfcd7ae 310
Yances64 1:7d7dabfcd7ae 311 case 4:{
Yances64 1:7d7dabfcd7ae 312 Servo7.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 313 Servo8.pulsewidth_us(2100);
Yances64 1:7d7dabfcd7ae 314 Transmi.printf("Mover tercer conjunto, ambos abiertos. \n");
Yances64 1:7d7dabfcd7ae 315 break;
Yances64 1:7d7dabfcd7ae 316 }
Yances64 0:9df42b937ac1 317
Yances64 1:7d7dabfcd7ae 318 default: {
Yances64 1:7d7dabfcd7ae 319 Transmi.printf("Motores no definidos. \n");
Yances64 1:7d7dabfcd7ae 320 }
Yances64 1:7d7dabfcd7ae 321 }
Yances64 1:7d7dabfcd7ae 322 }
Yances64 1:7d7dabfcd7ae 323
Yances64 1:7d7dabfcd7ae 324 else{
Yances64 1:7d7dabfcd7ae 325 Transmi.printf("Movimiento no definido. \n");
Yances64 1:7d7dabfcd7ae 326
Yances64 1:7d7dabfcd7ae 327 }
Yances64 0:9df42b937ac1 328 }
Yances64 1:7d7dabfcd7ae 329 }
Yances64 1:7d7dabfcd7ae 330 }
Yances64 1:7d7dabfcd7ae 331
Yances64 1:7d7dabfcd7ae 332
Yances64 1:7d7dabfcd7ae 333 }
Yances64 1:7d7dabfcd7ae 334
Yances64 1:7d7dabfcd7ae 335
Yances64 1:7d7dabfcd7ae 336
Yances64 1:7d7dabfcd7ae 337 int Conf_Servos(){
Yances64 1:7d7dabfcd7ae 338 Servo1.period_ms(20);
Yances64 1:7d7dabfcd7ae 339 Servo2.period_ms(20);
Yances64 1:7d7dabfcd7ae 340 Servo3.period_ms(20);
Yances64 1:7d7dabfcd7ae 341 Servo4.period_ms(20);
Yances64 1:7d7dabfcd7ae 342 Servo5.period_ms(20);
Yances64 1:7d7dabfcd7ae 343 Servo6.period_ms(20);
Yances64 1:7d7dabfcd7ae 344 Servo7.period_ms(20);
Yances64 1:7d7dabfcd7ae 345 Servo8.period_ms(20);
Yances64 1:7d7dabfcd7ae 346 return (0);
Yances64 1:7d7dabfcd7ae 347 }
Yances64 1:7d7dabfcd7ae 348
Yances64 1:7d7dabfcd7ae 349
Yances64 1:7d7dabfcd7ae 350 int ConfTransmi(){
Yances64 1:7d7dabfcd7ae 351 Transmi.baud(9600);
Yances64 1:7d7dabfcd7ae 352 return(0);
Yances64 1:7d7dabfcd7ae 353 }
Yances64 1:7d7dabfcd7ae 354
Yances64 1:7d7dabfcd7ae 355 int RutColor(){
Yances64 1:7d7dabfcd7ae 356
Yances64 1:7d7dabfcd7ae 357 S2=0;
Yances64 1:7d7dabfcd7ae 358 S3=0;
Yances64 1:7d7dabfcd7ae 359 rojo=Detectar();
Yances64 1:7d7dabfcd7ae 360
Yances64 1:7d7dabfcd7ae 361 S2=0;
Yances64 1:7d7dabfcd7ae 362 S3=1;
Yances64 1:7d7dabfcd7ae 363 azul=Detectar();
Yances64 1:7d7dabfcd7ae 364
Yances64 1:7d7dabfcd7ae 365 S2=1;
Yances64 1:7d7dabfcd7ae 366 S3=1;
Yances64 1:7d7dabfcd7ae 367 verde=Detectar();
Yances64 1:7d7dabfcd7ae 368
Yances64 1:7d7dabfcd7ae 369 if (rojo<azul & azul<verde){
Yances64 1:7d7dabfcd7ae 370 color=rojo;
Yances64 1:7d7dabfcd7ae 371 RutRojo();
Yances64 1:7d7dabfcd7ae 372 }
Yances64 1:7d7dabfcd7ae 373
Yances64 1:7d7dabfcd7ae 374 else{
Yances64 1:7d7dabfcd7ae 375 if (azul<rojo & rojo<verde){
Yances64 1:7d7dabfcd7ae 376 color=azul;
Yances64 1:7d7dabfcd7ae 377 RutAzul();
Yances64 1:7d7dabfcd7ae 378 }
Yances64 1:7d7dabfcd7ae 379
Yances64 1:7d7dabfcd7ae 380 else{
Yances64 1:7d7dabfcd7ae 381 RutVerde();
Yances64 1:7d7dabfcd7ae 382 color=verde;
Yances64 1:7d7dabfcd7ae 383 }
Yances64 1:7d7dabfcd7ae 384 }
Yances64 1:7d7dabfcd7ae 385 }
Yances64 1:7d7dabfcd7ae 386
Yances64 1:7d7dabfcd7ae 387 int Detectar(){
Yances64 1:7d7dabfcd7ae 388 tme=0;
Yances64 1:7d7dabfcd7ae 389 while (!entrada){}
Yances64 1:7d7dabfcd7ae 390 while (entrada) {}
Yances64 1:7d7dabfcd7ae 391 while (!entrada){}
Yances64 1:7d7dabfcd7ae 392 tiempo.start();
Yances64 1:7d7dabfcd7ae 393 while (entrada) {}
Yances64 1:7d7dabfcd7ae 394 while (!entrada){}
Yances64 1:7d7dabfcd7ae 395 while (entrada) {}
Yances64 1:7d7dabfcd7ae 396 while (!entrada){}
Yances64 1:7d7dabfcd7ae 397 while (entrada) {}
Yances64 1:7d7dabfcd7ae 398 tiempo.stop();
Yances64 1:7d7dabfcd7ae 399 tme=tiempo.read_us();
Yances64 1:7d7dabfcd7ae 400 tiempo.reset();
Yances64 1:7d7dabfcd7ae 401 return(tme);
Yances64 1:7d7dabfcd7ae 402
Yances64 0:9df42b937ac1 403
Yances64 0:9df42b937ac1 404 }
Yances64 1:7d7dabfcd7ae 405
Yances64 1:7d7dabfcd7ae 406
Yances64 1:7d7dabfcd7ae 407 int RutRojo(){
Yances64 1:7d7dabfcd7ae 408 MoverConjunto(1,0x1A);
Yances64 1:7d7dabfcd7ae 409 MoverConjunto(3,0x1A);
Yances64 1:7d7dabfcd7ae 410 wait (2);
Yances64 1:7d7dabfcd7ae 411
Yances64 1:7d7dabfcd7ae 412 MoverConjunto(2,0xDD);
Yances64 1:7d7dabfcd7ae 413 MoverConjunto(4,0xDD);
Yances64 1:7d7dabfcd7ae 414 wait (2);
Yances64 1:7d7dabfcd7ae 415
Yances64 1:7d7dabfcd7ae 416 MoverConjunto(1,0xDD);
Yances64 1:7d7dabfcd7ae 417 MoverConjunto(3,0xDD);
Yances64 1:7d7dabfcd7ae 418 wait (2);
Yances64 1:7d7dabfcd7ae 419
Yances64 1:7d7dabfcd7ae 420 MoverConjunto(2,0x1A);
Yances64 1:7d7dabfcd7ae 421 MoverConjunto(4,0x1A);
Yances64 1:7d7dabfcd7ae 422 wait (2);
Yances64 1:7d7dabfcd7ae 423 }
Yances64 1:7d7dabfcd7ae 424
Yances64 1:7d7dabfcd7ae 425 int RutAzul(){
Yances64 1:7d7dabfcd7ae 426
Yances64 1:7d7dabfcd7ae 427 MoverConjunto(1,0x1A);
Yances64 1:7d7dabfcd7ae 428 MoverConjunto(3,0x1A);
Yances64 1:7d7dabfcd7ae 429 wait (2);
Yances64 1:7d7dabfcd7ae 430
Yances64 1:7d7dabfcd7ae 431 MoverConjunto(2,0x1A);
Yances64 1:7d7dabfcd7ae 432 MoverConjunto(4,0x1A);
Yances64 1:7d7dabfcd7ae 433 wait (2);
Yances64 1:7d7dabfcd7ae 434
Yances64 1:7d7dabfcd7ae 435 MoverConjunto(1,0xDD);
Yances64 1:7d7dabfcd7ae 436 MoverConjunto(3,0xDD);
Yances64 1:7d7dabfcd7ae 437 wait (2);
Yances64 1:7d7dabfcd7ae 438
Yances64 1:7d7dabfcd7ae 439 MoverConjunto(2,0xDD);
Yances64 1:7d7dabfcd7ae 440 MoverConjunto(4,0xDD);
Yances64 1:7d7dabfcd7ae 441 wait (2);
Yances64 1:7d7dabfcd7ae 442 }
Yances64 1:7d7dabfcd7ae 443
Yances64 1:7d7dabfcd7ae 444 int RutVerde(){
Yances64 1:7d7dabfcd7ae 445 MoverConjunto(1,0x1A);
Yances64 1:7d7dabfcd7ae 446 MoverConjunto(3,0xDD);
Yances64 1:7d7dabfcd7ae 447 wait (2);
Yances64 1:7d7dabfcd7ae 448
Yances64 1:7d7dabfcd7ae 449 MoverConjunto(2,0xDD);
Yances64 1:7d7dabfcd7ae 450 MoverConjunto(4,0x1A);
Yances64 1:7d7dabfcd7ae 451 wait (2);
Yances64 1:7d7dabfcd7ae 452
Yances64 1:7d7dabfcd7ae 453 MoverConjunto(1,0x1A);
Yances64 1:7d7dabfcd7ae 454 MoverConjunto(3,0xDD);
Yances64 1:7d7dabfcd7ae 455 wait (2);
Yances64 1:7d7dabfcd7ae 456
Yances64 1:7d7dabfcd7ae 457 MoverConjunto(2,0xDD);
Yances64 1:7d7dabfcd7ae 458 MoverConjunto(4,0x1A);
Yances64 1:7d7dabfcd7ae 459 wait (2);
Yances64 1:7d7dabfcd7ae 460 }