Revision 12:814db1249a19, committed 2014-08-13

Comitter:

pyrostew

Date:

Wed Aug 13 06:43:02 2014 +0000

Parent:

11:b6958b3dbddf

Child:

13:18c376e5dc9a

Child:

15:cd409a54ceec

Commit message:

Changed various things in the homing routine and in the averaging routine.

Changed in this revision

--- a/main.cpp	Tue Aug 12 06:47:01 2014 +0000
+++ b/main.cpp	Wed Aug 13 06:43:02 2014 +0000
@@ -3,6 +3,7 @@
 
 DigitalIn RGHSinState(P0_11);
 DigitalIn RGHCosState(P0_12);
+DigitalIn HallSensorState(P0_2);
 InterruptIn RGHSinInterrupt(P0_11);
 InterruptIn RGHCosInterrupt(P0_12);
 InterruptIn HallSensor(P0_2);
@@ -12,44 +13,54 @@
 Timer RunningTime;
 
 bool Enabled = false;
-bool Homing = false;
-bool HallTriggered = false;
+volatile bool Homing = false;
+volatile bool HallTriggered = false;
 
 RawSerial pc(P1_27, P1_26);
 
-char PinState = 0;
+volatile char PinState = 0;
 
-int direction = 0;
-double position = 0.0;
+volatile int direction = 0;
+volatile double position = 0.0;
 double currentPower = 0.0;
-int interruptPeriod = 0;
-int lastTime = 0;
+volatile int lastTime = 0;
 
-int intteruptPeriodArray[15];
-int arrayTotal = 0;
-char arrayPos = 0;
+const int arraySize = 50;
+volatile int intteruptPeriodArray[arraySize];
+volatile int arrayTotal = 0;
+volatile char arrayPos = 0;
+volatile int interruptPeriod = 0;
 
 char counter = 0;
 
 void SmoothingAdd(int input)
 {
-    arrayTotal -= intteruptPeriodArray[arrayPos];
-    arrayTotal += input;
-    intteruptPeriodArray[arrayPos] = input;
+    //arrayTotal -= intteruptPeriodArray[arrayPos];
+    //arrayTotal += input;
+    intteruptPeriodArray[arrayPos] = input * direction;
     
-    if (arrayPos == 14)
+    if (arrayPos == arraySize-1)
     {
         arrayPos = 0;
     }
     else
     {
-        arrayTotal++;
+        arrayPos++;
     }
+    
+    //interruptPeriod = arrayTotal / 15;
 }
 
 int SmoothedInterruptPeriod()
 {
-    return arrayTotal / 15;
+    int arrayTotal = 0;
+    
+    for (char i = 0; i < arraySize; i++)
+    {
+        arrayTotal += intteruptPeriodArray[i];
+    }
+    
+    return arrayTotal / arraySize;
 }
 
 void RGHSinHandler()
@@ -67,6 +78,7 @@
             direction = 1;
             position += 0.04 * direction;
             SmoothingAdd(RunningTime.read_us() - lastTime);
+            interruptPeriod = RunningTime.read_us() - lastTime;
             lastTime = RunningTime.read_us();
         }
     }
@@ -91,6 +103,7 @@
             direction = -1;
             position += 0.04 * direction;
             SmoothingAdd(RunningTime.read_us() - lastTime);
+            interruptPeriod = RunningTime.read_us() - lastTime;
             lastTime = RunningTime.read_us();
         }
     }
@@ -142,14 +155,17 @@
         Enable();
     }
     
-    Homing = true;
-    HallTriggered = false;
-    
-    SetPower(-1.0);
-    
-    while (!HallTriggered)
+    if (HallSensorState == 1)
     {
-        wait(0.5);
+        Homing = true;
+        HallTriggered = false;
+        
+        SetPower(-0.6);
+        
+        while (!HallTriggered)
+        {
+            wait(0.5);
+        }
     }
     
     SetPower(1.0);
