File content as of revision 2:088eeae4287c:

#include "mbed.h"
#include "RawSerial.h"

DigitalIn RGHSinState(P0_11);
DigitalIn RGHCosState(P0_12);
InterruptIn RGHSinInterrupt(P0_11);
InterruptIn RGHCosInterrupt(P0_12);
InterruptIn HallSensor(P0_2);
DigitalOut ResetLine(P1_29);
PwmOut PhaseA(P0_9);
PwmOut PhaseB(P0_8);
Timer RunningTime;

bool Enabled = false;
bool Homing = false;
bool HallTriggered = false;

RawSerial pc(P1_27, P1_26);

char PinState = 0;

int direction = 0;
double position = 0.0;
double currentPower = 0.0;
int interruptPeriod = 0;
int lastTime = 0;

int intteruptPeriodArray[15];
int arrayTotal = 0;
char arrayPos = 0;

char counter = 0;

void SmoothingAdd(int input)
{
    arrayTotal -= intteruptPeriodArray[arrayPos];
    arrayTotal += input;
    intteruptPeriodArray[arrayPos] = input;
    
    if (arrayPos == 14)
    {
        arrayPos = 0;
    }
    else
    {
        arrayTotal++;
    }
}

int SmoothedInterruptPeriod()
{
    return arrayTotal / 15;
}

void RGHSinHandler()
{    
    if (PinState == 2)
    {        
        return;
    }
    else if (PinState == 1)
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
        
        if(PinState == 3)
        {
            direction = 1;
            position += 0.04 * direction;
            SmoothingAdd(RunningTime.read_us() - lastTime);
            lastTime = RunningTime.read_us();
        }
    }
    else
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
    }
}

void RGHCosHandler()
{    
    if (PinState == 1)
    {        
        return;
    }
    else if (PinState == 2)
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
        
        if (PinState == 3)
        {        
            direction = -1;
            position += 0.04 * direction;
            SmoothingAdd(RunningTime.read_us() - lastTime);
            lastTime = RunningTime.read_us();
        }
    }
    else
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
    }
}

void SetPower(double power)
{
    currentPower = power;
    if(!Enabled)
    {
        return;
    }
    
    if (power > 1.0 || power < -1.0)
    {
        return;
    }
    
    PhaseA = (power + 1.0) / 2;
    PhaseB = 1.0 - ((power + 1.0) / 2);
}

void Enable()
{
    SetPower(0.0);
    
    ResetLine = 1;
    
    Enabled = true;
}

void Disable()
{
    ResetLine = 0;
    
    SetPower(0.0);
    
    Enabled = false;
}

void Home()
{
    if (!Enabled)
    {
        Enable();
    }
    
    Homing = true;
    HallTriggered = false;
    
    SetPower(-1.0);
    
    while (!HallTriggered)
    {
        wait(0.5);
    }
    
    SetPower(1.0);
    
    while (position < 20.0)
    {
        
    }
    
    Homing = true;
    HallTriggered = false;
    
    SetPower(-0.5);
    
    while (!HallTriggered)
    {
        wait(0.5);
    }
}

