Revision 26:5e4b329defec, committed 2014-08-20

Comitter:

pyrostew

Date:

Wed Aug 20 08:36:08 2014 +0000

Parent:

25:0e4bde9e1adc

Child:

27:1d55ebab6214

Commit message:

Moved strut control code back into the EquatorStrutController Library and removed all none essential commented out code.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EquatorStrutController.lib	Wed Aug 20 08:36:08 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/pyrostew/code/EquatorStrutController/#3976e3f43470

--- a/main.cpp	Mon Aug 18 13:24:30 2014 +0000
+++ b/main.cpp	Wed Aug 20 08:36:08 2014 +0000
@@ -1,32 +1,11 @@
 #include "mbed.h"
 #include "RawSerial.h"
+#include "EquatorStrutController.h"
 
-DigitalIn HallSensorState(P0_2);
-InterruptIn RGHSinInterrupt(P0_11);
-InterruptIn RGHCosInterrupt(P0_12);
-InterruptIn RGHSinFallingInterrupt(P0_13);
-InterruptIn RGHCosFallingInterrupt(P0_14);
-InterruptIn HallSensor(P0_2);
-DigitalOut ResetLine(P1_29);
-DigitalOut PulseOut(P1_22);
-PwmOut PhaseA(P0_9);
-PwmOut PhaseB(P0_8);
 Timer RunningTime;
 
-bool Enabled = false;
-volatile bool Homing = false;
-volatile bool HallTriggered = false;
-
 RawSerial pc(P1_27, P1_26);
 
-volatile int direction = 1;
-volatile int position = 0;
-double currentPower = 0.0;
-int lastTime = 0;
-
-double SpeedInterval = 0.0;
-int LastPosition = 0;
-
 double vMax = 300;
 double vStep = 50;
 double pStep = 0.5;
@@ -38,89 +17,10 @@
 
 char counter = 0;
 
-volatile bool SinHigh = false;
-volatile bool CosHigh = false;
-volatile bool LastSin = false;
-volatile bool LastHigh = false;
+EquatorStrut strut;
 
-void 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 RGHSinRisingHandler()
+void LinearizePower(double power)
 {
-    PulseOut = 1;
-    ActionEvent(true, true);
-    PulseOut = 0;
-}
-
-void RGHSinFallingHandler()
-{
-    ActionEvent(false, true);
-}
-
-void RGHCosRisingHandler()
-{    
-    ActionEvent(true, false);
-}
-
-void RGHCosFallingHandler()
-{
-    ActionEvent(false, false);
-}
-
-void SetPower(double power)
-{
-    if(!Enabled)
-    {
-        return;
-    }
-    
-    //if (power > 1.0 || power < -1.0)
-    {
-    //    return;
-    }
-    
-    // Linear clamping
-    
     // Compute the corrected pwm value to linearise the velocity profile
     double correctedPwm;
     
@@ -143,53 +43,20 @@
     // Make sure our corrected value has the correct sign.
     if(power < 0) correctedPwm *= -1.0;
     
-     if (correctedPwm > 1.0) 
-        {
-            correctedPwm = 1.0;
-        }
-    
-        else if (correctedPwm < -1.0)
-        {
-            correctedPwm = -1.0;
-        }
-    
-    currentPower = correctedPwm;
-    
-    PhaseA = (correctedPwm + 1.0) / 2;
-    PhaseB = 1.0 - ((correctedPwm + 1.0) / 2);
-}
+    if (correctedPwm > 1.0) 
+    {
+        correctedPwm = 1.0;
+    }
 
-void Enable()
-{
-    SetPower(0.0);
-    
-    ResetLine = 1;
+    else if (correctedPwm < -1.0)
+    {
+        correctedPwm = -1.0;
+    }
     
-    Enabled = true;
-}
-
-void Disable()
-{
-    ResetLine = 0;
-    
-    SetPower(0.0);
-    
-    Enabled = false;
-}
-
-double GetSpeed()
-{
-    double interval = RunningTime.read() - SpeedInterval;
-    int positionDiff = position - LastPosition;
-    
-    SpeedInterval = RunningTime.read();
-    LastPosition = position;
-    
-    return (positionDiff * 0.01)/interval;
+    strut.SetPower(correctedPwm);
 }
 
 double PosError = 0;    //This has been defined here as it's being used in the serial transmit function
-//double VelError = 0;
 
 void SerialOut(double outputValue)
 {
@@ -269,25 +136,21 @@
 double PosKiGain = 0.0;
 double PosKdGain = 0.0;
 
-//double VelKpGain = 0.01;
-//double VelKiGain = 0.0;
-//double VelKdGain = 0.0;
-
 void SerialTransmit()
 {    
     SerialOut(RunningTime.read()); 
     
     pc.putc('\t');   
     
-    SerialOut((double)position*0.01);
+    SerialOut(strut.GetPosition());
     
     pc.putc('\t');
         
-    SerialOut(currentPower);
+    SerialOut(strut.CurrentPower());
     
     pc.putc('\t');
     
-    SerialOut(GetSpeed());
+    SerialOut(strut.CurrentSpeed());
     
     pc.putc('\t');
     
@@ -299,12 +162,6 @@
     
     pc.putc('\t');
     
- //   SerialOut(VelKpGain);
-    
-    //pc.putc('\t');
-    
-    //SerialOut(PosKdGain);
-    
     pc.putc(10);
     pc.putc(13);
 }
@@ -316,312 +173,148 @@
     pc.putc(13);
 }
 
