File content as of revision 2:3976e3f43470:

#include "EquatorStrutController.h"

EquatorStrut::EquatorStrut()
{
    position = 0;
    direction = 0;
    Homing = false;
    HallTriggered = false;
    Enabled = true;
    
    ResetLine = new DigitalOut(P1_29);
    
    Disable();
    
    RGHSinInterrupt = new InterruptIn(P0_11);
    RGHCosInterrupt = new InterruptIn(P0_12);
    RGHSinFallingInterrupt = new InterruptIn(P0_13);
    RGHCosFallingInterrupt = new InterruptIn(P0_14);
    
    HallSensor = new InterruptIn(P0_2);
    HallSensorState = new DigitalIn(P0_2);
    
    (*RunningTime).start();
    
    (*RGHSinInterrupt).rise(this, &EquatorStrut::RGHSinRisingHandler);
    (*RGHCosInterrupt).rise(this, &EquatorStrut::RGHCosRisingHandler);
    (*RGHSinFallingInterrupt).fall(this, &EquatorStrut::RGHSinFallingHandler);
    (*RGHCosFallingInterrupt).fall(this, &EquatorStrut::RGHCosFallingHandler);
    
    (*HallSensor).fall(this, &EquatorStrut::HallEffectFall);
    (*HallSensor).mode(PullUp);
    
    PhaseA = new PwmOut(P0_9);
    PhaseB = new PwmOut(P0_8);
}

bool EquatorStrut::IsEnabled()
{
    return Enabled;
}

void EquatorStrut::ActionEvent(bool CurrHigh, bool CurrSin)
{    
    // Same event again - DO NOTHING 
    if (CurrHigh == LastHigh && CurrSin == LastSin)
    {
        return;
    }
    
    if (CurrSin != LastSin) // Otherwave
    {
        // Other wave
        if ((CurrSin && CurrHigh == LastHigh) || 
            (!CurrSin && CurrHigh != LastHigh))
        {
            //Forwards
            direction = 1;
        }
        else
        {
            //Backwards
            direction = -1;
        }
        
        
    }
    else
    {
        // Reversal
        direction = -direction;
    }
    
    position += direction;
    
    // Set the state for the wave that fired
    if(CurrSin) SinHigh = CurrHigh;
    else        CosHigh = CurrHigh;
    
    // Set the last event values
    LastHigh = CurrHigh;
    LastSin = CurrSin;
}

void EquatorStrut::DisableInterrupts()
{
    (*RGHSinInterrupt).disable_irq();
    (*RGHCosInterrupt).disable_irq();
    (*RGHSinFallingInterrupt).disable_irq();
    (*RGHCosFallingInterrupt).disable_irq();
}

void EquatorStrut::EnableInterrupts()
{
    (*RGHSinInterrupt).enable_irq();
    (*RGHCosInterrupt).enable_irq();
    (*RGHSinFallingInterrupt).enable_irq();
    (*RGHCosFallingInterrupt).enable_irq();
}

void EquatorStrut::SetPower(double power)
{
    if(!Enabled)
    {
        return;
    }
    
    if (fabs(power) > 1.0) return;
    
    currentPower = power;
    
    (*PhaseA) = (power + 1.0) / 2;
    (*PhaseB) = 1.0 - ((power + 1.0) / 2);
}

double EquatorStrut::GetPosition()
{
    return position * 0.01;
}

void EquatorStrut::Home()
{
    if (!Enabled)
    {
        Enable();
    }
    
    if ((*HallSensorState) == 1)
    {
        DisableInterrupts();
        
        direction = -1;
        
        Homing = true;
        HallTriggered = false;
        
        SetPower(-0.6);
        
        while (!HallTriggered)
        {
            wait(0.1);
        }
        
        EnableInterrupts();
    }
    
    SetPower(1.0);
        
    while (position < 2000)
    {
        //SerialTransmit();
    }
    
    Homing = true;
    HallTriggered = false;
    
    DisableInterrupts();
    
    direction = -1;
    
    SetPower(-0.4);
    
    while (!HallTriggered)
    {
        wait(0.1);
    }
    
    EnableInterrupts();
}

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

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

double EquatorStrut::CurrentSpeed()
{
    double interval = (*RunningTime).read() - SpeedInterval;
    int positionDiff = position - LastPosition;
    
    SpeedInterval = (*RunningTime).read();
    LastPosition = position;
    
    return (positionDiff * 0.01)/interval;
}

double EquatorStrut::CurrentPower()
{
    return currentPower;
}

void EquatorStrut::RGHSinRisingHandler()
{
    ActionEvent(true, true);
}

void EquatorStrut::RGHSinFallingHandler()
{
    ActionEvent(false, true);
}

void EquatorStrut::RGHCosRisingHandler()
{
    ActionEvent(true, false);
}

void EquatorStrut::RGHCosFallingHandler()
{
    ActionEvent(false, false);
}

void EquatorStrut::HallEffectFall()
{    
    if (direction < 0)
    {        
        SetPower(0.0);
    
        if (Homing)
        {
            HallTriggered = true;
            Homing = false;
            position = 0.0;
        }
    }
}