File content as of revision 1:8574f732dba3:

#include "mbed.h"
#include "Serial.h"         //BIBLIOTECA COMUNICACION SERIAL
#define SS_TIME 100 // NO ME ACUERDOOOOO - EN MICROSEGUNDO
#ifndef SERVO_H
#define SERVO_H
#define MINPULSE 560
#define MAXPULSE 2400
#define MAXPOS 50// MAXIMA POSICION EN COORDENADAS EN mm
#define DRAW 50 
#define NODRAW 0
#define OP_EJECUTAR 0Xff
#define OP_GUARDAR 0Xfe
#define OP_SERVO 0Xfd
#define OP_SDRAW 0xfc
#define OP_SNODRAW 0xfb
#define OP_STOP 0xfa
#define OP_END 0xf0
#define MEM_SIZE 5000 //TAMAÑO DEL ARREGLO
#define MEM_TYPE uint32_t //TAMAÑO DE CADA CASILLA

class Servo
{
    private:
        uint8_t x,y,z; //UNSIGNED SHORT INT
        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;
}