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@0:af114e34a5ae, 2018-11-14 (annotated)

Committer:
Wreguerro
Date:
Wed Nov 14 21:49:17 2018 +0000
Revision:
0:af114e34a5ae
Child:
1:5650b51c59f7
Se agrego al programa el control con un Joystick

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 0:af114e34a5ae 253 while(cant<mpasos[1])
Wreguerro 0:af114e34a5ae 254 {
Wreguerro 0:af114e34a5ae 255 for (int aa=0;aa<4;aa++)
Wreguerro 0:af114e34a5ae 256 {
Wreguerro 0:af114e34a5ae 257 mbuff[0][1]=2;
Wreguerro 0:af114e34a5ae 258 mbuff[0][2]=aa+1;
Wreguerro 0:af114e34a5ae 259 mbuff[0][3]=3;
Wreguerro 0:af114e34a5ae 260 comando_2();
Wreguerro 0:af114e34a5ae 261 //wait_ms(200);
Wreguerro 0:af114e34a5ae 262 mbuff[0][1]=2;
Wreguerro 0:af114e34a5ae 263 mbuff[0][2]=aa+1;
Wreguerro 0:af114e34a5ae 264 mbuff[0][3]=2;
Wreguerro 0:af114e34a5ae 265 comando_2();
Wreguerro 0:af114e34a5ae 266 //wait_ms(200);
Wreguerro 0:af114e34a5ae 267 }
Wreguerro 0:af114e34a5ae 268 //wait_ms(100);
Wreguerro 0:af114e34a5ae 269 for (int bb=0;bb<4;bb++)
Wreguerro 0:af114e34a5ae 270 {
Wreguerro 0:af114e34a5ae 271 mbuff[0][1]=2;
Wreguerro 0:af114e34a5ae 272 mbuff[0][2]=bb+1;
Wreguerro 0:af114e34a5ae 273 mbuff[0][3]=4;
Wreguerro 0:af114e34a5ae 274 comando_2();
Wreguerro 0:af114e34a5ae 275 }
Wreguerro 0:af114e34a5ae 276 //wait_ms(200);
Wreguerro 0:af114e34a5ae 277 cant++;
Wreguerro 0:af114e34a5ae 278 }
Wreguerro 0:af114e34a5ae 279
Wreguerro 0:af114e34a5ae 280 }
Wreguerro 0:af114e34a5ae 281
Wreguerro 0:af114e34a5ae 282 void sensor_color()
Wreguerro 0:af114e34a5ae 283 {
Wreguerro 0:af114e34a5ae 284 emisor.printf("ingrese el tipo de escala de frecuencia \n");
Wreguerro 0:af114e34a5ae 285 escala=emisor.getc();
Wreguerro 0:af114e34a5ae 286 config_escala();
Wreguerro 0:af114e34a5ae 287 Selector_color();
Wreguerro 0:af114e34a5ae 288
Wreguerro 0:af114e34a5ae 289 for(int e=0;e<3;e++)
Wreguerro 0:af114e34a5ae 290 {
Wreguerro 0:af114e34a5ae 291 float a = mediciones[e][5];
Wreguerro 0:af114e34a5ae 292 emisor.printf(" %.1f",a);
Wreguerro 0:af114e34a5ae 293 }
Wreguerro 0:af114e34a5ae 294 emisor.printf("\n");
Wreguerro 0:af114e34a5ae 295 if(mediciones[0][5]>mediciones[1][5])
Wreguerro 0:af114e34a5ae 296 {
Wreguerro 0:af114e34a5ae 297 if (mediciones[0][5]>mediciones[2][5])
Wreguerro 0:af114e34a5ae 298 {
Wreguerro 0:af114e34a5ae 299 emisor.printf("azul TRES PASOS \n");
Wreguerro 0:af114e34a5ae 300 wait(1);
Wreguerro 0:af114e34a5ae 301
Wreguerro 0:af114e34a5ae 302 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 303 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 304 mbuff[0][3]=3;
Wreguerro 0:af114e34a5ae 305 paso();
Wreguerro 0:af114e34a5ae 306 }
Wreguerro 0:af114e34a5ae 307 }
Wreguerro 0:af114e34a5ae 308 if (mediciones[2][5]>mediciones[0][5])
Wreguerro 0:af114e34a5ae 309 {
Wreguerro 0:af114e34a5ae 310 if (mediciones[2][5]>mediciones[1][5])
Wreguerro 0:af114e34a5ae 311 {
Wreguerro 0:af114e34a5ae 312 emisor.printf("rojo DOS PASOS \n");
Wreguerro 0:af114e34a5ae 313 wait(1);
Wreguerro 0:af114e34a5ae 314
Wreguerro 0:af114e34a5ae 315 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 316 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 317 mbuff[0][3]=2;
Wreguerro 0:af114e34a5ae 318 paso(); }
Wreguerro 0:af114e34a5ae 319 }
Wreguerro 0:af114e34a5ae 320 if (mediciones[1][5]>mediciones[0][5])
Wreguerro 0:af114e34a5ae 321 {
Wreguerro 0:af114e34a5ae 322 if (mediciones[1][5]>mediciones[2][5])
Wreguerro 0:af114e34a5ae 323 {
Wreguerro 0:af114e34a5ae 324 emisor.printf("verde UN PASO \n");
Wreguerro 0:af114e34a5ae 325 wait(1);
Wreguerro 0:af114e34a5ae 326
Wreguerro 0:af114e34a5ae 327 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 328 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 329 mbuff[0][3]=1;
Wreguerro 0:af114e34a5ae 330 paso(); }
Wreguerro 0:af114e34a5ae 331 }
Wreguerro 0:af114e34a5ae 332
Wreguerro 0:af114e34a5ae 333 }
Wreguerro 0:af114e34a5ae 334 int Medicion_tiempo()
Wreguerro 0:af114e34a5ae 335 {
Wreguerro 0:af114e34a5ae 336 while(p_out){}
Wreguerro 0:af114e34a5ae 337 tiempo.start();
Wreguerro 0:af114e34a5ae 338 while(!p_out){}
Wreguerro 0:af114e34a5ae 339 tiempo.stop();
Wreguerro 0:af114e34a5ae 340 tim=tiempo.read_us();
Wreguerro 0:af114e34a5ae 341 tiempo.reset();
Wreguerro 0:af114e34a5ae 342
Wreguerro 0:af114e34a5ae 343 return tim;
Wreguerro 0:af114e34a5ae 344 }
Wreguerro 0:af114e34a5ae 345
Wreguerro 0:af114e34a5ae 346 void Selector_color()
Wreguerro 0:af114e34a5ae 347 {
Wreguerro 0:af114e34a5ae 348 for(int j =0;j<9;j++)
Wreguerro 0:af114e34a5ae 349 {
Wreguerro 0:af114e34a5ae 350 for(int i =0;i<3;i++)
Wreguerro 0:af114e34a5ae 351 {
Wreguerro 0:af114e34a5ae 352 s2.write(mcolor[i][0]);
Wreguerro 0:af114e34a5ae 353 s3.write(mcolor[i][1]);
Wreguerro 0:af114e34a5ae 354
Wreguerro 0:af114e34a5ae 355 switch(i)
Wreguerro 0:af114e34a5ae 356 {
Wreguerro 0:af114e34a5ae 357 case 0:
Wreguerro 0:af114e34a5ae 358 mediciones[i][j]= Medicion_tiempo();
Wreguerro 0:af114e34a5ae 359 break;
Wreguerro 0:af114e34a5ae 360 case 1:
Wreguerro 0:af114e34a5ae 361 mediciones[i][j]= Medicion_tiempo();
Wreguerro 0:af114e34a5ae 362 break;
Wreguerro 0:af114e34a5ae 363 case 2:
Wreguerro 0:af114e34a5ae 364 mediciones[i][j]= Medicion_tiempo();
Wreguerro 0:af114e34a5ae 365 break;
Wreguerro 0:af114e34a5ae 366 }
Wreguerro 0:af114e34a5ae 367 }
Wreguerro 0:af114e34a5ae 368 }
Wreguerro 0:af114e34a5ae 369
Wreguerro 0:af114e34a5ae 370 mediana();
Wreguerro 0:af114e34a5ae 371 }
Wreguerro 0:af114e34a5ae 372
Wreguerro 0:af114e34a5ae 373 void config_escala()
Wreguerro 0:af114e34a5ae 374 {
Wreguerro 0:af114e34a5ae 375 s0.write(mesc_freq[escala - 1][0]);
Wreguerro 0:af114e34a5ae 376 s1.write(mesc_freq[escala - 1][1]);
Wreguerro 0:af114e34a5ae 377 }
Wreguerro 0:af114e34a5ae 378
Wreguerro 0:af114e34a5ae 379 void mediana()
Wreguerro 0:af114e34a5ae 380 {
Wreguerro 0:af114e34a5ae 381 int aux;
Wreguerro 0:af114e34a5ae 382
Wreguerro 0:af114e34a5ae 383 for(int w=0;w<3;w++)
Wreguerro 0:af114e34a5ae 384 {
Wreguerro 0:af114e34a5ae 385 for(int u=0;u<9;u++)
Wreguerro 0:af114e34a5ae 386 {
Wreguerro 0:af114e34a5ae 387 for(int o=0;o<9;o++)
Wreguerro 0:af114e34a5ae 388 {
Wreguerro 0:af114e34a5ae 389 if (mediciones[w][o]>mediciones[w][o + 1])
Wreguerro 0:af114e34a5ae 390 {
Wreguerro 0:af114e34a5ae 391 aux=mediciones[w][o];
Wreguerro 0:af114e34a5ae 392 mediciones[w][o]=mediciones[w][o + 1];
Wreguerro 0:af114e34a5ae 393 mediciones[w][o + 1]=aux;
Wreguerro 0:af114e34a5ae 394 }
Wreguerro 0:af114e34a5ae 395 }
Wreguerro 0:af114e34a5ae 396 }
Wreguerro 0:af114e34a5ae 397 }
Wreguerro 0:af114e34a5ae 398 }
Wreguerro 0:af114e34a5ae 399 void joystick()
Wreguerro 0:af114e34a5ae 400 {
Wreguerro 0:af114e34a5ae 401 while(JY.read()*1000<600)
Wreguerro 0:af114e34a5ae 402 {while(JY.read()*1000>300)
Wreguerro 0:af114e34a5ae 403 { if(JX.read()*1000>600)
Wreguerro 0:af114e34a5ae 404 {
Wreguerro 0:af114e34a5ae 405 mbuff[0][1]=3;
Wreguerro 0:af114e34a5ae 406 mbuff[0][2]=1;
Wreguerro 0:af114e34a5ae 407 mbuff[0][3]=1;
Wreguerro 0:af114e34a5ae 408 paso();
Wreguerro 0:af114e34a5ae 409 }
Wreguerro 0:af114e34a5ae 410 else
Wreguerro 0:af114e34a5ae 411 {emisor.printf("Quieta\n");}
Wreguerro 0:af114e34a5ae 412 wait(0.5);
Wreguerro 0:af114e34a5ae 413 }
Wreguerro 0:af114e34a5ae 414 break;
Wreguerro 0:af114e34a5ae 415 }
Wreguerro 0:af114e34a5ae 416 {emisor.printf("Saliendo de mando con Joystick\n");
Wreguerro 0:af114e34a5ae 417 }
Wreguerro 0:af114e34a5ae 418
Wreguerro 0:af114e34a5ae 419 }