Steven Ortiz / Move_Servos

el programa permite ingresar la posición en el eje X,Y,Z, haciendo que el Servo MG90S con sus movimientos de 180° trabaje en mm de 0 a 50.

Dependents:   PICCOLO_CNC PICCOLO_CNC1

Servo_X.cpp

Committer:
Stevenor1997
Date:
2018-03-08
Revision:
4:4b884116fbe6
Parent:
3:07bcb3c9a8b8
Child:
6:c81ba0f175ac

File content as of revision 4:4b884116fbe6:

#include "Servo_X.h"
#include "mbed.h"
#include "Serial.h"         //serial


PwmOut mypwmX(PA_8);
PwmOut mypwmY(PB_10);
PwmOut mypwmZ(PB_4);

Servo::Servo()
{
    mypwmX.period_ms(20);
    mypwmY.period_ms(20);
    mypwmZ.period_ms(20);
    mypwmX.pulsewidth_us(MAXPULSE);
    mypwmY.pulsewidth_us(MAXPULSE);
    mypwmZ.pulsewidth_us(NODRAW);
    
}

Servo::~Servo()
{
}
int Servo::mm2pulse(float vmm)
{
  if (vmm < MAXPOS)
    return int (vmm*(MAXPULSE-MINPULSE)/(MAXPOS)) +MINPULSE;
  return MAXPULSE; 
}
void Servo::SetZ(uint8_t _z)
{
    z=_z;
    int PULSEZ=mm2pulse(CONVERTIDOR(z));
    mypwmZ.pulsewidth_us(PULSEZ); 
}
void Servo::SetServo(uint8_t _x,uint8_t _y)
{
    x=_x;
    y=_y;
    int PULSEX=mm2pulse(CONVERTIDOR(x));
    int PULSEY=mm2pulse(CONVERTIDOR(y));
    mypwmX.pulsewidth_us(PULSEX);
    mypwmY.pulsewidth_us(PULSEY);   
}
uint8_t Servo::GetServoX()
{
    return x;
}
uint8_t Servo::GetServoY()
{
    return y;
}
uint8_t Servo::GetServoZ()
{
    return z;
}
int Servo::CONVERTIDOR(int c)
{
    int u=0;
    u=c/16;
    return c-6*u;
}