File content as of revision 0:266a770a17e9:

#include "mbed.h"
#include "StepperDrive.h"

StepperDrive::StepperDrive(PinName in1, PinName in2, bool in3, PinName in4, PinName in5, bool in6, float in7, float in8, float in9):leftStep(in1), leftDir(in2), rightStep(in4), rightDir(in5)
{
    wheelCircum=in7;
    wheelSepar=in8;
    invertLeft=in3;
    invertRight=in6;
    pit.attach(this, &StepperDrive::pitCallback, in9);
    moveComplete=true;
}

int StepperDrive::move(float distance, float angle)
{
    if(moveComplete==false) //if there is a move in progress
    {                       //return
        return -1;
    }
    
    float stepDistance=wheelCircum/(200*Ustep);
    float d, dl, dr;
    
    if(distance==0 && angle!=0)
    {
        d=angle*wheelSepar/2;
        leftSteps=d/stepDistance;
        rightSteps=-1*leftSteps;
        leftStepsPC=(angle>0 ? -1:1);
        rightStepsPC=-1*leftStepsPC;
        moveComplete=false;
    }
    else if(angle==0 && distance!=0)
    {
        leftSteps=distance/stepDistance;
        rightSteps=leftSteps;
        leftStepsPC=-1;
        rightStepsPC=-1;
        moveComplete=false;
    }
    else if(angle>0 && distance!=0)
    {
        dl=angle*(distance/angle+wheelSepar/2);
        dr=angle*(distance/angle-wheelSepar/2);
        
        leftSteps=dl/stepDistance;
        rightSteps=dr/stepDistance;
        leftStepsPC=-1;
        rightStepsPC=-1*(dr/dl);
        moveComplete=false;
    }
    else if(angle<0 && distance!=0)
    {
        dl=angle*(distance/angle-wheelSepar/2);
        dr=angle*(distance/angle+wheelSepar/2);
        
        leftSteps=dl/stepDistance;
        rightSteps=dr/stepDistance;
        leftStepsPC=-1*(dl/dr);
        rightStepsPC=-1;
        moveComplete=false;
    }
    leftError=0;
    rightError=0;
    return 0;
}

void StepperDrive::pitCallback()
{
    if(moveComplete==true)
    {
        return;
    }
    else
    {
        if(leftSteps!=0)
        {
            leftError+=(fabs(leftStepsPC)-floor(fabs(leftStepsPC)));
            for(int i=0; i<floor(fabs(leftStepsPC)+leftError); i++)
            {
                stepLeft(leftStepsPC>0);
            }
            if(signbit(leftSteps)!=signbit(leftSteps-floor(fabs(leftStepsPC)+leftError)))
            {
                leftSteps=0;
            }
            if(leftError>=1)
            {
                leftError-=1;
            }
        }
        if(rightSteps!=0)
        {
            rightError+=(fabs(rightStepsPC)-floor(fabs(rightStepsPC)));
            for(int i=0; i<floor(fabs(rightStepsPC)+rightError); i++)
            {
                stepRight(rightStepsPC>0);
            }
            if(signbit(rightSteps)!=signbit(rightSteps-floor(fabs(rightStepsPC)+leftError)))
            {
                rightSteps=0;
            }
            if(rightError>=1)
            {
                rightError-=1;
            }
        }
        if(leftSteps==0 && rightSteps==0)
        {
            moveComplete=true;
        }
    }
}

void StepperDrive::stepLeft(bool dir)
{
    leftDir=(invertLeft^dir);
    leftStep=1;
    wait_us(3);
    leftStep=0;  
}

void StepperDrive::stepRight(bool dir)
{
    rightDir=(invertRight^dir);
    rightStep=1;
    wait_us(3);
    rightStep=0;    
}

bool StepperDrive::isMoveDone()
{
    return moveComplete;
}