Avance_Color

Users » sebasmartinez » Code » Avance_Color
sebastian martinez / Mbed 2 deprecated Avance_Color
mover.cpp

Committer:: sebasmartinez
Date:: 2018-11-20
Revision:: 1:59be7d7e1b54
Parent:: 0:73e805ecf765
File content as of revision 1:59be7d7e1b54:

#include "mover.h"
#include "mbed.h"
#include "math.h"
/*
PwmOut Servo1(PB_14); //ROJO
PwmOut Servo2(PB_0); //BLANCO
PwmOut Servo3(PA_11); //VERDE
PwmOut Servo4(PA_8);  //NEGRO
PwmOut Servo5(PB_6);  //NARANJA
PwmOut Servo6(PB_1);  //CAFE
PwmOut Servo7(PA_10); //AZUL
PwmOut Servo8(PA_6);  //AMARILLO
*/
PwmOut Servo1(PA_5); //ROJO
PwmOut Servo2(PA_6); //BLANCO
PwmOut Servo3(PB_2); //VERDE
PwmOut Servo4(PB_6);  //NEGRO
PwmOut Servo5(PC_7);  //NARANJA
PwmOut Servo6(PA_9);  //CAFE
PwmOut Servo7(PA_8); //AZUL
PwmOut Servo8(PB_10);  //AMARILLO

/*
PwmOut Servo6(PB_1);  //CAFE
PwmOut Servo7(PA_7); //AZUL
PwmOut Servo8(PA_6);  //AMARILLO

uint8_t  ss_time=50;     // tiempo  de espera para moverse 1 mm en microsegundos

void put_sstime(uint8_t vtime){
    ss_time=vtime;
    
}
*/
int coord2us(float coord)
{
    if(0 <= coord <= MAXPOS)
       { return int(500+coord*2000/180);// u6
        }
    return 500;
/*
Secuencia 0, 180, 135, 90, 45, 0.
FF 01 01 00 FE FF 01 01 B4 FE FF 01 01 87 FE FF 01 01 5A FE FF 01 01 2D FE FF 01 01 00 FE
Grados: 180-B4, 135-87, 90-5A, 45-2D, 0-00;
*/
}


 


void mover_ser(uint8_t motor, uint8_t pos){

    int pulseX = coord2us(pos);
    //int pulseX = 900;
    //int pulseY = 1500;
   // myServo1.pulsewidth_us(pulseX);
 switch ( motor ) {
  
  case 0x01: 
     Servo1.pulsewidth_us(pulseX);
    break;
  case 0x02: 
     Servo2.pulsewidth_us(pulseX);
    break;    
  case 0x03: 
     Servo3.pulsewidth_us(pulseX);
    break;
  case 0x04: 
     Servo4.pulsewidth_us(pulseX);
    break;
  case 0x05: 
     Servo5.pulsewidth_us(pulseX);
    break;
  case 0x06: 
     Servo6.pulsewidth_us(pulseX);
    break;
  case 0x07: 
     Servo7.pulsewidth_us(pulseX);
    break;   
  case 0x08: 
     Servo8.pulsewidth_us(pulseX);
    break;
            
  default: break;
}

}
void mover_par(uint8_t pares, uint8_t dir){
     
     switch ( pares ) {
  
  case 0x01: 
  if(dir == 0x01){
      Servo1.pulsewidth_us(ARRIBA);
      }else if(dir == 0x02){
          Servo1.pulsewidth_us(ABAJO);
          }else if(dir == 0x03){
              Servo2.pulsewidth_us(ADELANTE);
              }else if(dir == 0x04){
                  Servo2.pulsewidth_us(ATRAS);
                  } else{break;} 
  break;
  case 0x02:
    if(dir == 0x01){
      Servo3.pulsewidth_us(ARRIBA);
      }else if(dir == 0x02){
          Servo3.pulsewidth_us(ABAJO);
          }else if(dir == 0x03){
              Servo4.pulsewidth_us(ADELANTE);
              }else if(dir == 0x04){
                  Servo4.pulsewidth_us(ATRAS);
                  } else{break;} 
   break;
  case 0x03: 
    if(dir == 0x01){
      Servo5.pulsewidth_us(ARRIBA);
      }else if(dir == 0x02){
          Servo5.pulsewidth_us(ABAJO);
          }else if(dir == 0x03){
              Servo6.pulsewidth_us(ADELANTE);
              }else if(dir == 0x04){
                  Servo6.pulsewidth_us(ATRAS);
                  } else{break;} 
  break;
  case 0x04: 
    if(dir == 0x01){
      Servo7.pulsewidth_us(ARRIBA);
      }else if(dir == 0x02){
          Servo7.pulsewidth_us(ABAJO);
          }else if(dir == 0x03){
              Servo8.pulsewidth_us(ADELANTE);
              }else if(dir == 0x04){
                  Servo8.pulsewidth_us(ATRAS);
                  } else{break;} 
  break;

     default: break;
}
    }