double GetSpeed()
{
    if ((RunningTime - lastTime) > 10000)
    {
        return 0.0;
    }
    return (direction * 0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
}

void SerialTransmit()
{        
    double tempPos = position;
    double tempTime = RunningTime.read();
    double tempPow = currentPower;
    double tempSpeed = GetSpeed();
    int outChar = 0;
    
    if (tempPos < 0.0)
    {
        pc.putc('-');
        tempPos *= -1;
    }
    if (tempPos >= 100.0)
    {
        outChar = tempPos / 100;
        pc.putc(outChar + 48);
        tempPos -= outChar * 100.0;
    }
    if (tempPos >= 10.0)
    {
        outChar = tempPos / 10;
        pc.putc(outChar + 48);
        tempPos -= outChar * 10.0;
    }
    else if(outChar > 0)
    {
        pc.putc('0');
    }
    if (tempPos >= 1.0)
    {
        outChar = tempPos;
        pc.putc(outChar + 48);
        tempPos -= outChar;
    }
    else
    {
        pc.putc('0');
    }
    if (tempPos >= 0.1)
    {
        pc.putc('.');
        outChar = tempPos * 10;
        pc.putc(outChar + 48);
        tempPos -= (double)outChar / 10.0;
    }
    else
    {
        pc.putc('.');
        pc.putc('0');
    }
    if (tempPos >= 0.01)
    {
        outChar = tempPos * 100;
        pc.putc(outChar + 48);
    }
    
    pc.putc(',');
    
    outChar = 0;
    
    if (tempTime >= 100.0)
    {
        outChar = tempTime / 100;
        pc.putc(outChar + 48);
        tempTime -= outChar * 100.0;
    }
    if (tempTime >= 10.0)
    {
        outChar = tempTime / 10;
        pc.putc(outChar + 48);
        tempTime -= outChar * 10.0;
    }
    else if(outChar > 0)
    {
        pc.putc('0');
    }
    if (tempTime >= 1.0)
    {
        outChar = tempTime;
        pc.putc(outChar + 48);
        tempTime -= outChar;
    }
    else
    {
        pc.putc('0');
    }
    if (tempTime >= 0.1)
    {
        pc.putc('.');
        outChar = tempTime * 10;
        pc.putc(outChar + 48);
        tempTime -= (double)outChar / 10.0;
    }
    else
    {
        pc.putc('.');
        pc.putc('0');
    }
    if (tempTime >= 0.01)
    {
        outChar = tempTime * 100;
        pc.putc(outChar + 48);
        tempTime -= (double)outChar / 100.0;
    }
    else
    {
        pc.putc('0');
    }
    if (tempTime >= 0.001)
    {
        outChar= tempTime * 1000;
        pc.putc(outChar + 48);
    }
    
    pc.putc(',');
    outChar = 0;
    
    if (tempPow < 0.0)
    {
        pc.putc('-');
        tempPow *= -1;
    }
    if (tempPow >= 1.0)
    {
        outChar = tempPow;
        pc.putc(outChar + 48);
        tempPow -= outChar;
    }
    else
    {
        pc.putc('0');
    }
    if (tempPow >= 0.1)
    {
        pc.putc('.');
        outChar = tempPow * 10;
        pc.putc(outChar + 48);
        tempPow -= (double)outChar / 10.0;
    }
    else
    {
        pc.putc('.');
        pc.putc('0');
    }
    if (tempPow >= 0.01)
    {
        outChar = tempPow * 100;
        pc.putc(outChar + 48);
        tempPow -= (double)outChar / 100.0;
    }
    else
    {
        pc.putc('0');
    }
    if (tempPow >= 0.001)
    {
        outChar= tempPow * 1000;
        pc.putc(outChar + 48);
    }
    
    pc.putc(',');
    outChar = 0;
    
    if (tempSpeed < 0.0)
    {
        pc.putc('-');
        tempSpeed *= -1;
    }
    if (tempSpeed >= 100.0)
    {
        outChar = tempSpeed / 100;
        pc.putc(outChar + 48);
        tempSpeed -= outChar * 100.0;
    }
    if (tempSpeed >= 10.0)
    {
        outChar = tempSpeed / 10;
        pc.putc(outChar + 48);
        tempSpeed -= outChar * 10.0;
    }
    else if(outChar > 0)
    {
        pc.putc('0');
    }
    if (tempSpeed >= 1.0)
    {
        outChar = tempSpeed;
        pc.putc(outChar + 48);
        tempSpeed -= outChar;
    }
    else
    {
        pc.putc('0');
    }
    if (tempSpeed >= 0.1)
    {
        pc.putc('.');
        outChar = tempSpeed * 10;
        pc.putc(outChar + 48);
        tempSpeed -= (double)outChar / 10.0;
    }
    else
    {
        pc.putc('.');
        pc.putc('0');
    }
    if (tempSpeed >= 0.01)
    {
        outChar = tempSpeed * 100;
        pc.putc(outChar + 48);
    }
    
    pc.putc(10);
    pc.putc(13);
}

void HallEffectFall()
{    
    RGHSinInterrupt.disable_irq();
    RGHCosInterrupt.disable_irq();
    
    if (direction < 0)
    {        
        SetPower(0.0);
    
        if (Homing)
        {
            HallTriggered = true;
            Homing = false;
            position = 0.0;
        }
    }
    RGHSinInterrupt.enable_irq();
    RGHCosInterrupt.enable_irq();
}

int main()
{
    RGHSinInterrupt.rise(&RGHSinHandler);
    RGHCosInterrupt.rise(&RGHCosHandler);
    HallSensor.fall(&HallEffectFall);
    HallSensor.mode(PullUp);
    
    RunningTime.start();
    
    pc.baud(115200);
    
    Home();
    
    while(1)
    {        
        SerialTransmit();
        
        switch(counter)
        {
        case 0:
            if (position < 200)
            {
                SetPower(1.0);
            }
            else
            {
                counter++;
            }
            break;
        case 1:
            if (position > 10)
            {
                SetPower(-1.0);
            }
            else
            {
                counter = 0;
            }
            break;
        }
    }
}