File content as of revision 4:754c74beef20:

#include "mbed.h"
#include "StepperDrive.h"
#include <cmath> // fabs, floor, signbit

StepperDrive::StepperDrive(Serial &pc1, PinName in1, PinName in2, bool in3, PinName in4, PinName in5, bool in6, float in7, float in8, float in9):pc(pc1), leftStep(in1), leftDir(in2), rightStep(in4), rightDir(in5)
{
    wheelCircum=in7;
    wheelSepar=in8;
    invertLeft=in3;
    invertRight=in6;
    pit.attach_us(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)+rightError)))
            {
                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;

    /* completed 1 step (increment or decrement */
    if(leftSteps < 0)
        leftSteps++;
    else
        leftSteps--;
}

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

    /* completed 1 step (increment or decrement */
    if(rightSteps < 0)
        rightSteps++;
    else
        rightSteps--;
}

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

// FUNCTION:
//      void pauseMove()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Stops current move. Saves remaining steps in leftStepsPause
//      and rightStepsPause.
void StepperDrive::pauseMove();
{
    /* need to implement*/
}

// FUNCTION:
//      void resumeMove()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Restores remaining steps from leftStepsPause
//      and rightStepsPause to leftSteps and rightSteps. Resumes
//      move.
void StepperDrive::resumeMove();
{
    /* need to implement */
}