void centro(uint8_t pos_p){
switch ( pos_p ) {
  
  case 0x01: 
            Servo1.pulsewidth_us(CENTRO);
            wait(1);
            Servo2.pulsewidth_us(CENTRO);
            wait(1);
    break;
  case 0x02: 
            Servo3.pulsewidth_us(CENTRO);
            wait(1);
            Servo4.pulsewidth_us(CENTRO);
            wait(1);
    break;    
  case 0x03: 
            Servo5.pulsewidth_us(CENTRO);
            wait(1);
            Servo6.pulsewidth_us(CENTRO);
            wait(1);
    break;
  case 0x04: 
            Servo7.pulsewidth_us(CENTRO);
            wait(1);
            Servo8.pulsewidth_us(CENTRO);
            wait(1);
    break;
      default: break;
}
    }    
void direccion(uint8_t lado){
    switch ( lado ) {
  
  case 0x01: //Derecha
            
            Servo2.pulsewidth_us(ADELANTE);
            Servo1.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo1.pulsewidth_us(ABAJO);
            wait_ms(500);
            
            Servo6.pulsewidth_us(ADELANTE);
            Servo5.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo5.pulsewidth_us(ABAJO);
            wait_ms(500);
            
            Servo4.pulsewidth_us(ADELANTE);
            Servo3.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo3.pulsewidth_us(ABAJO);
            wait_ms(500);
            
            Servo8.pulsewidth_us(ADELANTE);
            Servo7.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo7.pulsewidth_us(ABAJO);
            wait_ms(500);

            Servo2.pulsewidth_us(CENTRO);
            Servo4.pulsewidth_us(CENTRO);
            Servo6.pulsewidth_us(CENTRO);
            Servo8.pulsewidth_us(CENTRO);
            wait(1);
    break;
  case 0x02: //Izquierda
            Servo2.pulsewidth_us(ATRAS);
            Servo1.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo1.pulsewidth_us(ABAJO);
            wait_ms(500);
            
            Servo6.pulsewidth_us(ATRAS);
            Servo5.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo5.pulsewidth_us(ABAJO);
            wait_ms(500);
            
            Servo4.pulsewidth_us(ATRAS);
            Servo3.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo3.pulsewidth_us(ABAJO);
            wait_ms(500);
            
            Servo8.pulsewidth_us(ATRAS);
            Servo7.pulsewidth_us(ARRIBA);
            wait_ms(300);
            Servo7.pulsewidth_us(ABAJO);
            wait_ms(500);

            Servo2.pulsewidth_us(CENTRO);
            Servo4.pulsewidth_us(CENTRO);
            Servo6.pulsewidth_us(CENTRO);
            Servo8.pulsewidth_us(CENTRO);
            wait(1);
    break; 
    case 0x03: // Adelante
        Servo1.pulsewidth_us(1500);//arriba
        Servo5.pulsewidth_us(1500);//arriba
        wait_ms(80);
        Servo2.pulsewidth_us(1100);//adelante
        Servo6.pulsewidth_us(900);//adelante
        wait_ms(80);
        Servo1.pulsewidth_us(2000);//abajo
        Servo5.pulsewidth_us(2000);//abajo
        wait_ms(80);
        Servo2.pulsewidth_us(900);//atras
        Servo6.pulsewidth_us(1200);//atras
        wait_ms(80);        
        Servo3.pulsewidth_us(1500);//arriba
        Servo7.pulsewidth_us(1500);//arriba
        wait_ms(80);
        Servo4.pulsewidth_us(2000);//adelante
        Servo8.pulsewidth_us(1900);//adelante
        wait_ms(80);
        Servo3.pulsewidth_us(2000);//abajo
        Servo7.pulsewidth_us(2000);//abajo
        wait_ms(80);
        Servo4.pulsewidth_us(1700);//atras
        Servo8.pulsewidth_us(2100);//atras
        wait_ms(80);

            
    break; 
    case 0x04: //Atras
    
        Servo1.pulsewidth_us(2000);//abajo
        Servo5.pulsewidth_us(2000);//abajo
        wait_ms(80);
        Servo2.pulsewidth_us(1100);//adelante
        Servo6.pulsewidth_us(900);//adelante
        wait_ms(80);
        Servo1.pulsewidth_us(1500);//arriba
        Servo5.pulsewidth_us(1500);//arriba
        wait_ms(80);
        Servo2.pulsewidth_us(900);//atras
        Servo6.pulsewidth_us(1200);//atras
        wait_ms(80);       

        Servo3.pulsewidth_us(2000);//abajo
        Servo7.pulsewidth_us(2000);//abajo
        wait_ms(80);
        Servo4.pulsewidth_us(2000);//adelante
        Servo8.pulsewidth_us(1900);//adelante
        wait_ms(80);      
        Servo3.pulsewidth_us(1500);//arriba
        Servo7.pulsewidth_us(1500);//arriba
        wait_ms(80);
        Servo4.pulsewidth_us(1700);//atras
        Servo8.pulsewidth_us(2100);//atras
        wait_ms(80);
 


            
    break;  
    case 0x05: //Saludar1
         Servo2.pulsewidth_us(CENTRO);
         Servo8.pulsewidth_us(FRONT);
         Servo4.pulsewidth_us(BACK);
         Servo6.pulsewidth_us(CENTRO);
            
         Servo3.pulsewidth_us(ABAJO);
         Servo5.pulsewidth_us(ABAJO);
         Servo7.pulsewidth_us(ABAJO);
         wait_ms(500);
         Servo1.pulsewidth_us(ARRIBA);
         Servo2.pulsewidth_us(ADELANTE);
         wait_ms(500);
         Servo2.pulsewidth_us(ATRAS);
         wait_ms(500);
         Servo2.pulsewidth_us(ADELANTE);
         wait_ms(500);
         Servo2.pulsewidth_us(ATRAS);
         wait_ms(500);
         Servo2.pulsewidth_us(ADELANTE);
         wait_ms(500);
         Servo2.pulsewidth_us(ATRAS);
         wait_ms(500);
         Servo2.pulsewidth_us(CENTRO);
         Servo1.pulsewidth_us(ABAJO);
         Servo4.pulsewidth_us(CENTRO);
         Servo8.pulsewidth_us(CENTRO);
         wait_ms(2000);
         
         
    break;  
        case 0x06: //Saludar2
            Servo1.pulsewidth_us(ARRIBA);
            Servo3.pulsewidth_us(ARRIBA);
            Servo7.pulsewidth_us(ABAJO);
            Servo5.pulsewidth_us(ABAJO);
            
            Servo2.pulsewidth_us(FRONT);
            Servo4.pulsewidth_us(FRONT);
            Servo8.pulsewidth_us(FRONT);
            Servo6.pulsewidth_us(FRONT);
            wait_ms(500);
            Servo1.pulsewidth_us(ABAJO);
            Servo3.pulsewidth_us(ABAJO);
            Servo3.pulsewidth_us(ABAJO);
            Servo7.pulsewidth_us(ARRIBA);
            Servo5.pulsewidth_us(ARRIBA);
            
            Servo8.pulsewidth_us(BACK);
            Servo6.pulsewidth_us(BACK);
            Servo2.pulsewidth_us(BACK);
            Servo4.pulsewidth_us(BACK);
            wait_ms(500);
    break;
    
    case 0x07: //Pruebas
    
        Servo1.pulsewidth_us(2000);//abajo
        Servo5.pulsewidth_us(2000);//abajo
        wait_ms(80);
        Servo2.pulsewidth_us(1100);//adelante
        Servo6.pulsewidth_us(900);//adelante
        wait_ms(80);
        Servo1.pulsewidth_us(1500);//arriba
        Servo5.pulsewidth_us(1500);//arriba
        wait_ms(80);
        Servo2.pulsewidth_us(900);//atras
        Servo6.pulsewidth_us(1200);//atras
        wait_ms(80);       

        Servo3.pulsewidth_us(2000);//abajo
        Servo7.pulsewidth_us(2000);//abajo
        wait_ms(80);
        Servo4.pulsewidth_us(2000);//adelante
        Servo8.pulsewidth_us(1900);//adelante
        wait_ms(80);      
        Servo3.pulsewidth_us(1500);//arriba
        Servo7.pulsewidth_us(1500);//arriba
        wait_ms(80);
        Servo4.pulsewidth_us(1700);//atras
        Servo8.pulsewidth_us(2100);//atras
        wait_ms(80);


               
    break;
      default: break;
}
    }
