Wilson Guerrero / Mbed 2 deprecated Entrega_Tercer_Corte

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:5650b51c59f7, 2018-11-20 (annotated)

Committer:
Wreguerro
Date:
Tue Nov 20 01:43:26 2018 +0000
Revision:
1:5650b51c59f7
Parent:
0:af114e34a5ae 
Entrega Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wreguerro 0:af114e34a5ae 1 // Universidad ECCI
Wreguerro 0:af114e34a5ae 2 // Sistemas embebidos
Wreguerro 0:af114e34a5ae 3 // Entregable programacion Comando 2 y sensor de color
Wreguerro 0:af114e34a5ae 4
Wreguerro 0:af114e34a5ae 5 #include "mbed.h"
Wreguerro 0:af114e34a5ae 6 #include "Timer.h"
Wreguerro 0:af114e34a5ae 7 #define MAX 20
Wreguerro 0:af114e34a5ae 8
Wreguerro 0:af114e34a5ae 9 // Funciones
Wreguerro 0:af114e34a5ae 10 void init_servo(); // Se crea la funcion iniciarlizar servo, esta funcion tiene como objetivo definirle el periodo al pwm
Wreguerro 0:af114e34a5ae 11 void mov_servo(int ser);
Wreguerro 0:af114e34a5ae 12 void imp_val();
Wreguerro 0:af114e34a5ae 13 void recibir_dat();
Wreguerro 0:af114e34a5ae 14 void comando_1(float gra);
Wreguerro 0:af114e34a5ae 15 void comando_2();
Wreguerro 0:af114e34a5ae 16 void paso();
Wreguerro 0:af114e34a5ae 17 int Medicion_tiempo();
Wreguerro 0:af114e34a5ae 18 void Selector_color();
Wreguerro 0:af114e34a5ae 19 void config_escala();
Wreguerro 0:af114e34a5ae 20 void mediana();
Wreguerro 0:af114e34a5ae 21 void sensor_color();
Wreguerro 0:af114e34a5ae 22 void joystick();
Wreguerro 0:af114e34a5ae 23
Wreguerro 0:af114e34a5ae 24 // Vectores
Wreguerro 0:af114e34a5ae 25 int32_t vbuff[MAX]; // creando vector que recibe la informacion
Wreguerro 0:af114e34a5ae 26 int mbuff[MAX][MAX]; // creando matriz para guardar la informacion validada
Wreguerro 0:af114e34a5ae 27 float vgrados[8]={0,0,0,0,0,0,0,0}; // vector donde se guardan los grados en los cuales se encuetra cada servomotor
Wreguerro 0:af114e34a5ae 28 float vgrados_ant[8]={0,0,0,0,0,0,0,0};
Wreguerro 0:af114e34a5ae 29 int mpasos[2];
Wreguerro 0:af114e34a5ae 30 int mcolor[3][2]={{0,0},{0,1},{1,1}};
Wreguerro 0:af114e34a5ae 31 int mesc_freq[3][2]={{0,1},{1,0},{1,1}};
Wreguerro 0:af114e34a5ae 32 int mediciones[3][9];
Wreguerro 0:af114e34a5ae 33
Wreguerro 0:af114e34a5ae 34
Wreguerro 0:af114e34a5ae 35 // Variables
Wreguerro 0:af114e34a5ae 36 int filas=1,colum=5; // numero de filas y columnas de las matrices
Wreguerro 0:af114e34a5ae 37 int ms = 20;
Wreguerro 0:af114e34a5ae 38 Timer tiempo;
Wreguerro 0:af114e34a5ae 39 int tim=0;
Wreguerro 0:af114e34a5ae 40 int escala;
Wreguerro 0:af114e34a5ae 41
Wreguerro 0:af114e34a5ae 42 // Comunicacion serial
Wreguerro 0:af114e34a5ae 43 Serial emisor(USBTX, USBRX); // se crea la clase serial poder intercambiar informacion entre la tarjeta y el pc
Wreguerro 0:af114e34a5ae 44
Wreguerro 0:af114e34a5ae 45 // Mototores y gpio
Wreguerro 0:af114e34a5ae 46 PwmOut servo1(PA_10);//d2
Wreguerro 0:af114e34a5ae 47 PwmOut servo2(PB_3);//d3 // se define la salida de proposito general a cada servomotr
Wreguerro 0:af114e34a5ae 48 PwmOut servo3(PB_4);//d5
Wreguerro 0:af114e34a5ae 49 PwmOut servo4(PA_9);//d8
Wreguerro 0:af114e34a5ae 50 PwmOut servo5(PC_7);//d9
Wreguerro 0:af114e34a5ae 51 PwmOut servo6(PB_6);//d10
Wreguerro 0:af114e34a5ae 52 PwmOut servo7(PA_7);//d11
Wreguerro 0:af114e34a5ae 53 PwmOut servo8(PA_8);//d12
Wreguerro 0:af114e34a5ae 54
Wreguerro 0:af114e34a5ae 55
Wreguerro 0:af114e34a5ae 56 DigitalOut s0(PB_8);//d14
Wreguerro 0:af114e34a5ae 57 DigitalOut s1(PB_9);//d15
Wreguerro 0:af114e34a5ae 58 DigitalOut s2(PB_10);//d6
Wreguerro 0:af114e34a5ae 59 DigitalOut s3(PA_5);//d13
Wreguerro 0:af114e34a5ae 60 DigitalIn p_out(PB_5);//d4
Wreguerro 0:af114e34a5ae 61
Wreguerro 0:af114e34a5ae 62 AnalogIn JY(A0);
Wreguerro 0:af114e34a5ae 63 AnalogIn JX(A1);
Wreguerro 0:af114e34a5ae 64
Wreguerro 0:af114e34a5ae 65 int main() {
Wreguerro 0:af114e34a5ae 66
Wreguerro 0:af114e34a5ae 67 init_servo();
Wreguerro 0:af114e34a5ae 68 emisor.baud(9600); // sincronicroniza la velocidad de intercambio de datos
Wreguerro 0:af114e34a5ae 69 emisor.printf("start \n"); // imprime en coolterm la señal de inicio de comunicacion
Wreguerro 0:af114e34a5ae 70
Wreguerro 0:af114e34a5ae 71 while(1)
Wreguerro 0:af114e34a5ae 72 {
Wreguerro 0:af114e34a5ae 73 recibir_dat();
Wreguerro 0:af114e34a5ae 74 switch(mbuff[0][1])
Wreguerro 0:af114e34a5ae 75 {
Wreguerro 0:af114e34a5ae 76 case 1:
Wreguerro 0:af114e34a5ae 77 comando_1(mbuff[0][3]);
Wreguerro 0:af114e34a5ae 78 break;
Wreguerro 0:af114e34a5ae 79 case 2:
Wreguerro 0:af114e34a5ae 80 comando_2();
Wreguerro 0:af114e34a5ae 81 break;
Wreguerro 0:af114e34a5ae 82 case 3:
Wreguerro 0:af114e34a5ae 83 paso();
Wreguerro 0:af114e34a5ae 84 break;
Wreguerro 0:af114e34a5ae 85 case 4:
Wreguerro 0:af114e34a5ae 86 sensor_color();
Wreguerro 0:af114e34a5ae 87 break;
Wreguerro 0:af114e34a5ae 88 case 5:
Wreguerro 0:af114e34a5ae 89 joystick();
Wreguerro 0:af114e34a5ae 90 break;
Wreguerro 0:af114e34a5ae 91 default: emisor.printf("tipo de comando erroneo");
Wreguerro 0:af114e34a5ae 92 }
Wreguerro 0:af114e34a5ae 93 }
Wreguerro 0:af114e34a5ae 94 }
Wreguerro 0:af114e34a5ae 95
Wreguerro 0:af114e34a5ae 96 void init_servo() // se define la duracion del periodo del pulso a cada servomotor
Wreguerro 0:af114e34a5ae 97 {
Wreguerro 0:af114e34a5ae 98 servo1.period_ms(ms);
Wreguerro 0:af114e34a5ae 99 servo2.period_ms(ms);
Wreguerro 0:af114e34a5ae 100 servo3.period_ms(ms);
Wreguerro 0:af114e34a5ae 101 servo4.period_ms(ms);
Wreguerro 0:af114e34a5ae 102 servo5.period_ms(ms);
Wreguerro 0:af114e34a5ae 103 servo6.period_ms(ms);
Wreguerro 0:af114e34a5ae 104 servo7.period_ms(ms);
Wreguerro 0:af114e34a5ae 105 servo8.period_ms(ms);
Wreguerro 0:af114e34a5ae 106 }
Wreguerro 0:af114e34a5ae 107
Wreguerro 0:af114e34a5ae 108 uint8_t ss_time=50; // tiempo de espera para moverse 1 mm en microsegundos
Wreguerro 0:af114e34a5ae 109
Wreguerro 0:af114e34a5ae 110 void put_sstime(uint8_t vtime)
Wreguerro 0:af114e34a5ae 111 {
Wreguerro 0:af114e34a5ae 112 ss_time=vtime;}
Wreguerro 0:af114e34a5ae 113
Wreguerro 0:af114e34a5ae 114 void mov_servo(int ser)
Wreguerro 0:af114e34a5ae 115 {
Wreguerro 0:af114e34a5ae 116 switch(ser) // con esta funcion se le asgina a cada servomotor el valor
Wreguerro 0:af114e34a5ae 117 {
Wreguerro 0:af114e34a5ae 118 // de pulsos para que se mueva al angulo correspondiente
Wreguerro 0:af114e34a5ae 119 case 1:
Wreguerro 0:af114e34a5ae 120 servo1.pulsewidth_us(vgrados[0]);
Wreguerro 0:af114e34a5ae 121 break;
Wreguerro 0:af114e34a5ae 122 case 2:
Wreguerro 0:af114e34a5ae 123 servo2.pulsewidth_us(vgrados[1]);
Wreguerro 0:af114e34a5ae 124 break;
Wreguerro 0:af114e34a5ae 125 case 3:
Wreguerro 0:af114e34a5ae 126 servo3.pulsewidth_us(vgrados[2]);
Wreguerro 0:af114e34a5ae 127 break;
Wreguerro 0:af114e34a5ae 128 case 4:
Wreguerro 0:af114e34a5ae 129 servo4.pulsewidth_us(487+vgrados[3]);
Wreguerro 0:af114e34a5ae 130 break;
Wreguerro 0:af114e34a5ae 131 case 5:
Wreguerro 0:af114e34a5ae 132 servo5.pulsewidth_us(vgrados[4]);
Wreguerro 0:af114e34a5ae 133 break;
Wreguerro 0:af114e34a5ae 134 case 6:
Wreguerro 0:af114e34a5ae 135 servo6.pulsewidth_us(3250 - (487+vgrados[5]));
Wreguerro 0:af114e34a5ae 136 break;
Wreguerro 0:af114e34a5ae 137 case 7:
Wreguerro 0:af114e34a5ae 138 servo7.pulsewidth_us(vgrados[6]);
Wreguerro 0:af114e34a5ae 139 break;
Wreguerro 0:af114e34a5ae 140 case 8:
Wreguerro 0:af114e34a5ae 141 servo8.pulsewidth_us(1625+(1625-vgrados[7]));
Wreguerro 0:af114e34a5ae 142 break;
Wreguerro 0:af114e34a5ae 143 }
Wreguerro 0:af114e34a5ae 144 }
Wreguerro 0:af114e34a5ae 145
Wreguerro 0:af114e34a5ae 146 void imp_val()
Wreguerro 0:af114e34a5ae 147 {
Wreguerro 0:af114e34a5ae 148 // imprimir informacion
Wreguerro 0:af114e34a5ae 149 for(int x = 0; x < filas; x++) // En esta parte del codigo se imprime la informacion que
Wreguerro 0:af114e34a5ae 150 {for(int y = 0; y < colum; y++) // recibio el programa, con esto el usuario puede confirmar
Wreguerro 0:af114e34a5ae 151 { // que el programa esta trabajando con los datos correctos
Wreguerro 0:af114e34a5ae 152 emisor.printf("%x",mbuff[x][y]); // se imprimen en formato Hexadecimal
Wreguerro 0:af114e34a5ae 153 }
Wreguerro 0:af114e34a5ae 154 emisor.printf("\n");
Wreguerro 0:af114e34a5ae 155 }
Wreguerro 0:af114e34a5ae 156
Wreguerro 0:af114e34a5ae 157 for(int w = 0; w < 8; w++) // Se imprime el vector de pulsos asignado para cada servo
Wreguerro 0:af114e34a5ae 158 {emisor.printf(" %.1f",vgrados[w]);} // este datos se entrega en numeros decimales
Wreguerro 0:af114e34a5ae 159 emisor.printf("\n");
Wreguerro 0:af114e34a5ae 160 emisor.printf("%d \n",mbuff[0][3]); // se imprime en decimal el valor de los grados ingresados
Wreguerro 0:af114e34a5ae 161
Wreguerro 0:af114e34a5ae 162 }
Wreguerro 0:af114e34a5ae 163
Wreguerro 0:af114e34a5ae 164 void recibir_dat()
Wreguerro 0:af114e34a5ae 165 {
Wreguerro 0:af114e34a5ae 166 //diligenciar vector
Wreguerro 0:af114e34a5ae 167 for(int a = 0;a < colum;a++) // Este vector recibe y valida los datos recibidos
Wreguerro 0:af114e34a5ae 168 { // las validaciones que hace son si el telecomando inicia con el comando
Wreguerro 0:af114e34a5ae 169 vbuff[a]=emisor.getc(); // correcto(ff), el numero de servomotor se encuentra entre 1 a
Wreguerro 0:af114e34a5ae 170 if(vbuff[0]!=0xff) // 8 y si los grados se encuebtran entre 0 y 180, si no cumple estos parametros
Wreguerro 0:af114e34a5ae 171 {a--;}
Wreguerro 0:af114e34a5ae 172 // descarta la informacion
Wreguerro 0:af114e34a5ae 173 if(vbuff[4]!=0xfd)
Wreguerro 0:af114e34a5ae 174 {if(vbuff[4]!=0x00)
Wreguerro 0:af114e34a5ae 175 {emisor.printf("Corregir comando final servo %x\n",vbuff[2]);
Wreguerro 0:af114e34a5ae 176 vbuff[4]=0;
Wreguerro 0:af114e34a5ae 177 a = a - 5;}}
Wreguerro 0:af114e34a5ae 178
Wreguerro 0:af114e34a5ae 179 if(vbuff[2]>0x08)
Wreguerro 0:af114e34a5ae 180 { emisor.printf("Corregir el numero del servomotor %x\n",vbuff[2]);
Wreguerro 0:af114e34a5ae 181 vbuff[2]=0;
Wreguerro 0:af114e34a5ae 182 a = a - 3;}
Wreguerro 0:af114e34a5ae 183
Wreguerro 0:af114e34a5ae 184 if(vbuff[2]<0x00)
Wreguerro 0:af114e34a5ae 185 { emisor.printf("Corregir el numero del servomotor %x\n",vbuff[2]);
Wreguerro 0:af114e34a5ae 186 vbuff[2]=0;
Wreguerro 0:af114e34a5ae 187 a = a - 3;}
Wreguerro 0:af114e34a5ae 188
Wreguerro 0:af114e34a5ae 189 if(vbuff[3]>0xb4)
Wreguerro 0:af114e34a5ae 190 { emisor.printf("Corregir los grados asignados al servomotor %x\n",vbuff[2]);
Wreguerro 0:af114e34a5ae 191 vbuff[3]=0;
Wreguerro 0:af114e34a5ae 192 a = a - 4;}
Wreguerro 0:af114e34a5ae 193
Wreguerro 0:af114e34a5ae 194 if(vbuff[3]<0x00)
Wreguerro 0:af114e34a5ae 195 { emisor.printf("Corregir los grados asignados al servomotor %x\n",vbuff[2]);
Wreguerro 0:af114e34a5ae 196 vbuff[3]=0;
Wreguerro 0:af114e34a5ae 197 a = a - 4;}
Wreguerro 0:af114e34a5ae 198 }
Wreguerro 0:af114e34a5ae 199
Wreguerro 0:af114e34a5ae 200 //pasar vector a matriz
Wreguerro 0:af114e34a5ae 201 for(int i = 0;i<filas;i++) // Esta matriz se encarga de almacenar los datos despues de que se realizaron
Wreguerro 0:af114e34a5ae 202 {for(int j =0; j<colum;j++) // las validaciones, con esto aseguramos que le entregamos datos correctos para
Wreguerro 0:af114e34a5ae 203 { // el que el programa pueda trabajar sin inconvenientes
Wreguerro 0:af114e34a5ae 204 mbuff[i][j]=vbuff[j];
Wreguerro 0:af114e34a5ae 205 }
Wreguerro 0:af114e34a5ae 206 }
Wreguerro 0:af114e34a5ae 207
Wreguerro 0:af114e34a5ae 208 }
Wreguerro 0:af114e34a5ae 209
Wreguerro 0:af114e34a5ae 210 void comando_1(float gra)
Wreguerro 0:af114e34a5ae 211 {
Wreguerro 0:af114e34a5ae 212 vgrados[(mbuff[0][2])- 1]= (((2600-650)*gra)/180)+650; // el vector convierte los grados (hexadecimales) a pulsos
Wreguerro 0:af114e34a5ae 213 imp_val();
Wreguerro 0:af114e34a5ae 214 mov_servo(mbuff[0][2]);
Wreguerro 0:af114e34a5ae 215
Wreguerro 0:af114e34a5ae 216 } // y se le asigna a la casilla del servomotor correspondiente
Wreguerro 0:af114e34a5ae 217 // en hexadecimal
Wreguerro 0:af114e34a5ae 218 void comando_2()
Wreguerro 0:af114e34a5ae 219 {
Wreguerro 0:af114e34a5ae 220 int mbrazos[4][2]={{0,1},{2,3},{4,5},{6,7}};
Wreguerro 0:af114e34a5ae 221 float mmov[4][2]={{0x1e,0x5a},{0x46,0x5a},{0x1e,0x5a},{0x46,0x2d}};
Wreguerro 0:af114e34a5ae 222 int vnew[2];
Wreguerro 0:af114e34a5ae 223 int vnew2[2];
Wreguerro 0:af114e34a5ae 224 int q=mbuff[0][3];
Wreguerro 0:af114e34a5ae 225 if (q>0x02)
Wreguerro 0:af114e34a5ae 226 {
Wreguerro 0:af114e34a5ae 227 vnew[0]=mbrazos[mbuff[0][2] - 1][1];
Wreguerro 0:af114e34a5ae 228 vnew[1]=mmov[mbuff[0][3] - 1][1];
Wreguerro 0:af114e34a5ae 229 }
Wreguerro 0:af114e34a5ae 230 vnew2[0]=mbrazos[mbuff[0][2] - 1][0];
Wreguerro 0:af114e34a5ae 231 vnew2[1]=mmov[mbuff[0][3] - 1][0];
Wreguerro 0:af114e34a5ae 232
Wreguerro 0:af114e34a5ae 233 if (q>0x02)
Wreguerro 0:af114e34a5ae 234 {
Wreguerro 0:af114e34a5ae 235 mbuff[0][1]=0x01;
Wreguerro 0:af114e34a5ae 236 mbuff[0][2]=vnew[0]+1;
Wreguerro 0:af114e34a5ae 237 mbuff[0][3]=vnew[1];
Wreguerro 0:af114e34a5ae 238 comando_1(mbuff[0][3]);
Wreguerro 0:af114e34a5ae 239 }
Wreguerro 0:af114e34a5ae 240
Wreguerro 0:af114e34a5ae 241 mbuff[0][2]=vnew2[0]+1;
Wreguerro 0:af114e34a5ae 242 mbuff[0][3]=vnew2[1];
Wreguerro 0:af114e34a5ae 243 comando_1(mbuff[0][3]);
Wreguerro 0:af114e34a5ae 244 }
Wreguerro 0:af114e34a5ae 245
Wreguerro 0:af114e34a5ae 246 void paso()
Wreguerro 0:af114e34a5ae 247 {
Wreguerro 0:af114e34a5ae 248
Wreguerro 0:af114e34a5ae 249 mpasos[0]= mbuff[0][2];
Wreguerro 0:af114e34a5ae 250 mpasos[1]= mbuff[0][3];
Wreguerro 0:af114e34a5ae 251 int cant=0;
Wreguerro 0:af114e34a5ae 252 emisor.printf("pasos %d ",mpasos[1]);
Wreguerro 1:5650b51c59f7 253 switch(mpasos[0])
Wreguerro 0:af114e34a5ae 254 {
Wreguerro 1:5650b51c59f7 255 case 1:
Wreguerro 1:5650b51c59f7 256
Wreguerro 1:5650b51c59f7 257 while(cant<mpasos[1])
Wreguerro 1:5650b51c59f7 258 {
Wreguerro 1:5650b51c59f7 259 for (int aa=0;aa<4;aa++)
Wreguerro 1:5650b51c59f7 260 {
Wreguerro 1:5650b51c59f7 261 mbuff[0][1]=2;
Wreguerro 1:5650b51c59f7 262 mbuff[0][2]=aa+1;
Wreguerro 1:5650b51c59f7 263 mbuff[0][3]=3;
Wreguerro 1:5650b51c59f7 264 comando_2();
Wreguerro 1:5650b51c59f7 265 //wait_ms(200);
Wreguerro 1:5650b51c59f7 266 mbuff[0][1]=2;
Wreguerro 1:5650b51c59f7 267 mbuff[0][2]=aa+1;
Wreguerro 1:5650b51c59f7 268 mbuff[0][3]=2;
Wreguerro 1:5650b51c59f7 269 comando_2();
Wreguerro 1:5650b51c59f7 270 //wait_ms(200);
Wreguerro 1:5650b51c59f7 271 }
Wreguerro 1:5650b51c59f7 272 //wait_ms(100);
Wreguerro 1:5650b51c59f7 273 for (int bb=0;bb<4;bb++)
Wreguerro 1:5650b51c59f7 274 {
Wreguerro 1:5650b51c59f7 275 mbuff[0][1]=2;
Wreguerro 1:5650b51c59f7 276 mbuff[0][2]=bb+1;
Wreguerro 1:5650b51c59f7 277 mbuff[0][3]=4;
Wreguerro 1:5650b51c59f7 278 comando_2();
Wreguerro 1:5650b51c59f7 279 }
Wreguerro 1:5650b51c59f7 280 //wait_ms(200);
Wreguerro 1:5650b51c59f7 281 cant++;
Wreguerro 1:5650b51c59f7 282 }
Wreguerro 1:5650b51c59f7 283 break;
Wreguerro 1:5650b51c59f7 284 case 2:
Wreguerro 1:5650b51c59f7 285
Wreguerro 1:5650b51c59f7 286 while(cant<mpasos[1])
Wreguerro 1:5650b51c59f7 287 {
Wreguerro 1:5650b51c59f7 288 for (int zz=0;zz<4;zz++)
Wreguerro 1:5650b51c59f7 289 {
Wreguerro 1:5650b51c59f7 290 mbuff[0][1]=2;
Wreguerro 1:5650b51c59f7 291 mbuff[0][2]=zz+1;
Wreguerro 1:5650b51c59f7 292 mbuff[0][3]=4;
Wreguerro 1:5650b51c59f7 293 comando_2();
Wreguerro 1:5650b51c59f7 294 //wait_ms(200);
Wreguerro 1:5650b51c59f7 295 mbuff[0][1]=2;
Wreguerro 1:5650b51c59f7 296 mbuff[0][2]=zz+1;
Wreguerro 1:5650b51c59f7 297 mbuff[0][3]=2;
Wreguerro 1:5650b51c59f7 298 comando_2();
Wreguerro 1:5650b51c59f7 299 //wait_ms(200);
Wreguerro 1:5650b51c59f7 300 }
Wreguerro 1:5650b51c59f7 301 //wait_ms(100);
Wreguerro 1:5650b51c59f7 302 for (int vv=0;vv<4;vv++)
Wreguerro 1:5650b51c59f7 303 {
Wreguerro 1:5650b51c59f7 304 mbuff[0][1]=2;
Wreguerro 1:5650b51c59f7 305 mbuff[0][2]=vv+1;
Wreguerro 1:5650b51c59f7 306 mbuff[0][3]=3;
Wreguerro 1:5650b51c59f7 307 comando_2();
Wreguerro 1:5650b51c59f7 308 }
Wreguerro 1:5650b51c59f7 309 //wait_ms(200);
Wreguerro 1:5650b51c59f7 310 cant++;
Wreguerro 1:5650b51c59f7 311 }
Wreguerro 1:5650b51c59f7 312 break;
Wreguerro 0:af114e34a5ae 313 }
Wreguerro 0:af114e34a5ae 314 }
Wreguerro 0:af114e34a5ae 315
Wreguerro 0:af114e34a5ae 316 void sensor_color()
Wreguerro 0:af114e34a5ae 317 {
Wreguerro 0:af114e34a5ae 318 emisor.printf("ingrese el tipo de escala de frecuencia \n");
Wreguerro 0:af114e34a5ae 319 escala=emisor.getc();
Wreguerro 0:af114e34a5ae 320 config_escala();
Wreguerro 0:af114e34a5ae 321 Selector_color();
Wreguerro 0:af114e34a5ae 322
Wreguerro 0:af114e34a5ae 323 for(int e=0;e<3;e++)
Wreguerro 0:af114e34a5ae 324 {
Wreguerro 0:af114e34a5ae 325 float a = mediciones[e][5];
Wreguerro 0:af114e34a5ae 326 emisor.printf(" %.1f",a);
Wreguerro 0:af114e34a5ae 327 }
Wreguerro 0:af114e34a5ae 328 emisor.printf("\n");
Wreguerro 0:af114e34a5ae 329 if(mediciones[0][5]>mediciones[1][5])
Wreguerro 0:af114e34a5ae 330 {
Wreguerro 0:af114e34a5ae 331 if (mediciones[0][5]>mediciones[2][5])
Wreguerro 0:af114e34a5ae 332 {
Wreguerro 0:af114e34a5ae 333 emisor.printf("azul TRES PASOS \n");
Wreguerro 0:af114e34a5ae 334 wait(1);
Wreguerro 0:af114e34a5ae 335
Wreguerro 0:af114e34a5ae 336 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 337 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 338 mbuff[0][3]=3;
Wreguerro 0:af114e34a5ae 339 paso();
Wreguerro 0:af114e34a5ae 340 }
Wreguerro 0:af114e34a5ae 341 }
Wreguerro 0:af114e34a5ae 342 if (mediciones[2][5]>mediciones[0][5])
Wreguerro 0:af114e34a5ae 343 {
Wreguerro 0:af114e34a5ae 344 if (mediciones[2][5]>mediciones[1][5])
Wreguerro 0:af114e34a5ae 345 {
Wreguerro 0:af114e34a5ae 346 emisor.printf("rojo DOS PASOS \n");
Wreguerro 0:af114e34a5ae 347 wait(1);
Wreguerro 0:af114e34a5ae 348
Wreguerro 0:af114e34a5ae 349 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 350 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 351 mbuff[0][3]=2;
Wreguerro 0:af114e34a5ae 352 paso(); }
Wreguerro 0:af114e34a5ae 353 }
Wreguerro 0:af114e34a5ae 354 if (mediciones[1][5]>mediciones[0][5])
Wreguerro 0:af114e34a5ae 355 {
Wreguerro 0:af114e34a5ae 356 if (mediciones[1][5]>mediciones[2][5])
Wreguerro 0:af114e34a5ae 357 {
Wreguerro 0:af114e34a5ae 358 emisor.printf("verde UN PASO \n");
Wreguerro 0:af114e34a5ae 359 wait(1);
Wreguerro 0:af114e34a5ae 360
Wreguerro 0:af114e34a5ae 361 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 362 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 363 mbuff[0][3]=1;
Wreguerro 0:af114e34a5ae 364 paso(); }
Wreguerro 0:af114e34a5ae 365 }
Wreguerro 0:af114e34a5ae 366
Wreguerro 0:af114e34a5ae 367 }
Wreguerro 0:af114e34a5ae 368 int Medicion_tiempo()
Wreguerro 0:af114e34a5ae 369 {
Wreguerro 0:af114e34a5ae 370 while(p_out){}
Wreguerro 0:af114e34a5ae 371 tiempo.start();
Wreguerro 0:af114e34a5ae 372 while(!p_out){}
Wreguerro 0:af114e34a5ae 373 tiempo.stop();
Wreguerro 0:af114e34a5ae 374 tim=tiempo.read_us();
Wreguerro 0:af114e34a5ae 375 tiempo.reset();
Wreguerro 0:af114e34a5ae 376
Wreguerro 0:af114e34a5ae 377 return tim;
Wreguerro 0:af114e34a5ae 378 }
Wreguerro 0:af114e34a5ae 379
Wreguerro 0:af114e34a5ae 380 void Selector_color()
Wreguerro 0:af114e34a5ae 381 {
Wreguerro 0:af114e34a5ae 382 for(int j =0;j<9;j++)
Wreguerro 0:af114e34a5ae 383 {
Wreguerro 0:af114e34a5ae 384 for(int i =0;i<3;i++)
Wreguerro 0:af114e34a5ae 385 {
Wreguerro 0:af114e34a5ae 386 s2.write(mcolor[i][0]);
Wreguerro 0:af114e34a5ae 387 s3.write(mcolor[i][1]);
Wreguerro 0:af114e34a5ae 388
Wreguerro 0:af114e34a5ae 389 switch(i)
Wreguerro 0:af114e34a5ae 390 {
Wreguerro 0:af114e34a5ae 391 case 0:
Wreguerro 0:af114e34a5ae 392 mediciones[i][j]= Medicion_tiempo();
Wreguerro 0:af114e34a5ae 393 break;
Wreguerro 0:af114e34a5ae 394 case 1:
Wreguerro 0:af114e34a5ae 395 mediciones[i][j]= Medicion_tiempo();
Wreguerro 0:af114e34a5ae 396 break;
Wreguerro 0:af114e34a5ae 397 case 2:
Wreguerro 0:af114e34a5ae 398 mediciones[i][j]= Medicion_tiempo();
Wreguerro 0:af114e34a5ae 399 break;
Wreguerro 0:af114e34a5ae 400 }
Wreguerro 0:af114e34a5ae 401 }
Wreguerro 0:af114e34a5ae 402 }
Wreguerro 0:af114e34a5ae 403
Wreguerro 0:af114e34a5ae 404 mediana();
Wreguerro 0:af114e34a5ae 405 }
Wreguerro 0:af114e34a5ae 406
Wreguerro 0:af114e34a5ae 407 void config_escala()
Wreguerro 0:af114e34a5ae 408 {
Wreguerro 0:af114e34a5ae 409 s0.write(mesc_freq[escala - 1][0]);
Wreguerro 0:af114e34a5ae 410 s1.write(mesc_freq[escala - 1][1]);
Wreguerro 0:af114e34a5ae 411 }
Wreguerro 0:af114e34a5ae 412
Wreguerro 0:af114e34a5ae 413 void mediana()
Wreguerro 0:af114e34a5ae 414 {
Wreguerro 0:af114e34a5ae 415 int aux;
Wreguerro 0:af114e34a5ae 416
Wreguerro 0:af114e34a5ae 417 for(int w=0;w<3;w++)
Wreguerro 0:af114e34a5ae 418 {
Wreguerro 0:af114e34a5ae 419 for(int u=0;u<9;u++)
Wreguerro 0:af114e34a5ae 420 {
Wreguerro 0:af114e34a5ae 421 for(int o=0;o<9;o++)
Wreguerro 0:af114e34a5ae 422 {
Wreguerro 0:af114e34a5ae 423 if (mediciones[w][o]>mediciones[w][o + 1])
Wreguerro 0:af114e34a5ae 424 {
Wreguerro 0:af114e34a5ae 425 aux=mediciones[w][o];
Wreguerro 0:af114e34a5ae 426 mediciones[w][o]=mediciones[w][o + 1];
Wreguerro 0:af114e34a5ae 427 mediciones[w][o + 1]=aux;
Wreguerro 0:af114e34a5ae 428 }
Wreguerro 0:af114e34a5ae 429 }
Wreguerro 0:af114e34a5ae 430 }
Wreguerro 0:af114e34a5ae 431 }
Wreguerro 0:af114e34a5ae 432 }
Wreguerro 0:af114e34a5ae 433 void joystick()
Wreguerro 0:af114e34a5ae 434 {
Wreguerro 0:af114e34a5ae 435 while(JY.read()*1000<600)
Wreguerro 0:af114e34a5ae 436 {while(JY.read()*1000>300)
Wreguerro 0:af114e34a5ae 437 { if(JX.read()*1000>600)
Wreguerro 0:af114e34a5ae 438 {
Wreguerro 0:af114e34a5ae 439 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 440 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 441 mbuff[0][3]=1;
Wreguerro 0:af114e34a5ae 442 paso();
Wreguerro 0:af114e34a5ae 443 }
Wreguerro 1:5650b51c59f7 444 else if (JX.read()*1000<400)
Wreguerro 1:5650b51c59f7 445 {emisor.printf("atras\n");
Wreguerro 1:5650b51c59f7 446 mbuff[0][1]=3;
Wreguerro 1:5650b51c59f7 447 mbuff[0][2]=2;
Wreguerro 1:5650b51c59f7 448 mbuff[0][3]=1;
Wreguerro 1:5650b51c59f7 449 paso();
Wreguerro 1:5650b51c59f7 450 }
Wreguerro 0:af114e34a5ae 451 else
Wreguerro 0:af114e34a5ae 452 {emisor.printf("Quieta\n");}
Wreguerro 0:af114e34a5ae 453 wait(0.5);
Wreguerro 0:af114e34a5ae 454 }
Wreguerro 0:af114e34a5ae 455 break;
Wreguerro 0:af114e34a5ae 456 }
Wreguerro 0:af114e34a5ae 457 {emisor.printf("Saliendo de mando con Joystick\n");
Wreguerro 0:af114e34a5ae 458 }
Wreguerro 0:af114e34a5ae 459
Wreguerro 0:af114e34a5ae 460 }