File content as of revision 1:580fded7b5b2:

#include "EquatorStrutController.h"

EquatorStrut::EquatorStrut()
{
    PinState = 0;
    FullWavePeriod = 0;
    PartWavePeriod = 0;
    position = 0.0;
    direction = 0;
    Homing = false;
    HallTriggered = false;
    Enabled = true;
    
    ResetLine = new DigitalOut(P1_29);
    PulseOut1 = new DigitalOut(P1_27);
    PulseOut2 = new DigitalOut(P1_26);
    
    Disable();
    
    RGHSin = new DigitalIn(P0_11);
    RGHCos = new DigitalIn(P0_12);
    HallSensor = new InterruptIn(P0_2);
    
    InputReadTick.attach_us(this, &EquatorStrut::InputRead, 20);
    (*HallSensor).fall(this, &EquatorStrut::HallEffectFall);
    (*HallSensor).mode(PullUp);
    
    PhaseA = new PwmOut(P0_9);
    PhaseB = new PwmOut(P0_8);
    
    SinInterruptInterval.start();
}

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

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

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

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

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

double EquatorStrut::CurrentSpeed()
{
    if (SinInterruptInterval.read_us() < 100000)
    {    
        if (FullWavePeriod > 100000)
        {
            return 0.0;
        }
        else
        {
            return (0.04 / ((double)FullWavePeriod / 1000000)) * direction;
        }
    }
    else
    {
        return 0.0;
    }
}

void EquatorStrut::InputRead()
{    
    if (PinState == 3)
    {        
        PinState = 0 | ((*RGHSin) << 1) | (*RGHCos);
        
        if (PinState == 1)
        {
            direction = 1;
            position += (0.04 * direction);
            FullWavePeriod = SinInterruptInterval.read_us();
            SinInterruptInterval.reset();
        }
        else if (PinState == 2)
        {
            direction = -1;
            position += (0.04 * direction);
            FullWavePeriod = SinInterruptInterval.read_us();
            SinInterruptInterval.reset();
        }
    }
    else
    {
        PinState = 0 | ((*RGHSin) << 1) | (*RGHCos);
    }
}

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