Diff: EquatorStrutController.cpp

Revision:

0:a6ade00ff41d

Child:

1:580fded7b5b2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EquatorStrutController.cpp	Tue Jul 29 08:44:30 2014 +0000
@@ -0,0 +1,161 @@
+#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.02 / ((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);
+        }
+        else if (PinState == 2)
+        {
+            direction = -1;
+            position += (0.04 * direction);
+        }
+    }
+    else
+    {
+        PinState = 0 | ((*RGHSin) << 1) | (*RGHCos);
+    }
+}
+
+void EquatorStrut::HallEffectFall()
+{    
+    if (direction < 0)
+    {        
+        SetPower(0.0);
+    
+        if (Homing)
+        {
+            HallTriggered = true;
+            Homing = false;
+            position = 0.0;
+        }
+    }
+}
\ No newline at end of file