Revision 0:a6ade00ff41d, committed 2014-07-29

Comitter:

pyrostew

Date:

Tue Jul 29 08:44:30 2014 +0000

Child:

1:580fded7b5b2

Commit message:

Equator Strut Controller with position monitoring. No Speed feedback.

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EquatorStrutController.h	Tue Jul 29 08:44:30 2014 +0000
@@ -0,0 +1,52 @@
+#ifndef EQUATOR_STRUT_CONTROLLER_H
+#define EQUATOR_STRUT_CONTROLLER_H
+
+#include "mbed.h"
+
+class EquatorStrut
+{
+public:
+    EquatorStrut();
+    void SetPower(double power);
+    double GetPosition();
+    double CurrentSpeed();
+    void Home();
+    void Enable();
+    void Disable();
+    
+private:
+    Timer SinInterruptInterval;
+    Ticker InputReadTick;
+    
+    InterruptIn* HallSensor;
+    
+    PwmOut* PhaseA;
+    PwmOut* PhaseB;
+    
+    DigitalIn* RGHSin;
+    DigitalIn* RGHCos;
+    
+    DigitalOut* ResetLine;
+    DigitalOut* PulseOut1;
+    DigitalOut* PulseOut2;
+      
+    bool Valid();
+    
+    void InputRead();
+    void HallEffectFall();
+    
+    char PinState;
+    
+    int FullWavePeriod;
+    int PartWavePeriod;
+    
+    double position;
+    
+    int direction;
+    
+    bool Homing;
+    bool HallTriggered;
+    bool Enabled;
+};
+
+#endif
\ No newline at end of file