File content as of revision 8:4fa7fadc583d:

#include "motor.h"

MotorCtl::MotorCtl(PinName Pwm, PinName Dir, PinName tachoA, PinName tachoB):
    _pwm(Pwm), _Dir(Dir), _tachoA(tachoA), _tachoB(tachoB)
{
    _Dir=1;
    dr = 10;
    _pwm.period_ms(32);
    _pwm.write(0);
    _tick.attach_us(this,&MotorCtl::PIDControl,32000);
    kp=0.1;
    ki=0.2;
    kd=0.0;
    integ=0;
    derv=0;
    cpidx =0;

    DeltaT = 32; // Timer Interrupt period = 32ms = 32,000 us
    CntPerRev = 2800; // 100 shft/rev x 7 pls/shft x 4 evnt/pls
    duty = 50;
    PreviousEncode = 0;
    TargetSpeed = 0; //initial target speed
    vel=0;
    DistancePosition =0;
}

MotorCtl::~MotorCtl()
{
}

int MotorCtl::getRPM()
{
    return CurrentSpeed;
}
// Rturn target speed
int MotorCtl::getTarget()
{
    return TargetSpeed;
}
int MotorCtl::getError()
{
    return Error;
}
float MotorCtl::getKP()
{
    return kp;
}
float MotorCtl::getKI()
{
    return ki;
}
float MotorCtl::getKD()
{
    return kd;
}
int *MotorCtl::getHistory()
{
    return cw0;
}
int MotorCtl::getCurrentPosition()
{
    return CurrentPosition;
}

//set Method

void MotorCtl::SetPeriod(long pwmPeriod)
{
    _pwm.period_ms(pwmPeriod);
}

void MotorCtl::setTarget(int spd)
{
    if(spd<0) dr=0;
    else dr=10;
    setDirection();
    
    TargetSpeed = spd;
    integ = 0;
    derv=0;
    cpidx=0; // clear
//    printf("Target %d\r\n",TargetSpeed);
}
void MotorCtl::setPID(float p, float i, float d)
{
    kp = p;
    ki = i;
    kd = d;
    integ =0;
    derv=0;
    cpidx=0;
}

void MotorCtl::Reset(void)
{
    integ=0;
    derv=0;
    cpidx=0;
}

void MotorCtl::setDirection()
{
    if (dr>5) {
        _Dir=1;
    } else {
        _Dir=0;
    }
}

void MotorCtl::UpdateCurrentPosition()
{
    unsigned char CurrentEncode;
    CurrentEncode = (_tachoA << 1 | _tachoB);
    switch(CurrentEncode) {
        case 0:
            if (PreviousEncode==2) CurrentPosition +=1;
            else if(PreviousEncode==1) CurrentPosition -=1;
            DistancePosition +=1;
            break;
        case 1:
            if(PreviousEncode==0) CurrentPosition +=1;
            else if(PreviousEncode==3) CurrentPosition -=1;
            DistancePosition +=1;
            break;
        case 2:
            if(PreviousEncode==3) CurrentPosition +=1;
            else if(PreviousEncode==0) CurrentPosition -=1;
            DistancePosition +=1;
            break;
        case 3:
            if(PreviousEncode==1) CurrentPosition +=1;
            else if(PreviousEncode==2) CurrentPosition -=1;
            DistancePosition +=1;
            break;
        default:
            break;
    }
    PreviousEncode = CurrentEncode;
//    printf("CSP: %x\r\n",CurrentPosition);
}


void MotorCtl::PIDControl()
{
    int DeltaCnt;
    DeltaCnt = CurrentPosition - PreviousPosition;
    PreviousPosition = CurrentPosition;
    CurrentSpeed = (int)CalculateRPM(DeltaCnt);
//    printf("DeltaCnt: %d  CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed);
    Error = TargetSpeed - CurrentSpeed;

    integ += Error;
    derv = Error-PreviousError;
    control = (int)(kp*Error+ki*integ+kd*derv);
    if (control>40) control=40;
    else if(control <-40) control = -40;
    PreviousError = Error;
    if(TargetSpeed>0){
        vel =(TargetSpeed+control)/200.0;
         _pwm.write(vel);
        _Dir=1;
//    printf("control: %d\r\n",control);
    }else{
        vel =(-TargetSpeed-control)/200.0;
        _pwm.write(vel);
        _Dir=0;
    } 
    //    printf("Duty: %d  Target:%d control:%d\r\n",duty,TargetSpeed, control);
    
}

float  MotorCtl::CalculateRPM(int DeltaCnt)
{
   
    return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us
}

float  MotorCtl::CalculateCumDis() //accumulate Distance
{
   
    return (DistancePosition*3.14159265359*0.043/CntPerRev); // Time measured in us
}

float MotorCtl::CalculateRelaDis() //relative Distance
{
    return (CurrentPosition*3.14159265359*0.043/CntPerRev);
}
float MotorCtl::CalculateVelocity()
{
    return getRPM()*3.14159265359*0.043/60;
}