@@ -162,7 +178,7 @@
     Homing = true;
     HallTriggered = false;
     
-    SetPower(-0.7);
+    SetPower(-0.4);
     
     while (!HallTriggered)
     {
@@ -172,280 +188,115 @@
 
 double GetSpeed()
 {
-    if ((RunningTime - lastTime) > 10000 || interruptPeriod == 0)
-    {
-        return 0.0;
-    }
-    return (direction * 0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
+    //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0)
+    //{
+    //    return 0.0;
+    //}
+    return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
+    //return SmoothedInterruptPeriod();
 }
 
 double Error = 0;
 
-void SerialTransmit()
-{        
-    double tempPos = position;
-    double tempTime = RunningTime.read();
-    double tempPow = currentPower;
-    double tempSpeed = GetSpeed();
-    double tempError = Error;
+void SerialOut(double outputValue)
+{
     int outChar = 0;
     
-    if (tempPos < 0.0)
+    if (outputValue < 0.0)
     {
         pc.putc('-');
-        tempPos *= -1;
+        outputValue *= -1.0;
     }
-    if (tempPos >= 100.0)
+    if (outputValue >= 100.0)
     {
-        outChar = tempPos / 100;
+        outChar = outputValue / 100;
         pc.putc(outChar + 48);
-        tempPos -= outChar * 100.0;
+        outputValue -= outChar * 100.0;
     }
-    if (tempPos >= 10.0)
+    if (outputValue >= 10.0)
     {
-        outChar = tempPos / 10;
+        outChar = outputValue / 10;
         pc.putc(outChar + 48);
-        tempPos -= outChar * 10.0;
+        outputValue -= outChar * 10.0;
     }
     else if(outChar > 0)
     {
         pc.putc('0');
     }
-    if (tempPos >= 1.0)
+    if (outputValue >= 1.0)
     {
-        outChar = tempPos;
+        outChar = outputValue;
         pc.putc(outChar + 48);
-        tempPos -= outChar;
+        outputValue -= outChar;
     }
     else
     {
         pc.putc('0');
     }
-    if (tempPos >= 0.1)
-    {
-        pc.putc('.');
-        outChar = tempPos * 10;
-        pc.putc(outChar + 48);
-        tempPos -= (double)outChar / 10.0;
-    }
-    else
+    if (outputValue >= 0.1)
     {
         pc.putc('.');
-        pc.putc('0');
-    }
-    if (tempPos >= 0.01)
-    {
-        outChar = tempPos * 100;
-        pc.putc(outChar + 48);
-    }
-    
-    pc.putc('\t');
-    
-    outChar = 0;
-    
-    if (tempTime >= 100.0)
-    {
-        outChar = tempTime / 100;
-        pc.putc(outChar + 48);
-        tempTime -= outChar * 100.0;
-    }
-    if (tempTime >= 10.0)
-    {
-        outChar = tempTime / 10;
+        outChar = outputValue * 10;
         pc.putc(outChar + 48);
-        tempTime -= outChar * 10.0;
-    }
-    else if(outChar > 0)
-    {
-        pc.putc('0');
-    }
-    if (tempTime >= 1.0)
-    {
-        outChar = tempTime;
-        pc.putc(outChar + 48);
-        tempTime -= outChar;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (tempTime >= 0.1)
-    {
-        pc.putc('.');
-        outChar = tempTime * 10;
-        pc.putc(outChar + 48);
-        tempTime -= (double)outChar / 10.0;
+        outputValue -= (double)outChar / 10.0;
     }
     else
     {
         pc.putc('.');
         pc.putc('0');
     }
-    if (tempTime >= 0.01)
-    {
-        outChar = tempTime * 100;
-        pc.putc(outChar + 48);
-        tempTime -= (double)outChar / 100.0;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (tempTime >= 0.001)
-    {
-        outChar= tempTime * 1000;
-        pc.putc(outChar + 48);
-    }
-    
-    pc.putc('\t');
-    outChar = 0;
-    
-    if (tempPow < 0.0)
-    {
-        pc.putc('-');
-        tempPow *= -1;
-    }
-    if (tempPow >= 1.0)
+    if (outputValue >= 0.01)
     {
-        outChar = tempPow;
-        pc.putc(outChar + 48);
-        tempPow -= outChar;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (tempPow >= 0.1)
-    {
-        pc.putc('.');
-        outChar = tempPow * 10;
+        outChar = outputValue * 100;
         pc.putc(outChar + 48);
-        tempPow -= (double)outChar / 10.0;
-    }
-    else
-    {
-        pc.putc('.');
-        pc.putc('0');
-    }
-    if (tempPow >= 0.01)
-    {
-        outChar = tempPow * 100;
-        pc.putc(outChar + 48);
-        tempPow -= (double)outChar / 100.0;
+        outputValue -= (double)outChar / 100.0;
     }
     else
     {
         pc.putc('0');
     }
-    if (tempPow >= 0.001)
+    if (outputValue >= 0.001)
     {
-        outChar= tempPow * 1000;
+        outChar= outputValue * 1000;
         pc.putc(outChar + 48);
     }
+}
+
+double KpGain = 0.0;
+double KiGain = 0.0;
+double KdGain = 0.0;
+
+void SerialTransmit()
+{    
+    SerialOut(RunningTime.read()); 
+    
+    pc.putc('\t');   
+    
+    SerialOut(position);
     
     pc.putc('\t');
-    outChar = 0;
+        
+    SerialOut(currentPower);
     
-    if (tempSpeed < 0.0)
-    {
-        pc.putc('-');
-        tempSpeed *= -1;
-    }
-    if (tempSpeed >= 100.0)
-    {
-        outChar = tempSpeed / 100;
-        pc.putc(outChar + 48);
-        tempSpeed -= outChar * 100.0;
-    }
-    if (tempSpeed >= 10.0)
-    {
-        outChar = tempSpeed / 10;
-        pc.putc(outChar + 48);
-        tempSpeed -= outChar * 10.0;
-    }
-    else if(outChar > 0)
-    {
-        pc.putc('0');
-    }
-    if (tempSpeed >= 1.0)
-    {
-        outChar = tempSpeed;
-        pc.putc(outChar + 48);
-        tempSpeed -= outChar;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (tempSpeed >= 0.1)
-    {
-        pc.putc('.');
-        outChar = tempSpeed * 10;
-        pc.putc(outChar + 48);
-        tempSpeed -= (double)outChar / 10.0;
-    }
-    else
-    {
-        pc.putc('.');
-        pc.putc('0');
-    }
-    if (tempSpeed >= 0.01)
-    {
-        outChar = tempSpeed * 100;
-        pc.putc(outChar + 48);
-    }
+    pc.putc('\t');
+    
+    SerialOut(GetSpeed());
     
     pc.putc('\t');
-    outChar = 0;
+    
+    SerialOut(Error);
+    
+    pc.putc('\t');
+    
+    SerialOut(KpGain);
     
-    if (tempError < 0.0)
-    {
-        pc.putc('-');
-        tempError *= -1;
-    }
-    if (tempError >= 100.0)
-    {
-        outChar = tempError / 100;
-        pc.putc(outChar + 48);
-        tempError -= outChar * 100.0;
-    }
-    if (tempError >= 10.0)
-    {
-        outChar = tempError / 10;
-        pc.putc(outChar + 48);
-        tempError -= outChar * 10.0;
-    }
-    else if(outChar > 0)
-    {
-        pc.putc('0');
-    }
-    if (tempError >= 1.0)
-    {
-        outChar = tempError;
-        pc.putc(outChar + 48);
-        tempError -= outChar;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (tempError >= 0.1)
-    {
-        pc.putc('.');
-        outChar = tempError * 10;
-        pc.putc(outChar + 48);
-        tempError -= (double)outChar / 10.0;
-    }
-    else
-    {
-        pc.putc('.');
-        pc.putc('0');
-    }
-    if (tempError >= 0.01)
-    {
-        outChar = tempError * 100;
-        pc.putc(outChar + 48);
-    }
+    pc.putc('\t');
+    
+    SerialOut(KiGain);
+    
+    pc.putc('\t');
+    
+    SerialOut(KdGain);
     
     pc.putc(10);
     pc.putc(13);
@@ -477,10 +328,6 @@
 double ErrorInt;
 double ErrorDer;
 
-double KpGain = 0.0;
-double KiGain = 0.0;
-double KdGain = 0.0;
-
 int errorcounter;
 double PreviousError [10];
 
@@ -492,49 +339,49 @@
 int previousTime = 0;
 
 void Controller ()
+{
+    if (position <= SetPoint || 1)
     {
-         if (position <= SetPoint || 1)
-            {
-                int timeStep = RunningTime.read_us() - previousTime;
-                previousTime = RunningTime.read_us();
-            
-                Error = SetPoint - position;
-        
-                KpTerm = Error * KpGain;
-        
-                ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
-        
-                ErrorInt = ErrorInt + Error * timeStep;
-        
-                NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
-        
-                if (NewPwm > 1.0) 
-                {
-                    pwm = 1.0;
-                }
-            
-                else if (NewPwm < -1.0)
-                {
-                    pwm = -1.0;
-                }
-            
-                else
-                {
-                    pwm = NewPwm;
-                }
-            
-                SetPower(pwm);
+        int timeStep = RunningTime.read_us() - previousTime;
+        previousTime = RunningTime.read_us();
+    
+        Error = SetPoint - position;
+
+        KpTerm = Error * KpGain;
+
+        ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
+
+        ErrorInt = ErrorInt + Error * timeStep;
+
+        NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
 
-                errorcounter ++;
-        
-                if (errorcounter > 9)
-                {
-                    errorcounter = 0;   
-                }
+        if (NewPwm > 1.0) 
+        {
+            pwm = 1.0;
+        }
+    
+        else if (NewPwm < -1.0)
+        {
+            pwm = -1.0;
+        }
+    
+        else
+        {
+            pwm = NewPwm;
+        }
+    
+        SetPower(pwm);
 
-                PreviousError[errorcounter] = Error;
-            }
+        errorcounter ++;
+
+        if (errorcounter > 9)
+        {
+            errorcounter = 0;   
+        }
+
+        PreviousError[errorcounter] = Error;
     }
+}
 
 int main()
 
@@ -558,38 +405,27 @@
 
     while(Enabled)
     {
-        double pow = 0.0;
-        for (int i = 0; i < 20; i++)
+        //double pow = 0.0;
+        while(KpGain < 1.0)
         {
-            //KpGain += 0.1;
-            float iterationStart = RunningTime.read();
-            
-            pow += 0.05;
-            
-            if (pow > 1.0)
+            KpGain += 0.01;
+            while(KiGain < 1.0)
             {
-                pow = 1.0;
-            }
-            
-            //while(RunningTime.read()-iterationStart < 10.0)
-            //{
-            //    SerialTransmit();
+                KiGain += 0.01;
                 
-            //    Controller();
-            //}
-            
-            while(RunningTime.read() - iterationStart < 30.0 && position < 210.0)
-            {
-                SerialTransmit();
-                SetPower(pow);
-            }
-            
-            iterationStart = RunningTime.read();
-            
-            while(RunningTime.read() - iterationStart < 30.0 && position > 10.0)
-            {
-                SerialTransmit();
-                SetPower(-pow);
+                while(KdGain < 1.0)
+                {
+                    KdGain += 0.01;
+                    
+                    float iterationStart = RunningTime.read();
+                    
+                    while(RunningTime.read()-iterationStart < 10.0)
+                    {
+                        SerialTransmit();
+                        
+                        Controller();
+                    }
+                }
             }
             
             Home();