Libreria para manejo de shield controlador de motores AdafuitMotorShield para Arduino
Embed:
(wiki syntax)
Show/hide line numbers
ArduMotor.cpp
00001 #include "ArduMotor.h" 00002 #include "mbed.h" 00003 00004 //Valores para calibrar giro y avanze 00005 #define tiempo_g 300.0 00006 #define tiempo_a 57000.0 00007 00008 //Los siguientes arreglos, todos de 8 posiciones, contienen las palabras con las que el registro de desplazamiento configura el sentido de giro de los motores. 00009 int dato_p[8] = {0,0,0,0,0,0,0,0}; 00010 00011 //Arreglos para moviemientos con 4 motores. 00012 int a1a2a3a4[8] = {0,0,1,1,0,1,0,1}; 00013 int r1r2r3r4[8] = {1,1,0,0,1,0,1,0}; 00014 00015 00016 //Arreglos para movimientos con 2 motores. 00017 //7,6,5,4,3,2,1,0 00018 int a2a4[8] = {0,0,0,1,0,0,0,1}; //para motores 2 y 4. 00019 int r2a4[8] = {0,0,0,0,0,0,1,1}; //para motores 2 y 4. 00020 int r2r4[8] = {0,1,0,0,0,0,1,0}; //para motores 2 y 4. 00021 int a2r4[8] = {0,1,0,1,0,0,0,0}; //para motores 2 y 4. 00022 00023 int a2a3[8] = {0,0,1,1,0,0,0,0}; //para motores 2 y 3. 00024 int r2a3[8] = {0,0,1,0,0,0,1,0}; //para motores 2 y 3. 00025 int r2r3[8] = {1,0,0,0,0,0,1,0}; //para motores 2 y 3. 00026 int a2r3[8] = {1,0,0,1,0,0,0,0}; //para motores 2 y 3. 00027 00028 int a3a4[8] = {0,0,1,0,0,0,0,1}; //para motores 3 y 4. 00029 int r3a4[8] = {1,0,0,0,0,0,0,1}; //para motores 3 y 4. 00030 int r3r4[8] = {1,1,0,0,0,0,0,0}; //para motores 3 y 4. 00031 int a3r4[8] = {0,1,1,0,0,0,0,0}; //para motores 3 y 4. 00032 00033 int a1a2[8] = {0,0,0,1,0,1,0,0}; //para motores 1 y 2. 00034 int r1a2[8] = {0,0,0,1,1,0,0,0}; //para motores 1 y 2. 00035 int r1r2[8] = {0,0,0,0,1,0,1,0}; //para motores 1 y 2. 00036 int a1r2[8] = {0,0,0,0,0,1,1,0}; //para motores 1 y 2. 00037 00038 int a1a3[8] = {0,0,1,0,0,1,0,0}; //para motores 1 y 3. 00039 int r1a3[8] = {0,0,1,0,1,0,0,0}; //para motores 1 y 3. 00040 int r1r3[8] = {1,0,0,0,1,0,0,0}; //para motores 1 y 3. 00041 int a1r3[8] = {1,0,0,0,0,1,0,0}; //para motores 1 y 3. 00042 00043 int a1a4[8] = {0,0,0,0,0,1,0,1}; //para motores 1 y 4. 00044 int r1a4[8] = {0,0,0,0,1,0,0,1}; //para motores 1 y 4. 00045 int r1r4[8] = {0,1,0,0,1,0,0,0}; //para motores 1 y 4. 00046 int a1r4[8] = {0,1,0,0,0,1,0,0}; //para motores 1 y 4. 00047 00048 //Arreglos para movimientos con 1 motor, (giro derecha, giro izquierda). 00049 int a1[8] = {0,0,0,0,0,1,0,0}; 00050 int r1[8] = {0,0,0,0,1,0,0,0}; 00051 00052 int a2[8] = {0,0,0,1,0,0,0,0}; 00053 int r2[8] = {0,0,0,0,0,0,1,0}; 00054 00055 int a3[8] = {0,0,1,0,0,0,0,0}; 00056 int r3[8] = {1,0,0,0,0,0,0,0}; 00057 00058 int a4[8] = {0,0,0,0,0,0,0,1}; 00059 int r4[8] = {0,1,0,0,0,0,0,0}; 00060 00061 int dato[8]; 00062 00063 int motor_A, motor_B, par_A, par_B, comb, comb4; //Variables enteras globles usadas por el codigo. 00064 00065 DigitalOut sd(D8); //Pin que se usa para el envío de datos en forma serie al registro de desplazamiento. 00066 DigitalOut oe(D7); //Pin que habilita la salida de datos almacenados en el registro de desplazamiento. 00067 DigitalOut store(D12); //Pin que hace que el registro de desplazamiento almacene los datos en su memoria. 00068 DigitalOut shift(D4); //Pin conectado al registro de desplazamiento, se usa para almacenar el dato enviado en forma serie. 00069 PwmOut m1(PTA6); //Pin de salida pwm que alimenta el motor 1 del shield. 00070 PwmOut m2(D3); //Pin de salida pwm que alimenta el motor 2 del shield. 00071 PwmOut m4(D5); //Pin de salida pwm que alimenta el motor 4 del shield. 00072 PwmOut m3(D6); //Pin de salida pwm que alimenta el motor 3 del shield. 00073 DigitalOut led_g(LED_GREEN); //Led verde de la placa. 00074 DigitalOut led_r(LED_RED); //Led rojo de la placa. 00075 00076 ArduMotor::ArduMotor(int Motor_A, int Motor_B){ //Definición de la clase ArduMotor, esta clase se usa para el manejo de dos motores. 00077 motor_A = Motor_A; //Dentro de esta clase se define el periodo que usan las salidas PWM de cada motor. 00078 motor_B = Motor_B; //En la variable combinación se almacena el resultado de una ecuación que se usa para reconocer que 00079 comb = (motor_A*10)+(motor_B); //motores se escogieron en esta clase. 00080 m1.period_ms(5); 00081 m2.period_ms(5); 00082 m3.period_ms(5); 00083 m4.period_ms(5); 00084 } 00085 00086 ArduMotor_4::ArduMotor_4(int Par_A, int Par_B){ //Definición de la clase ArduMotor_4, esta clase se usa para el manejo de cuatro motores. 00087 par_A = Par_A; //Dentro de esta clase se define el periodo que usan las salidas PWM de cada motor. 00088 par_B = Par_B; //En la variable combinación se almacena el resultado de una ecuación que se usa para reconocer que 00089 comb4 = (Par_A*100)+Par_B; //motores se escogieron en esta clase. 00090 m1.period_ms(5); 00091 m2.period_ms(5); 00092 m3.period_ms(5); 00093 m4.period_ms(5); 00094 } 00095 00096 void ArduMotor::grabar_shield(){ //Función que se encarga de grabar, en el registro de desplazamiento del shield, la configuración para controlar 00097 int i; //el sentido de giro de los motores, los datos se envían en forma serie. Entre cada pulso del pin shift se coloca 00098 shift = 0; //un bit de la palabra de 8 bits que configura el registro de desplazamiento. 00099 sd = 0; //Los leds se usan para conocer el estado del grabado de datos. 00100 store = 0; //El pin store es el que almacena la palabra de 8 bits dentro de la memoria interna del registro de desplazamiento. 00101 led_r = 0; 00102 led_g = 1; 00103 for(i=0; i<8; i++){ 00104 sd = dato[i]; 00105 led_r = !led_r; 00106 shift = !shift; 00107 shift = !shift; 00108 led_r = !led_r; 00109 } 00110 store = 1; 00111 oe = 0; 00112 led_r = 1; 00113 } 00114 00115 void ArduMotor::Avanzar(float Distancia, int Potencia){ //Función Avanzar, de la clase ArduMotor. Esta función siempre tiene 2 motores activos. 00116 led_g = 0; //Encendido del led verde para indicar que esta corriendo esta función 00117 switch(comb){ //En el switch-case de la función se analiza cuales son los motores involucrados en la clase. 00118 case 12: for(int i=0; i<8; i++) //De acuerdo a esto, es la palabra que se pasa al registro de desplazamiento. 00119 dato[i] = a1a2[i]; 00120 m1 = 1; m2 = (Potencia/100.0); break; 00121 case 13: for(int i=0; i<8; i++) 00122 dato[i] = a1a3[i]; 00123 m1 = 1; m3 = (Potencia/100.0); break; 00124 case 14: for(int i=0; i<8; i++) 00125 dato[i] = a1a4[i]; 00126 m1 = 1; m4 = (Potencia/100.0); break; 00127 case 21: for(int i=0; i<8; i++) 00128 dato[i] = a1a2[i]; 00129 m1 = 1; m2 = (Potencia/100.0); break; 00130 case 23: for(int i=0; i<8; i++) 00131 dato[i] = a2a3[i]; 00132 m2 = (Potencia/100.0); m3 = (Potencia/100.0); break; 00133 case 24: for(int i=0; i<8; i++) 00134 dato[i] = a2a4[i]; 00135 m2 = (Potencia/100.0); m4 = (Potencia/100.0); break; 00136 case 31: for(int i=0; i<8; i++) 00137 dato[i] = a1a3[i]; 00138 m1 = 1; m3 = (Potencia/100.0); break; 00139 case 32: for(int i=0; i<8; i++) 00140 dato[i] = a2a3[i]; 00141 m3 = (Potencia/100.0); m2 = (Potencia/100.0); break; 00142 case 34: for(int i=0; i<8; i++) 00143 dato[i] = a3a4[i]; 00144 m3 = (Potencia/100.0); m4 = (Potencia/100.0); break; 00145 case 41: for(int i=0; i<8; i++) 00146 dato[i] = a1a4[i]; 00147 m4 = (Potencia/100.0); m1 = 1; break; 00148 case 42: for(int i=0; i<8; i++) 00149 dato[i] = a2a4[i]; 00150 m4 = (Potencia/100.0); m2 = (Potencia/100.0); break; 00151 case 43: for(int i=0; i<8; i++) 00152 dato[i] = a3a4[i]; 00153 m4 = (Potencia/100.0); m3 = (Potencia/100.0); break; 00154 }//fin switch 00155 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00156 wait_ms(Distancia*(tiempo_a/Potencia)); //Tiempo de espera para que carro recorra la distancia determinada. 00157 }//fin avanzar 00158 00159 void ArduMotor::Retroceder(float Distancia, int Potencia){ //Función que hace retroceder el vehículo, siempre tiene 2 motores activos. 00160 led_g = 0; //Encendido del led verde para indicar que esta corriendo esta función 00161 switch(comb){ //Funciona de igual manera que la funcion Avanzar. 00162 case 12: for(int i=0; i<8; i++) 00163 dato[i] = r1r2[i]; 00164 m1 = 1; m2 = (Potencia/100.0); break; 00165 case 13: for(int i=0; i<8; i++) 00166 dato[i] = r1r3[i]; 00167 m1 = 1; m3 = (Potencia/100.0); break; 00168 case 14: for(int i=0; i<8; i++) 00169 dato[i] = r1r4[i]; 00170 m1 = 1; m4 = (Potencia/100.0); break; 00171 case 21: for(int i=0; i<8; i++) 00172 dato[i] = r1r2[i]; 00173 m1 = 1; m2 = (Potencia/100.0); break; 00174 case 23: for(int i=0; i<8; i++) 00175 dato[i] = r2r3[i]; 00176 m2 = (Potencia/100.0); m3 = (Potencia/100.0); break; 00177 case 24: for(int i=0; i<8; i++) 00178 dato[i] = r2r4[i]; 00179 m2 = (Potencia/100.0); m4 = (Potencia/100.0); break; 00180 case 31: for(int i=0; i<8; i++) 00181 dato[i] = r1r3[i]; 00182 m1 = 1; m3 = (Potencia/100.0); break; 00183 case 32: for(int i=0; i<8; i++) 00184 dato[i] = r2r3[i]; 00185 m3 = (Potencia/100.0); m2 = (Potencia/100.0); break; 00186 case 34: for(int i=0; i<8; i++) 00187 dato[i] = r3r4[i]; 00188 m3 = (Potencia/100.0); m4 = (Potencia/100.0); break; 00189 case 41: for(int i=0; i<8; i++) 00190 dato[i] = r1r4[i]; 00191 m4 = (Potencia/100.0); m1 = 1; break; 00192 case 42: for(int i=0; i<8; i++) 00193 dato[i] = r2r4[i]; 00194 m4 = (Potencia/100.0); m2 = (Potencia/100.0); break; 00195 case 43: for(int i=0; i<8; i++) 00196 dato[i] = r3r4[i]; 00197 m4 = (Potencia/100.0); m3 = (Potencia/100.0); break; 00198 } 00199 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00200 wait_ms(Distancia*(tiempo_a/Potencia)); //Tiempo de espera para que carro recorra la distancia determinada. 00201 } 00202 00203 void ArduMotor::Girar_der(int Grados){ //Función que hace girar el carrito hacia su derecha, esta función siempre tiene un motor activo. 00204 int p=40; //Se detiene un motor, mientras el otro motor gira. 00205 led_g = 0; //Encendido del led verde para indicar que esta corriendo esta función 00206 switch(comb){ //El switch-case de la función envía la palabra según la combinación de motores escogida. 00207 case 12: for(int i=0; i<8; i++) 00208 dato[i] = a1[i]; 00209 m1 = 1; m2 = (p/100.0); break; 00210 case 13: for(int i=0; i<8; i++) 00211 dato[i] = a1[i]; 00212 m1 = 1; m3 = (p/100.0); break; 00213 case 14: for(int i=0; i<8; i++) 00214 dato[i] = a1[i]; 00215 m1 = 1; m4 = (p/100.0); break; 00216 case 21: for(int i=0; i<8; i++) 00217 dato[i] = a2[i]; 00218 m1 = 1; m2 = (p/100.0); break; 00219 case 23: for(int i=0; i<8; i++) 00220 dato[i] = a2[i]; 00221 m2 = (p/100.0); m3 = (p/100.0); break; 00222 case 24: for(int i=0; i<8; i++) 00223 dato[i] = a2[i]; 00224 m2 = (p/100.0); m4 = (p/100.0); break; 00225 case 31: for(int i=0; i<8; i++) 00226 dato[i] = a3[i]; 00227 m1 = 1; m3 = (p/100.0); break; 00228 case 32: for(int i=0; i<8; i++) 00229 dato[i] = a3[i]; 00230 m3 = (p/100.0); m2 = (p/100.0); break; 00231 case 34: for(int i=0; i<8; i++) 00232 dato[i] = a3[i]; 00233 m3 = (p/100.0); m4 = (p/100.0); break; 00234 case 41: for(int i=0; i<8; i++) 00235 dato[i] = a4[i]; 00236 m4 = (p/100.0); m1 = 1; break; 00237 case 42: for(int i=0; i<8; i++) 00238 dato[i] = a4[i]; 00239 m4 = (p/100.0); m2 = (p/100.0); break; 00240 case 43: for(int i=0; i<8; i++) 00241 dato[i] = a4[i]; 00242 m4 = (p/100.0); m3 = (p/100.0); break; 00243 } 00244 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00245 wait_ms(Grados*(tiempo_g/p)); //Tiempo de espera para que carro gire los grados requeridos. 00246 } 00247 00248 void ArduMotor::Girar_izq(int Grados){ //Función que hace girar el carrito hacia su derecha, esta función siempre tiene un motor activo. 00249 int p=40; //Funciona de igual manera que la función Girar_der. 00250 led_g = 0; //Encendido del led verde para indicar que esta corriendo esta función 00251 switch(comb){ 00252 case 12: for(int i=0; i<8; i++) 00253 dato[i] = a2[i]; 00254 m1 = 1; m2 = (p/100.0); break; 00255 case 13: for(int i=0; i<8; i++) 00256 dato[i] = a3[i]; 00257 m1 = 1; m3 = (p/100.0); break; 00258 case 14: for(int i=0; i<8; i++) 00259 dato[i] = a4[i]; 00260 m1 = 1; m4 = (p/100.0); break; 00261 case 21: for(int i=0; i<8; i++) 00262 dato[i] = a1[i]; 00263 m1 = 1; m2 = (p/100.0); break; 00264 case 23: for(int i=0; i<8; i++) 00265 dato[i] = a3[i]; 00266 m2 = (p/100.0); m3 = (p/100.0); break; 00267 case 24: for(int i=0; i<8; i++) 00268 dato[i] = a4[i]; 00269 m2 = (p/100.0); m4 = (p/100.0); break; 00270 case 31: for(int i=0; i<8; i++) 00271 dato[i] = a1[i]; 00272 m1 = 1; m3 = (p/100.0); break; 00273 case 32: for(int i=0; i<8; i++) 00274 dato[i] = a2[i]; 00275 m3 = (p/100.0); m2 = (p/100.0); break; 00276 case 34: for(int i=0; i<8; i++) 00277 dato[i] = a4[i]; 00278 m3 = (p/100.0); m4 = (p/100.0); break; 00279 case 41: for(int i=0; i<8; i++) 00280 dato[i] = a1[i]; 00281 m4 = (p/100.0); m1 = 1; break; 00282 case 42: for(int i=0; i<8; i++) 00283 dato[i] = a2[i]; 00284 m4 = (p/100.0); m2 = (p/100.0); break; 00285 case 43: for(int i=0; i<8; i++) 00286 dato[i] = a3[i]; 00287 m4 = (p/100.0); m3 = (p/100.0); break; 00288 00289 } 00290 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00291 wait_ms(Grados*(tiempo_g/p)); //Tiempo de espera para que carro gire los grados requeridos. 00292 } 00293 00294 void ArduMotor::Detener(float Tiempo){ //Función que se encarga de pasar al registro la palabra para que detenga el funcionamiento de todos los motores. 00295 led_g = 1; //Funciona de igual manera para 2 y 4 motores, se puede escoger cuanto tiempo se esta detenido. 00296 for(int i=0; i<8; i++) //Apagado de led para indicar que el carro esta detnido. 00297 dato[i]=dato_p[i]; 00298 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00299 wait_ms(Tiempo*1000); //Carro detenido durante el tiempo determinado. 00300 } 00301 00302 //------------------------------------------------------------------------------------------------------------------------------------------------------ 00303 00304 void ArduMotor_4::grabar_shield(){ //Función que graba en el registro de desplazamiento la palabra que se desea pasar, es igual a la función 00305 int i; //del mismo nombre de la clase ArduMotor. 00306 shift = 0; 00307 sd = 0; 00308 store = 0; 00309 led_r = 0; 00310 led_g = 1; 00311 for(i=0; i<8; i++){ 00312 sd = dato[i]; 00313 led_r = !led_r; 00314 shift = !shift; 00315 shift = !shift; 00316 led_r = !led_r; 00317 } 00318 store = 1; 00319 oe = 0; 00320 led_r = 1; 00321 } 00322 00323 void ArduMotor_4::Avanzar(float Distancia, int Potencia){ //Función Avanzar, de la clase ArduMotor_4. Función para el avance del carro, usa 4 motores activos. 00324 led_g = 0; //Encendido del led verde para indicar que esta corriendo esta función 00325 switch(comb4){ //Funciona de igual manera que la funcion Avanzar de ArduMotor. 00326 //Debido a que esta función no contempla todas las combinaciones posibles de conecciones de motores, se 00327 case 3421: for(int i=0; i<8; i++) //agrego una rutina que informa cuando se ha ingresado una combinación no contemplada por medio del led rojo. 00328 dato[i] = a1a2a3a4[i]; 00329 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00330 case 1243: for(int i=0; i<8; i++) 00331 dato[i] = a1a2a3a4[i]; 00332 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00333 case 2314: for(int i=0; i<8; i++) 00334 dato[i] = a1a2a3a4[i]; 00335 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00336 case 4132: for(int i=0; i<8; i++) 00337 dato[i] = a1a2a3a4[i]; 00338 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00339 default: led_r = 1; //En caso de ingresar una combinacion incorrecta, el led rojo parpadéa 3 veces. 00340 int t=0; 00341 while(1){ 00342 led_r = 0; 00343 wait_ms(200); 00344 led_r = 1; 00345 wait_ms(200); 00346 t++; 00347 if(t>2){ 00348 wait_ms(1500); 00349 t=0; 00350 } 00351 }; 00352 }//switch 00353 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00354 wait_ms(Distancia*(tiempo_a/Potencia)); //Tiempo de espera para que carro recorra la distancia determinada. 00355 }//fin avanzar 00356 00357 void ArduMotor_4::Retroceder(float Distancia, int Potencia){ //Función para retroceder el carro, involucra 4 motores activos. 00358 led_g = 0; //Encendido del led verde para indicar que esta corriendo esta función 00359 switch(comb4){ //Funciona de igual manera que la función Retroceder de la clase ArduMotor. 00360 //Tambien involucra una rutina que informa en caso de usar una combinacion no contemplada. 00361 case 3421: for(int i=0; i<8; i++) 00362 dato[i] = r1r2r3r4[i]; 00363 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00364 case 1243: for(int i=0; i<8; i++) 00365 dato[i] = r1r2r3r4[i]; 00366 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00367 case 2314: for(int i=0; i<8; i++) 00368 dato[i] = r1r2r3r4[i]; 00369 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00370 case 4132: for(int i=0; i<8; i++) 00371 dato[i] = r1r2r3r4[i]; 00372 m1=(Potencia/100.0); m2=(Potencia/100.0); m3=(Potencia/100.0); m4=(Potencia/100.0); break; 00373 default: led_r = 1; //En caso de ingresar una combinacion incorrecta, el led rojo parpadéa 3 veces. 00374 int t=0; 00375 while(1){ 00376 led_r = 0; 00377 wait_ms(200); 00378 led_r = 1; 00379 wait_ms(200); 00380 t++; 00381 if(t>2){ 00382 wait_ms(1500); 00383 t=0; 00384 } 00385 }; 00386 } 00387 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00388 wait_ms(Distancia*(tiempo_a/Potencia)); //Tiempo de espera para que carro recorra la distancia determinada. 00389 } 00390 00391 void ArduMotor_4::Girar_der(int Grados){ //Función para girar el carro, involucra 2 motores activos y funciona de manera similar a 00392 int p=40; //la función Girar_der de la clase ArduMotor. 00393 led_g = 0; //Tambien informa si se ingreso una combinación no contemplada. 00394 switch(comb4){ 00395 00396 case 3421: for(int i=0; i<8; i++) 00397 dato[i] = a3a4[i]; 00398 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00399 case 1243: for(int i=0; i<8; i++) 00400 dato[i] = a1a2[i]; 00401 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00402 case 2314: for(int i=0; i<8; i++) 00403 dato[i] = a2a3[i]; 00404 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00405 case 4132: for(int i=0; i<8; i++) 00406 dato[i] = a1a4[i]; 00407 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00408 default: led_r = 1; //En caso de ingresar una combinacion incorrecta, el led rojo parpadéa 3 veces. 00409 int t=0; 00410 while(1){ 00411 led_r = 0; 00412 wait_ms(200); 00413 led_r = 1; 00414 wait_ms(200); 00415 t++; 00416 if(t>2){ 00417 wait_ms(1500); 00418 t=0; 00419 } 00420 }; 00421 } 00422 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00423 wait_ms(Grados*(tiempo_g/p)); //Tiempo de espera para que carro gire los grados requeridos. 00424 } 00425 00426 void ArduMotor_4::Girar_izq(int Grados){ //Función para girar el carro, involucra 2 motores activos y funciona de manera similar a 00427 int p=40; //la función Girar_izq de la clase ArduMotor. 00428 led_g = 0; //Tambien informa en caso de ingresar una combinación no contemplada en el switch-case. 00429 switch(comb4){ 00430 00431 case 3421: for(int i=0; i<8; i++) 00432 dato[i] = a1a2[i]; 00433 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00434 case 1243: for(int i=0; i<8; i++) 00435 dato[i] = a3a4[i]; 00436 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00437 case 2314: for(int i=0; i<8; i++) 00438 dato[i] = a1a4[i]; 00439 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00440 case 4132: for(int i=0; i<8; i++) 00441 dato[i] = a2a3[i]; 00442 m1=(p/100.0); m2=(p/100.0); m3=(p/100.0); m4=(p/100.0); break; 00443 default: led_r = 1; //En caso de ingresar una combinacion incorrecta, el led rojo parpadéa 3 veces. 00444 int t=0; 00445 while(1){ 00446 led_r = 0; 00447 wait_ms(200); 00448 led_r = 1; 00449 wait_ms(200); 00450 t++; 00451 if(t>2){ 00452 wait_ms(1500); 00453 t=0; 00454 } 00455 }; 00456 } 00457 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00458 wait_ms(Grados*(tiempo_g/p)); //Tiempo de espera para que carro gire los grados requeridos. 00459 } 00460 00461 void ArduMotor_4::Detener(float Tiempo){ //Función que detiene todos los motores manejados por el shield, se puede escoger cuanto tiempo se esta detenido. 00462 led_g = 1; //Apagado de led para indicar que el carro se detuvo. 00463 for(int i=0; i<8; i++) 00464 dato[i]=dato_p[i]; 00465 grabar_shield(); //Grabado de la palabra en el registro de desplazamiento. 00466 wait_ms(Tiempo*1000); //Carro detenido durante el tiempo determinado. 00467 }
Generated on Sun Aug 14 2022 19:41:43 by
1.7.2