File content as of revision 3:df8469e71728:

#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.2;
    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;
 //   printf("PreviousEncode: %x  CSP: %x\r\n",PreviousEncode,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>35) control=35;
    else if(control <-35) control = -35;
    PreviousError = Error;
    if(TargetSpeed>0){
        duty =TargetSpeed+control;
        _Dir=1;
    //printf("control: %d\r\n",control);
    }else{
        duty =TargetSpeed-control;
        _Dir=0;
    }
    
    //if(duty<=0){duty=-duty;}
    vel=duty/180.0;
    
   printf("Duty: %d  vel: %f\r\n",duty,vel);
    _pwm.write(vel);
  
    
   
        
    
    
}

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