void arrancar()
{
    Servo1.pulsewidth_us(ABAJO);
    Servo3.pulsewidth_us(ABAJO);
    Servo5.pulsewidth_us(ABAJO);
    Servo7.pulsewidth_us(ABAJO);
wait_ms(1000);
    Servo1.pulsewidth_us(ARRIBA);
wait_ms(500);
Servo1.pulsewidth_us(ABAJO);
wait_ms(500);
Servo1.pulsewidth_us(CENTRO);
wait_ms(500);
Servo2.pulsewidth_us(ADELANTE);
wait_ms(500);
Servo2.pulsewidth_us(ATRAS);
wait_ms(500);
Servo2.pulsewidth_us(CENTRO);
wait_ms(500);

Servo3.pulsewidth_us(ARRIBA);
wait_ms(500);
Servo3.pulsewidth_us(ABAJO);
wait_ms(500);
Servo3.pulsewidth_us(CENTRO);
wait_ms(500);
Servo4.pulsewidth_us(ADELANTE);
wait_ms(500);
Servo4.pulsewidth_us(ATRAS);
wait_ms(500);
Servo4.pulsewidth_us(CENTRO);
wait_ms(500);

Servo5.pulsewidth_us(ARRIBA);
wait_ms(500);
Servo5.pulsewidth_us(ABAJO);
wait_ms(500);
Servo5.pulsewidth_us(CENTRO);
wait_ms(500);
Servo6.pulsewidth_us(ADELANTE);
wait_ms(500);
Servo6.pulsewidth_us(ATRAS);
wait_ms(500);
Servo6.pulsewidth_us(CENTRO);
wait_ms(500);

Servo7.pulsewidth_us(ARRIBA);
wait_ms(500);
Servo7.pulsewidth_us(ABAJO);
wait_ms(500);
Servo7.pulsewidth_us(CENTRO);
wait_ms(500);
Servo8.pulsewidth_us(ADELANTE);
wait_ms(500);
Servo8.pulsewidth_us(ATRAS);
wait_ms(500);
Servo8.pulsewidth_us(CENTRO);
wait_ms(500);
    }   
void init_servo()
{
   Servo1.period_ms(20);
   Servo2.period_ms(20);
   Servo3.period_ms(20);
   Servo4.period_ms(20);
   Servo5.period_ms(20);
   Servo6.period_ms(20);
   Servo7.period_ms(20);
   Servo8.period_ms(20);
   /*
   //Posicion inicial a cada servo CENTRO
   Servo1.pulsewidth_us(CENTRO);
   Servo2.pulsewidth_us(CENTRO);
   Servo3.pulsewidth_us(CENTRO);
   Servo4.pulsewidth_us(CENTRO);
   Servo5.pulsewidth_us(CENTRO);
   Servo6.pulsewidth_us(CENTRO);
   Servo7.pulsewidth_us(CENTRO);
   Servo8.pulsewidth_us(CENTRO);
   */
}
Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	10 Oct 2018
Imports:	1
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	1
Followers:	1
Important changes to repositories hosted on mbed.com

mover.cpp

File content as of revision 1:59be7d7e1b54:

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

mover.cpp

File content as of revision 1:59be7d7e1b54:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