-void DisableInterrupts()
-{
-    RGHSinInterrupt.disable_irq();
-    RGHCosInterrupt.disable_irq();
-    RGHSinFallingInterrupt.disable_irq();
-    RGHCosFallingInterrupt.disable_irq();
-}
-
-void EnableInterrupts()
-{
-    RGHSinInterrupt.enable_irq();
-    RGHCosInterrupt.enable_irq();
-    RGHSinFallingInterrupt.enable_irq();
-    RGHCosFallingInterrupt.enable_irq();
-}
-
-void Home()
-{
-    if (!Enabled)
-    {
-        Enable();
-    }
-    
-    if (HallSensorState == 1)
-    {
-        DisableInterrupts();
-        
-        direction = -1;
-        
-        Homing = true;
-        HallTriggered = false;
-        
-        SetPower(-0.2);
-        
-        while (!HallTriggered)
-        {
-            wait(0.1);
-        }
-        
-        EnableInterrupts();
-    }
-    
-    SetPower(1.0);
-        
-    while (position < 2000)
-    {
-        //SerialTransmit();
-    }
-    
-    Homing = true;
-    HallTriggered = false;
-    
-    DisableInterrupts();
-    
-    direction = -1;
-    
-    SetPower(-0.1);
-    
-    while (!HallTriggered)
-    {
-        wait(0.1);
-    }
-    
-    EnableInterrupts();
-}
-
-void HallEffectFall()
-{    
-    if (direction < 0)
-    {        
-        SetPower(0.0);
-    
-        if (Homing)
-        {
-            HallTriggered = true;
-            Homing = false;
-            position = 0.0;
-        }
-    }
-}
-
-
-double SetPoint = 70.0; //Target Position in Millimeter per second
+double SetPoint = 150.0; //Target Position in Millimeters
 
 double PosProError;
 double PosIntError;
 double PosDifError;
 
-//double VelProError;
-//double VelIntError;
-//double VelDifError;
-
 int errorcounter;
 double PosPreviousError [10];
-//double VelPreviousError [10];
 
 double PwmChange=0;
 
 double pwm;
 
-
 double TargetVelocity;
 
 double PosiState;
-//double VeliState;
 
 int PreviousTime = 0;
 
-
 void Controller ()
 {
-    
     ///////////////////////////////////////////////////////////////////////////////////////////////
     //Position PID
     ///////////////////////////////////////////////////////////////////////////////////////////////
-    
-    //if (position <= SetPoint || 1)
-    
+    int timeStep = RunningTime.read_us() - PreviousTime;
+    PreviousTime = RunningTime.read_us();
     
-    //{
-        int timeStep = RunningTime.read_us() - PreviousTime;
-        PreviousTime = RunningTime.read_us();
-        
-        double integral_velmax = vMax/PosKiGain;
-        double integral_velmin = -vMax/PosKiGain ;
-           
-        PosError = SetPoint - (position * 0.01);
+    double integral_velmax = vMax/PosKiGain;
+    double integral_velmin = -vMax/PosKiGain ;
+       
+    PosError = SetPoint - (strut.GetPosition());
 
-        PosProError = PosError * PosKpGain;
+    PosProError = PosError * PosKpGain;
 
-        PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;           
-        
-        PosiState += PosError;
-        
-        if (PosiState > integral_velmax)     
-        {
-            PosiState = integral_velmax;   
-        }
-        else if (PosiState < integral_velmin)   
-        {
-            PosiState = integral_velmin;
-        }
-        PosIntError = PosKiGain * PosiState;
-        
-        TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
+    PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;           
+    
+    PosiState += PosError;
+    
+    if (PosiState > integral_velmax)     
+    {
+        PosiState = integral_velmax;   
+    }
+    else if (PosiState < integral_velmin)   
+    {
+        PosiState = integral_velmin;
+    }
+    PosIntError = PosKiGain * PosiState;
+    
+    TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
 
-        
-        if (TargetPwm > 1.0) 
-        {
-            TargetPwm = 1.0;
-        }
+    
+    if (TargetPwm > 1.0) 
+    {
+        TargetPwm = 1.0;
+    }
+
+    else if (TargetPwm < -1.0)
+    {
+        TargetPwm = -1.0;
+    }
     
-        else if (TargetPwm < -1.0)
-        {
-            TargetPwm = -1.0;
-        }
-        
-        errorcounter ++;
+    strut.SetPower(TargetPwm);
+    
+    errorcounter++;
 
-        if (errorcounter > 9)
-        {
-            errorcounter = 0;   
-        }
+    if (errorcounter > 9)
+    {
+        errorcounter = 0;   
+    }
 
-        PosPreviousError[errorcounter] = PosError;
-   // }
-        
+    PosPreviousError[errorcounter] = PosError;        
 }
 
 int main()
 
