Revision 17:f54cdc9ae52f, committed 2014-08-14

Comitter:

pyrostew

Date:

Thu Aug 14 09:20:50 2014 +0000

Parent:

16:47d761226df6

Child:

18:ab282713f4a7

Commit message:

Working encoder readings with updated speed code.

Changed in this revision

--- a/main.cpp	Wed Aug 13 08:47:02 2014 +0000
+++ b/main.cpp	Thu Aug 14 09:20:50 2014 +0000
@@ -1,8 +1,6 @@
 #include "mbed.h"
 #include "RawSerial.h"
 
-DigitalIn RGHSinState(P0_11);
-DigitalIn RGHCosState(P0_12);
 DigitalIn HallSensorState(P0_2);
 InterruptIn RGHSinInterrupt(P0_11);
 InterruptIn RGHCosInterrupt(P0_12);
@@ -10,6 +8,7 @@
 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;
@@ -20,85 +19,83 @@
 
 RawSerial pc(P1_27, P1_26);
 
-volatile char PinState = 0;
-
-volatile int direction = 0;
-volatile double position = 0.0;
+volatile int direction = 1;
+volatile int position = 0;
 double currentPower = 0.0;
-volatile int lastTime = 0;
+int lastTime = 0;
 
-const int arraySize = 50;
-volatile int intteruptPeriodArray[arraySize];
-volatile int arrayTotal = 0;
-volatile char arrayPos = 0;
-volatile int interruptPeriod = 0;
+double SpeedInterval = 0.0;
+int LastPosition = 0;
 
 char counter = 0;
 
 volatile bool SinHigh = false;
 volatile bool CosHigh = false;
+volatile bool LastSin = false;
+volatile bool LastHigh = false;
 
