File content as of revision 0:2032b926fd86:

#include "mbed.h"
#include "Serial.h"         //serial
#ifndef SERVO_H
#define SERVO_H
#define MINPULSE 560
#define MAXPULSE 2400
#define MAXPOS 50// poscisión max de la coordenasda en mm
#define DRAW 50 
#define NODRAW 0
#define CM_EJECUTAR 0Xff
#define CM_GUARDAR 0Xfe
#define CM_SERVO 0Xfd
#define CM_SDRAW 0xfc
#define CM_SN0DRAW 0xfb
#define CM_STOP 0xfa
#define CM_END 0xf0

class Servo
{
    private:
        uint8_t x,y,z;
        int mm2pulse(float vmm);
    public:
        Servo();
        ~Servo();
        void SetServo(uint8_t,uint8_t);
        void SetZ(uint8_t);
        uint8_t GetServoX();
        uint8_t GetServoY();
        uint8_t GetServoZ();
        int CONVERTIDOR(int);
};

#endif

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;
}