-{
-    RGHSinInterrupt.rise(&RGHSinRisingHandler);
-    RGHCosInterrupt.rise(&RGHCosRisingHandler);
-    RGHSinFallingInterrupt.fall(&RGHSinFallingHandler);
-    RGHCosFallingInterrupt.fall(&RGHCosFallingHandler);
-    HallSensor.fall(&HallEffectFall);
-    HallSensor.mode(PullUp);
-    
-    RGHSinFallingInterrupt.mode(PullNone);
-    RGHCosFallingInterrupt.mode(PullNone);
-    
+{    
     RunningTime.start();
     
     pc.baud(115200);
     
-    Home();
+    strut.Home();
     //Enable();
     
- errorcounter = 0;
+    errorcounter = 0;
     PosPreviousError[errorcounter]=0;
     PreviousTime = RunningTime.read_us();
 
-    while(Enabled)
+    while(strut.IsEnabled())
     {
-        double pow = 0.0;
-        while(pow < 1.0)
-       // PosKpGain = 10.0;
-       // while(PosKpGain < 50.0)
+        int counter = 0;
+        //double pow = 0.0;
+        //while(pow < 1.0)
+        PosKpGain = 0.0;
+        while(PosKpGain < 1.0)
         {
-            pow += 0.05;
-            //PosKpGain += 1.0;
+            counter++;
             
-          //  VelKpGain = 0.003;
-          //  while (VelKpGain < 0.008)
-          //  {
-          //      VelKpGain += 0.001;
+            //pow += 0.05;
+            PosKpGain += 0.01;
+          
+            SerialNewFile();
+          
+            RunningTime.reset();
                                 
-                float iterationStart = RunningTime.read();
+            float iterationStart = RunningTime.read();
+                
+            while(RunningTime.read()-iterationStart < 10.0)
+            {
+                SerialTransmit();
+                
+                Controller();
+            }
                 
-                    
-                SetPower(pow);
-                
-                while(position < 21000 && RunningTime.read()-iterationStart < 30.0)
-                {
-                    SerialTransmit();
-                }
-                
-                iterationStart = RunningTime.read();
-                SerialNewFile();
+            //SetPower(pow);
+            
+            //while(position < 21000 && RunningTime.read()-iterationStart < 30.0)
+            //{
+            //    SerialTransmit();
+            //}
+            
+            //iterationStart = RunningTime.read();
+            //SerialNewFile();
+            
+            //SetPower(-pow);
+            
+            //while(position > 1000 && RunningTime.read()-iterationStart < 30.0)
+            //{
+            //    SerialTransmit();
+            //}
                 
-                SetPower(-pow);
-                
-                while(position > 1000 && RunningTime.read()-iterationStart < 30.0)
-                {
-                    SerialTransmit();
-                }
-                
-                SerialNewFile();
-                Home();
-          //  }
+            //SerialNewFile();
+            strut.Home();
+            
+            if (counter == 10)
+            {
+                counter = 0;
+                strut.Disable();
+                wait(60);
+                strut.Enable();
+            }
         }
         
-        Disable();
+        strut.Disable();
     }
-}
-
-
-/* Change this throttle value range to 1.0 and 0.0 for car speed
-m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
-
-            double throttle = m_throttle + m_throttlechange;
-
-            if (new_throttle > 1.0) {
-                m_throttle = 1.0;
-            } else if (new_throttle < 0.0) {
-                m_throttle = 0.0;
-            } else {
-                m_throttle = new_throttle;
-            }*/
-
-
-    /*
-    
-    
-    errorcounter = 0;
-    PosPreviousError[errorcounter]=0;
-    
-    PreviousTime = RunningTime.read_us();
-
-    while(Enabled)
-    {
-        //double pow = 0.4;
-        //while(pow < 1.0)
-        PosKpGain = 10.0;
-        while(PosKpGain < 50.0)
-        {
-            //pow += 0.05;
-            PosKpGain += 1.0;
-            
-          //  VelKpGain = 0.003;
-          //  while (VelKpGain < 0.008)
-          //  {
-          //      VelKpGain += 0.001;
-                                
-                float iterationStart = RunningTime.read();
-                
-                while(RunningTime.read()-iterationStart < 10.0)
-                {
-                    SerialTransmit();
-                    
-                    Controller();
-                }
-                
-                Home();
-          //  }
-        }
-        
-        Disable();
-    }
-}
-*/
-/* Change this throttle value range to 1.0 and 0.0 for car speed
-m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
-
-            double throttle = m_throttle + m_throttlechange;
-
-            if (new_throttle > 1.0) {
-                m_throttle = 1.0;
-            } else if (new_throttle < 0.0) {
-                m_throttle = 0.0;
-            } else {
-                m_throttle = new_throttle;
-            }*/
-
-
-
-
-
+}
\ No newline at end of file