Diff: main.cpp

Revision:

15:cd409a54ceec

Parent:

12:814db1249a19

Child:

16:47d761226df6

--- a/main.cpp	Wed Aug 13 06:43:02 2014 +0000
+++ b/main.cpp	Wed Aug 13 08:45:00 2014 +0000
@@ -6,6 +6,8 @@
 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);
 PwmOut PhaseA(P0_9);
@@ -33,10 +35,11 @@
 
 char counter = 0;
 
+volatile bool SinHigh = false;
+volatile bool CosHigh = false;
+
 void SmoothingAdd(int input)
 {
-    //arrayTotal -= intteruptPeriodArray[arrayPos];
-    //arrayTotal += input;
     intteruptPeriodArray[arrayPos] = input * direction;
     
     if (arrayPos == arraySize-1)
@@ -47,8 +50,6 @@
     {
         arrayPos++;
     }
-    
-    //interruptPeriod = arrayTotal / 15;
 }
 
 int SmoothedInterruptPeriod()
@@ -63,54 +64,41 @@
     return arrayTotal / arraySize;
 }
 
-void RGHSinHandler()
-{    
-    if (PinState == 2)
+void RGHSinRisingHandler()
+{
+    SinHigh = true;
+}
+
+void RGHSinFallingHandler()
+{
+    SinHigh = false; 
+}
+
+void RGHCosRisingHandler()
+{
+    if (CosHigh)
     {        
         return;
     }
-    else if (PinState == 1)
-    {
-        PinState = 0 |(RGHSinState << 1) | RGHCosState;
-        
-        if(PinState == 3)
-        {
-            direction = 1;
-            position += 0.04 * direction;
-            SmoothingAdd(RunningTime.read_us() - lastTime);
-            interruptPeriod = RunningTime.read_us() - lastTime;
-            lastTime = RunningTime.read_us();
-        }
+    else if (SinHigh)
+    {    
+        direction = -1;
     }
     else
     {
-        PinState = 0 |(RGHSinState << 1) | RGHCosState;
+        direction = 1;
     }
+    
+    position += 0.04 * direction;
+    SmoothingAdd(RunningTime.read_us() - lastTime);
+    lastTime = RunningTime.read_us();
+    
+    CosHigh = true;
 }
 
-void RGHCosHandler()
-{    
-    if (PinState == 1)
-    {        
-        return;
-    }
-    else if (PinState == 2)
-    {
-        PinState = 0 |(RGHSinState << 1) | RGHCosState;
-        
-        if (PinState == 3)
-        {        
-            direction = -1;
-            position += 0.04 * direction;
-            SmoothingAdd(RunningTime.read_us() - lastTime);
-            interruptPeriod = RunningTime.read_us() - lastTime;
-            lastTime = RunningTime.read_us();
-        }
-    }
-    else
-    {
-        PinState = 0 |(RGHSinState << 1) | RGHCosState;
-    }
+void RGHCosFallingHandler()
+{
+    CosHigh = false;
 }
 
 void SetPower(double power)
@@ -306,6 +294,8 @@
 {    
     RGHSinInterrupt.disable_irq();
     RGHCosInterrupt.disable_irq();
+    RGHSinFallingInterrupt.disable_irq();
+    RGHCosFallingInterrupt.disable_irq();
     
     if (direction < 0)
     {        
@@ -320,6 +310,8 @@
     }
     RGHSinInterrupt.enable_irq();
     RGHCosInterrupt.enable_irq();
+    RGHSinFallingInterrupt.enable_irq();
+    RGHCosFallingInterrupt.enable_irq();
 }
 
 double SetPoint = 50.0; //Millimeter per second
@@ -386,8 +378,10 @@
 int main()
 
 {
-    RGHSinInterrupt.rise(&RGHSinHandler);
-    RGHCosInterrupt.rise(&RGHCosHandler);
+    RGHSinInterrupt.rise(&RGHSinRisingHandler);
+    RGHCosInterrupt.rise(&RGHCosRisingHandler);
+    RGHSinFallingInterrupt.fall(&RGHSinFallingHandler);
+    RGHCosFallingInterrupt.fall(&RGHCosFallingHandler);
     HallSensor.fall(&HallEffectFall);
     HallSensor.mode(PullUp);
     
@@ -403,7 +397,7 @@
     
     previousTime = RunningTime.read_us();
 
-    while(Enabled)
+    /*while(Enabled)
     {
         //double pow = 0.0;
         while(KpGain < 1.0)
@@ -432,7 +426,7 @@
         }
         
         Disable();
-    }
+    }*/
 }
 
 /* Change this throttle value range to 1.0 and 0.0 for car speed