ENTREGA

Dependencies:   mbed

Fork of 01-04EntregaPrimerCorte by ferney alberto beltran molina

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers mover.cpp Source File

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 }