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-04-24
Revision:
11:5bc741e99c53
Parent:
10:62a8fabbdf97

File content as of revision 11:5bc741e99c53:

#include "Servo_X.h"
#include "mbed.h"
#include "math.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()
{
}
void Servo::t_espera(float xi,float yi)
{
    int t=sqrt(xi*xi+yi*yi);
    wait_ms(int((t*100)/60));
}
int Servo::mm2pulse(float vmm)
{
  if (vmm < MAXPOS)
    return int (vmm*(MAXPULSE-MINPULSE)/(MAXPOS)) +MINPULSE;
  return MAXPULSE; 
}
void Servo::SetZ(float _z)
{
    z=_z;
    int PULSEZ=mm2pulse(z);
    mypwmZ.pulsewidth_us(PULSEZ); 
}
void Servo::SetServo(float _x,float _y)
{
    //float xi,yi;
    xa=x;
    ya=y;
    x=_x;
    y=_y;
    //xi=(x-xa)/50;
    //yi=(y-ya)/50;
    //for(int i=1;i<50;i++)
    //{
        //xa=xa+xi;
        //ya=ya+yi;
        int PULSEX=mm2pulse(xa);
        int PULSEY=mm2pulse(ya);
        mypwmX.pulsewidth_us(PULSEX);
        mypwmY.pulsewidth_us(PULSEY);
        //t_espera(xi,yi);
    //}   
}
float Servo::GetServoX()
{
    return x;
}
float Servo::GetServoY()
{
    return y;
}
float Servo::GetServoZ()
{
    return z;
}