File content as of revision 2:1de1be9f0ab7:

#include "Motor.h"

Motor::Motor(PinName PWM, PinName Direction, PinName Enc1,PinName Enc2,PinName HomingSW,float interval):
    encoder(Enc1,Enc2,NC,4200),
    MotorThrottle(PWM),
    MotorDirection(Direction),
    HomingSwitch(HomingSW),
    controller(1,0,0,interval)
{
    SetPID(1,0,0);
    _GearRatio=1;
    MotorThrottle=0;
    MotorDirection=0;
}

void Motor::GotoPos(float Rad){
    // Enter The current values
    controller.setSetPoint(Rad);
    controller.setProcessValue(GetPos());
    
    // Compute controller output
    float OutPut=controller.compute();
    
    // Direction handling
    float Direction=0;
    
    if(OutPut<0){
        Direction=1;
    }
    
    OutPut=fabs(OutPut);
    
    // Output schrijven
    MotorThrottle.write(OutPut);
    MotorDirection=Direction;
}

float Motor::GetPos(){
    return (encoder.getPulses()/4200.0f)*(2.0f*3.1415f)/_GearRatio;
}


void Motor::Homing(bool direction, float PWM,float HomingPos){
    HomingSwitch.mode(PullUp);
    MotorThrottle=PWM;
    MotorDirection=direction;
    while (HomingSwitch.read()==1){
    }
    MotorDirection=0;
    MotorThrottle=0.0f;
    encoder.reset((int)4200.0f*_GearRatio*(HomingPos/2.0f/3.1415));
    Stop();
}

void Motor::SetPID(float P,float Ti,float Td){
    controller.setTunings(P,Ti, Td);
}

void Motor::SetGearRatio(float GearRatio){
    _GearRatio=GearRatio;
}

void Motor::SetInputLimits(float Imin, float Imax){
    controller.setInputLimits(Imin,Imax);
}

void Motor::SetOutputLimits(float Omin, float Omax){
    controller.setOutputLimits(Omin,Omax);
}

void Motor::Stop(){
    MotorThrottle=0;
    MotorDirection=0;
}