ENTREGA
Fork of 01-04EntregaPrimerCorte by
Embed:
(wiki syntax)
Show/hide line numbers
mover.cpp
00001 #include "mover.h" 00002 #include "main.h" 00003 #include "mbed.h" 00004 #include "math.h" 00005 00006 //indicamos la variables de salidas de señales PWM para cada uno de los servos. 00007 00008 PwmOut myServo1(PB_10); 00009 PwmOut myServo2(PA_8); 00010 PwmOut myServo3(PA_9); 00011 PwmOut myServo4(PC_7); 00012 PwmOut myServo5(PB_6); 00013 PwmOut myServo6(PA_7); 00014 PwmOut myServo7(PA_6); 00015 PwmOut myServo8(PA_5); 00016 00017 00018 uint8_t ss_time=50; // variable de tiempo de espera para moverse 1 mm en microsegundos 00019 00020 void put_sstime(uint8_t vtime){ 00021 ss_time=vtime; 00022 } 00023 00024 int coord2us(float coord){ // asignacion de formula general para obtener la equivalencia de 0 a 180° y 1 a 2 ms para mover el servo (regla de tres) 00025 if(0 <= coord <= MAXPOS) 00026 return int(750+coord*1900/180);// valor indicado de retorno 00027 return 750; 00028 } 00029 00030 00031 void mover_ser(uint8_t motores, uint8_t posicion){ //se agregan los datos de "nmotor" y "grados" y se almacenan en "motor" y "pos". 00032 00033 int pulseX = coord2us(posicion); //se asigna la vaiable "pulseX" y se le carga el valor obtenido de la formula general obtenida en "coord2us" 00034 switch (motores){ //se realiza un caso switch con la variable asignada para seleccionar qué motor mover 00035 00036 case 0x01: { 00037 myServo1.pulsewidth_us(pulseX); //al motor seleccionado se le asigna y se le indica el ancho de pulso almacenado en la variable "pulseX" para que se mueva a la posición seleccinada 00038 break; 00039 } 00040 00041 case 0x02: { 00042 myServo2.pulsewidth_us(pulseX); 00043 break; 00044 } 00045 00046 case 0x03:{ 00047 myServo3.pulsewidth_us(pulseX); 00048 break; 00049 } 00050 00051 case 0x04:{ 00052 myServo4.pulsewidth_us(pulseX); 00053 break; 00054 } 00055 00056 case 0x05:{ 00057 myServo5.pulsewidth_us(pulseX); 00058 break; 00059 } 00060 00061 case 0x06:{ 00062 myServo6.pulsewidth_us(pulseX); 00063 break; 00064 } 00065 00066 case 0x07:{ 00067 myServo7.pulsewidth_us(pulseX); 00068 break; 00069 } 00070 00071 case 0x08:{ 00072 myServo8.pulsewidth_us(pulseX); 00073 break; 00074 } 00075 00076 default: break; 00077 } 00078 } 00079 00080 00081 void mover_conjunto(uint8_t motores, uint8_t posicion){ 00082 00083 00084 if (posicion==0xDD){//patas adelante 00085 00086 switch(motores){ 00087 case 1:{ 00088 myServo2.pulsewidth_us(2800); 00089 00090 debug_m("Mover pata 1, adelante. \n"); 00091 break; 00092 } 00093 00094 case 2:{ 00095 00096 myServo4.pulsewidth_us(2800); 00097 debug_m("Mover pata 2, adelante. \n"); 00098 break ; 00099 } 00100 00101 case 3:{ 00102 00103 myServo6.pulsewidth_us(550); 00104 debug_m("Mover pata 3, adelante. . \n"); 00105 break; 00106 } 00107 00108 case 4:{ 00109 myServo8.pulsewidth_us(700); 00110 debug_m("Mover pata 4, adelante. \n"); 00111 break; 00112 } 00113 00114 default: { 00115 debug_m("Motores no definidos. \n"); 00116 } 00117 } 00118 } 00119 00120 else{ 00121 if (posicion==0xDA){ 00122 00123 switch(motores){ 00124 case 1:{ 00125 myServo2.pulsewidth_us(550); 00126 debug_m("Mover pata 1, atras. \n"); 00127 break; 00128 } 00129 00130 case 2:{ 00131 myServo4.pulsewidth_us(550); 00132 debug_m("Mover pata 2, atras. \n"); 00133 break; 00134 } 00135 00136 case 3:{ 00137 00138 myServo6.pulsewidth_us(2800); 00139 debug_m("Mover pata 3, atras. \n"); 00140 break; 00141 } 00142 00143 case 4:{ 00144 00145 myServo8.pulsewidth_us(2800); 00146 debug_m("Mover pata 4, atras. \n"); 00147 break; 00148 } 00149 00150 default: { 00151 debug_m("Motores no definidos. \n"); 00152 } 00153 } 00154 } 00155 00156 00157 else{ 00158 00159 if (posicion==0x1D){ 00160 00161 switch(motores){ 00162 case 1:{ 00163 myServo1.pulsewidth_us(550); 00164 00165 debug_m("Mover pata 1, arriba. \n"); 00166 break; 00167 } 00168 00169 case 2:{ 00170 myServo3.pulsewidth_us(550); 00171 00172 debug_m("Mover pata 2, arriba. \n"); 00173 break; 00174 } 00175 00176 case 3:{ 00177 myServo5.pulsewidth_us(550); 00178 00179 debug_m("Mover pata 3, arriba. \n"); 00180 break; 00181 } 00182 00183 case 4:{ 00184 myServo7.pulsewidth_us(550); 00185 00186 debug_m("Mover pata 4, arriba. \n"); 00187 break; 00188 } 00189 00190 default: { 00191 debug_m("Motores no definidos. \n"); 00192 } 00193 } 00194 } 00195 else{ 00196 if (posicion==0x1A){ 00197 00198 switch(motores){ 00199 case 1:{ 00200 myServo1.pulsewidth_us(1500); 00201 00202 debug_m("\nMover pata 1, abajo.\n"); 00203 break; 00204 } 00205 00206 case 2:{ 00207 myServo3.pulsewidth_us(1300); 00208 00209 debug_m("\nMover pata 2, abajo. \n"); 00210 break; 00211 } 00212 00213 case 3:{ 00214 myServo5.pulsewidth_us(1500); 00215 00216 debug_m("Mover pata 3, abajo. \n"); 00217 break; 00218 } 00219 00220 case 4:{ 00221 myServo7.pulsewidth_us(1500); 00222 00223 debug_m("Mover pata 4, abajo. \n"); 00224 break; 00225 } 00226 00227 default: { 00228 debug_m("Motores no definidos. \n"); 00229 } 00230 } 00231 } 00232 00233 else{ 00234 debug_m("Movimiento no definido. \n"); 00235 00236 } 00237 } 00238 } 00239 } 00240 wait_ms (700); 00241 debug_m("\n ******fin mover conjunto de motores.****** \n\n"); 00242 } 00243 void iniciando_(){ 00244 myServo1.pulsewidth_us(1500);//1 00245 wait_ms(100); 00246 myServo3.pulsewidth_us(1300); 00247 wait_ms(100); 00248 myServo5.pulsewidth_us(1500); 00249 wait_ms(100); 00250 myServo7.pulsewidth_us(1500);//1 00251 wait_ms(100); 00252 myServo1.pulsewidth_us(550);//2 00253 wait_ms(100); 00254 myServo2.pulsewidth_us(1150); 00255 wait_ms(500); 00256 myServo2.pulsewidth_us(2200); 00257 wait_ms(500); 00258 myServo2.pulsewidth_us(1700); 00259 wait_ms(100); 00260 myServo1.pulsewidth_us(1500);//2 00261 wait_ms(500); 00262 myServo3.pulsewidth_us(550);//3 00263 wait_ms(100); 00264 myServo4.pulsewidth_us(1150); 00265 wait_ms(500); 00266 myServo4.pulsewidth_us(2200); 00267 wait_ms(500); 00268 myServo4.pulsewidth_us(1700); 00269 wait_ms(100); 00270 myServo3.pulsewidth_us(1300);//3*****************1500 00271 wait_ms(500); 00272 myServo5.pulsewidth_us(550);//4 00273 wait_ms(100); 00274 myServo6.pulsewidth_us(1150); 00275 wait_ms(500); 00276 myServo6.pulsewidth_us(2200); 00277 wait_ms(500); 00278 myServo6.pulsewidth_us(1700); 00279 wait_ms(100); 00280 myServo5.pulsewidth_us(1500);//4 00281 wait_ms(500); 00282 myServo7.pulsewidth_us(550);//5 00283 wait_ms(100); 00284 myServo8.pulsewidth_us(1150); 00285 wait_ms(500); 00286 myServo8.pulsewidth_us(2200); 00287 wait_ms(500); 00288 myServo8.pulsewidth_us(1700); 00289 wait_ms(100); 00290 myServo7.pulsewidth_us(1500);//5 00291 wait_ms(50); 00292 } 00293 void adelante_(){ 00294 myServo1.pulsewidth_us(1500);//1 00295 myServo5.pulsewidth_us(1500); 00296 00297 myServo2.pulsewidth_us(800); 00298 myServo3.pulsewidth_us(550); 00299 myServo4.pulsewidth_us(2800); 00300 wait(0.1); 00301 myServo3.pulsewidth_us(1300); 00302 wait(0.1); 00303 myServo4.pulsewidth_us(1700);/////////////////// 00304 //wait(0.3); 00305 myServo1.pulsewidth_us(550); 00306 myServo2.pulsewidth_us(1700); 00307 wait(0.1); 00308 myServo1.pulsewidth_us(1500); 00309 wait(0.1); //1 00310 00311 myServo7.pulsewidth_us(550); 00312 myServo6.pulsewidth_us(2800); 00313 myServo8.pulsewidth_us(750); 00314 wait(0.1); 00315 myServo7.pulsewidth_us(1500); 00316 wait(0.1); 00317 myServo8.pulsewidth_us(1700); 00318 myServo5.pulsewidth_us(550); 00319 myServo6.pulsewidth_us(1700); 00320 wait(0.1); 00321 myServo5.pulsewidth_us(1500); 00322 00323 /*wait(1); 00324 myServo8.pulsewidth_us(1800); 00325 wait(1); 00326 myServo7.pulsewidth_us(1200); 00327 wait(1);*/ 00328 } 00329 00330 void atras_(){ 00331 00332 myServo1.pulsewidth_us(1500);//1 00333 myServo3.pulsewidth_us(1300); 00334 myServo5.pulsewidth_us(1500); 00335 myServo7.pulsewidth_us(1500); 00336 00337 myServo4.pulsewidth_us(2800); 00338 myServo1.pulsewidth_us(550); 00339 myServo2.pulsewidth_us(800); 00340 wait(0.1); 00341 myServo1.pulsewidth_us(1500); 00342 wait(0.1); 00343 myServo2.pulsewidth_us(1700);/////////////////// 00344 //wait(0.3); 00345 myServo3.pulsewidth_us(550); 00346 myServo4.pulsewidth_us(1700); 00347 wait(0.1); 00348 myServo3.pulsewidth_us(1300); 00349 wait(0.1); //1 00350 00351 myServo5.pulsewidth_us(550); 00352 myServo8.pulsewidth_us(800); 00353 myServo6.pulsewidth_us(2800); 00354 wait(0.1); 00355 myServo5.pulsewidth_us(1500); 00356 wait(0.1); 00357 myServo6.pulsewidth_us(1700); 00358 myServo7.pulsewidth_us(550); 00359 myServo8.pulsewidth_us(1700); 00360 wait(0.1); 00361 myServo7.pulsewidth_us(1500); 00362 00363 } 00364 void derecha_(){ 00365 myServo2.pulsewidth_us(700); 00366 myServo7.pulsewidth_us(550); 00367 myServo8.pulsewidth_us(700); 00368 wait(0.3); 00369 myServo7.pulsewidth_us(1300); 00370 wait(0.3); 00371 myServo8.pulsewidth_us(1300); 00372 myServo1.pulsewidth_us(1300); 00373 myServo7.pulsewidth_us(550); 00374 myServo8.pulsewidth_us(700); 00375 wait(0.3); 00376 myServo7.pulsewidth_us(1300); 00377 myServo8.pulsewidth_us(550); 00378 wait(1); 00379 myServo2.pulsewidth_us(1700); 00380 wait(0.3); 00381 myServo4.pulsewidth_us(1700); 00382 myServo6.pulsewidth_us(1700); 00383 myServo8.pulsewidth_us(1700); 00384 } 00385 void izquierda_(){ 00386 00387 //myServo7.pulsewidth_us(2200); 00388 // myServo8.pulsewidth_us(2500); 00389 } 00390 void mover_frenteanalog(){adelante_();} 00391 void mover_atrasanalog(){atras_();} 00392 void init_servo() //se configura el periodo de los servos 00393 { 00394 myServo1.period_ms(20); 00395 myServo2.period_ms(20); 00396 myServo3.period_ms(20); 00397 myServo4.period_ms(20); 00398 myServo5.period_ms(20); 00399 myServo6.period_ms(20); 00400 myServo7.period_ms(20); 00401 myServo8.period_ms(20); 00402 00403 00404 }
Generated on Thu Aug 18 2022 05:10:52 by
1.7.2
