Diff: EquatorStrutController.cpp

Revision:

2:3976e3f43470

Parent:

1:580fded7b5b2

diff -r 580fded7b5b2 -r 3976e3f43470 EquatorStrutController.cpp
--- a/EquatorStrutController.cpp	Tue Jul 29 11:12:10 2014 +0000
+++ b/EquatorStrutController.cpp	Wed Aug 20 08:34:59 2014 +0000
@@ -2,33 +2,99 @@
 
 EquatorStrut::EquatorStrut()
 {
-    PinState = 0;
-    FullWavePeriod = 0;
-    PartWavePeriod = 0;
-    position = 0.0;
+    position = 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);
+    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);
     
-    InputReadTick.attach_us(this, &EquatorStrut::InputRead, 20);
+    (*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;
+    }
     
-    SinInterruptInterval.start();
+    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)
@@ -38,18 +104,17 @@
         return;
     }
     
-    if (power > 1.0 || power < -1.0)
-    {
-        return;
-    }
+    if (fabs(power) > 1.0) return;
     
-    *PhaseA = (power + 1.0) / 2;
-    *PhaseB = 1.0 - ((power + 1.0) / 2);
+    currentPower = power;
+    
+    (*PhaseA) = (power + 1.0) / 2;
+    (*PhaseB) = 1.0 - ((power + 1.0) / 2);
 }
 
 double EquatorStrut::GetPosition()
 {
-    return position;
+    return position * 0.01;
 }
 
 void EquatorStrut::Home()
@@ -59,30 +124,47 @@
         Enable();
     }
     
-    Homing = true;
-    
-    SetPower(-1.0);
-    
-    while (!HallTriggered)
+    if ((*HallSensorState) == 1)
     {
-        wait(0.5);
+        DisableInterrupts();
+        
+        direction = -1;
+        
+        Homing = true;
+        HallTriggered = false;
+        
+        SetPower(-0.6);
+        
+        while (!HallTriggered)
+        {
+            wait(0.1);
+        }
+        
+        EnableInterrupts();
     }
     
     SetPower(1.0);
-    
-    while (position < 20.0)
+        
+    while (position < 2000)
     {
-        
+        //SerialTransmit();
     }
     
     Homing = true;
+    HallTriggered = false;
     
-    SetPower(-0.5);
+    DisableInterrupts();
+    
+    direction = -1;
+    
+    SetPower(-0.4);
     
     while (!HallTriggered)
     {
-        wait(0.5);
+        wait(0.1);
     }
+    
+    EnableInterrupts();
 }
 
 void EquatorStrut::Enable()
@@ -105,48 +187,38 @@
 
 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;
-    }
+    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::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::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()