-void SmoothingAdd(int input)
-{
-    intteruptPeriodArray[arrayPos] = input * direction;
+void ActionEvent(bool CurrHigh, bool CurrSin)
+{    
+    // Same event again - DO NOTHING 
+    if (CurrHigh == LastHigh && CurrSin == LastSin)
+    {
+        return;
+    }
     
-    if (arrayPos == arraySize-1)
+    if (CurrSin != LastSin) // Otherwave
     {
-        arrayPos = 0;
+        // Other wave
+        if ((CurrSin && CurrHigh == LastHigh) || 
+            (!CurrSin && CurrHigh != LastHigh))
+        {
+            //Forwards
+            direction = 1;
+        }
+        else
+        {
+            //Backwards
+            direction = -1;
+        }
+        
+        
     }
     else
     {
-        arrayPos++;
-    }
-}
-
-int SmoothedInterruptPeriod()
-{
-    int arrayTotal = 0;
-    
-    for (char i = 0; i < arraySize; i++)
-    {
-        arrayTotal += intteruptPeriodArray[i];
+        // Reversal
+        direction = -direction;
     }
     
-    return arrayTotal / arraySize;
+    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()
 {
-    SinHigh = true;
+    PulseOut = 1;
+    ActionEvent(true, true);
+    PulseOut = 0;
 }
 
 void RGHSinFallingHandler()
 {
-    SinHigh = false; 
+    ActionEvent(false, true);
 }
 
 void RGHCosRisingHandler()
-{
-    if (CosHigh)
-    {        
-        return;
-    }
-    else if (SinHigh)
-    {    
-        direction = -1;
-    }
-    else
-    {
-        direction = 1;
-    }
-    
-    position += 0.04 * direction;
-    SmoothingAdd(RunningTime.read_us() - lastTime);
-    lastTime = RunningTime.read_us();
-    
-    CosHigh = true;
+{    
+    ActionEvent(true, false);
 }
 
 void RGHCosFallingHandler()
 {
-    CosHigh = false;
+    ActionEvent(false, false);
 }
 
 void SetPower(double power)
@@ -136,52 +133,15 @@
     Enabled = false;
 }
 
-void Home()
-{
-    if (!Enabled)
-    {
-        Enable();
-    }
-    
-    if (HallSensorState == 1)
-    {
-        Homing = true;
-        HallTriggered = false;
-        
-        SetPower(-0.6);
-        
-        while (!HallTriggered)
-        {
-            wait(0.5);
-        }
-    }
-    
-    SetPower(1.0);
-    
-    while (position < 20.0)
-    {
-        
-    }
-    
-    Homing = true;
-    HallTriggered = false;
-    
-    SetPower(-0.4);
-    
-    while (!HallTriggered)
-    {
-        wait(0.5);
-    }
-}
-
 double GetSpeed()
 {
-    //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0)
-    //{
-    //    return 0.0;
-    //}
-    return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
-    //return SmoothedInterruptPeriod();
+    double interval = RunningTime.read() - SpeedInterval;
+    int positionDiff = position - LastPosition;
+    
+    SpeedInterval = RunningTime.read();
+    LastPosition = position;
+    
+    return (positionDiff * 0.01)/interval;
 }
 
 double PosError = 0;    //This has been defined here as it's being used in the serial transmit function
@@ -255,7 +215,7 @@
 double PosKiGain = 0.0;
 double PosKdGain = 0.0;
 
-double VelKpGain = 0.0;
+double VelKpGain = 0.01;
 double VelKiGain = 0.0;
 double VelKdGain = 0.0;
 
@@ -265,7 +225,7 @@
     
     pc.putc('\t');   
     
-    SerialOut(position);
+    SerialOut((double)position*0.01);
     
     pc.putc('\t');
         
@@ -295,6 +255,44 @@
     pc.putc(13);
 }
 
+void Home()
+{
+    if (!Enabled)
+    {
+        Enable();
+    }
+    
+    if (HallSensorState == 1)
+    {
+        Homing = true;
+        HallTriggered = false;
+        
+        SetPower(-0.6);
+        
+        while (!HallTriggered)
+        {
+            wait(0.5);
+        }
+    }
+    
+    SetPower(1.0);
+        
+    while (position < 2000)
+    {
+        //SerialTransmit();
+    }
+    
+    Homing = true;
+    HallTriggered = false;
+    
+    SetPower(-0.4);
+    
+    while (!HallTriggered)
+    {
+        wait(0.5);
+    }
+}
+
 void HallEffectFall()
 {    
     RGHSinInterrupt.disable_irq();
@@ -364,7 +362,7 @@
         double integral_velmax = vMax/VelKiGain;
         double integral_velmin = -vMax/VelKiGain ;
            
-        PosError = SetPoint - position;
+        PosError = SetPoint - (position * 0.01);
 
         PosProError = PosError * PosKpGain;
 
@@ -454,6 +452,9 @@
     HallSensor.fall(&HallEffectFall);
     HallSensor.mode(PullUp);
     
+    RGHSinFallingInterrupt.mode(PullNone);
+    RGHCosFallingInterrupt.mode(PullNone);
+    
     RunningTime.start();
     
     pc.baud(115200);
@@ -465,37 +466,32 @@
     PosPreviousError[errorcounter]=0;
     
     PreviousTime = RunningTime.read_us();
+    
+    wait(5.0);
 
-    /*while(Enabled)
+    while(Enabled)
     {
-        //double pow = 0.0;
+        //double pow = 0.4;
+        //while(pow < 1.0)
         while(PosKpGain < 1.0)
         {
+            //pow += 0.05;
             PosKpGain += 0.01;
-            while(PosKiGain < 1.0)
-            {
-                PosKiGain += 0.01;
-                
-                while(PosKdGain < 1.0)
-                {
-                    PosKdGain += 0.01;
                     
-                    float iterationStart = RunningTime.read();
-                    
-                    while(RunningTime.read()-iterationStart < 10.0)
-                    {
-                        SerialTransmit();
-                        
-                        Controller();
-                    }
-                }
+            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