sebastian martinez / Mbed 2 deprecated Movimiento_8Servos

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

Fork of 01-04_Primer_Avance by sebastian martinez

mover.cpp

Committer:
sebasmartinez
Date:
2018-10-06
Revision:
5:6ca97bfe9406
Parent:
4:ba51736778e1

File content as of revision 5:6ca97bfe9406:

#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 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);
            wait(0.5);
            Servo2.pulsewidth_us(ADELANTE);
            wait(0.5);
            Servo1.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo2.pulsewidth_us(ATRAS);
            wait(0.5);
         }else{
            Servo1.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo2.pulsewidth_us(ATRAS);
            wait(0.5);
            Servo1.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo2.pulsewidth_us(ADELANTE);
            wait(0.5);
          }
    break;
    case 0x02: 
        if(dir==0x01){
            Servo3.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo4.pulsewidth_us(ADELANTE);
            wait(0.5);
            Servo3.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo4.pulsewidth_us(ATRAS);
            wait(0.5);
         }else{
            Servo3.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo4.pulsewidth_us(ATRAS);
            wait(0.5);
            Servo3.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo4.pulsewidth_us(ADELANTE);
            wait(0.5);
          }
    break;
    case 0x03: 
        if(dir==0x01){
            Servo5.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo6.pulsewidth_us(ADELANTE);
            wait(0.5);
            Servo5.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo6.pulsewidth_us(ATRAS);
            wait(0.5);
         }else{
            Servo5.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo6.pulsewidth_us(ATRAS);
            wait(0.5);
            Servo5.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo6.pulsewidth_us(ADELANTE);
            wait(0.5);
          }
    break;
    case 0x04: 
        if(dir==0x01){
            Servo7.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo8.pulsewidth_us(ADELANTE);
            wait(0.5);
            Servo7.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo8.pulsewidth_us(ATRAS);
            wait(0.5);
         }else{
            Servo7.pulsewidth_us(ARRIBA);
            wait(0.5);
            Servo8.pulsewidth_us(ATRAS);
            wait(0.5);
            Servo7.pulsewidth_us(ABAJO);
            wait(0.5);
            Servo8.pulsewidth_us(ADELANTE);
            wait(0.5);
          }
    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 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);
   */
}