File content as of revision 2:01227a4bd3d7:

#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=7.0;
    ki=0.05;
    kd=0.0;
    integ=0;
    derv=0;
    cpidx =0;

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

}

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
}
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;
            break;
        case 1:
            if(PreviousEncode==0) CurrentPosition +=1;
            else if(PreviousEncode==3) CurrentPosition -=1;
            break;
        case 2:
            if(PreviousEncode==3) CurrentPosition +=1;
            else if(PreviousEncode==0) CurrentPosition -=1;
            break;
        case 3:
            if(PreviousEncode==1) CurrentPosition +=1;
            else if(PreviousEncode==2) CurrentPosition -=1;
            break;
        default:
            break;
    }
    PreviousEncode = CurrentEncode;
}


void MotorCtl::PIDControl()
{
    int DeltaCnt;
    DeltaCnt = CurrentPosition - PreviousPosition;
    PreviousPosition = CurrentPosition;
    CurrentSpeed = (int)CalculateRPM(DeltaCnt);
    if (cpidx<MaxBuf) cw0[cpidx++]=CurrentSpeed;

    Error = TargetSpeed - CurrentSpeed;

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

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