File content as of revision 29:33c5c61f1068:

#include "mbed.h"
#include "include/motor.hpp"

Motor::Motor(PinName aPin, PinName fPin, PinName bPin, PinName pwmPin, QEI &enc):aIn(aPin),Forward(fPin),Backward(bPin),pwmOut(pwmPin),encoder(enc){ 
//15,17,18,21,qei25,26 and 16,19,20,22,qei23,24
    freq =.001;
    voltage = 12.0;
    mode=0;
    
    kp=3;
    kd=0.04;
    
    pwmOut.period_us(.0005);
    }

void Motor::start(){
    t.attach(this, &Motor::Control, freq);
}

void Motor::stop(){
    t.detach();
    pwmOut.write(0);
    dAngle=0;
    dAngularVelocity=0;
}

void Motor::zero(){
    encoder.reset();
}

void Motor::Control(){
    float preAngle=angle; // Storing the angle in the previous time step
    angle = encoder.getPulses()/1200.0*6.283; // Updating new motor angle
    speed =Motor::filterLowPass(speed,(angle-preAngle)/freq, .2); // Updating new motor angular vel
    float mCurrent = getCurrent();
    
    float dCurrent;
    switch(mode){
        case 1:
            dCurrent = kp*(dAngle-angle);
        case 2:
            dCurrent = kd*(dAngularVelocity-speed);
        case 3:
            dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
        case 0:
            dCurrent = dTorque/3.27;
    }    
    
    float duty=0;
    if (dCurrent>0){
        Forward=1;
        Backward=0;
        duty= (abs(dCurrent)*3.27+0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
    }else if (dCurrent<0){
        Forward=0;
        Backward=1;
        duty= (abs(dCurrent)*3.27-0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
    }
    
    if (duty>0.99){
        duty =1;
    }
    
    pwmOut.write(duty);
}

float Motor::getCurrent()
{
    return (1/.140)*3.3 * aIn;
}

int Motor::getPos()
{
    return encoder.getPulses();
}

float Motor::filterLowPass(float old, float currentIn, float alphar){
    return (old+alphar*(currentIn-old));
}

void Motor::setTorque(float t){
    mode = 0;
    dTorque = t;
}

void Motor::setPos(float pos){
    mode = 1;
    dAngle=pos;
}

void Motor::setVel(float vel){
    mode = 2;
    dAngularVelocity = vel;
}

void Motor::setPosVel(float pos, float vel){
    mode = 3;
    dAngle = pos;
    dAngularVelocity = vel;
}