Diff: main.cpp

Revision:

25:0e4bde9e1adc

Parent:

24:214f2d426484

Child:

26:5e4b329defec

--- a/main.cpp	Fri Aug 15 14:25:56 2014 +0000
+++ b/main.cpp	Mon Aug 18 13:24:30 2014 +0000
@@ -114,9 +114,9 @@
         return;
     }
     
-    if (power > 1.0 || power < -1.0)
+    //if (power > 1.0 || power < -1.0)
     {
-        return;
+    //    return;
     }
     
     // Linear clamping
@@ -124,13 +124,20 @@
     // Compute the corrected pwm value to linearise the velocity profile
     double correctedPwm;
     
-    if (fabs(power) < pLinStep)
+    if (fabs(power) < 1.0)
     {
-        correctedPwm = (fabs(power) * Glin) / G1;  
+        if (fabs(power) < pLinStep)
+        {
+            correctedPwm = (fabs(power) * Glin) / G1;  
+        }
+        else
+        {
+            correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2;   
+        }
     }
     else
     {
-        correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2;   
+        correctedPwm = 1.0;
     }
     
     // Make sure our corrected value has the correct sign.
@@ -309,6 +316,22 @@
     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)
@@ -318,15 +341,21 @@
     
     if (HallSensorState == 1)
     {
+        DisableInterrupts();
+        
+        direction = -1;
+        
         Homing = true;
         HallTriggered = false;
         
-        SetPower(-0.6);
+        SetPower(-0.2);
         
         while (!HallTriggered)
         {
             wait(0.1);
         }
+        
+        EnableInterrupts();
     }
     
     SetPower(1.0);
@@ -339,21 +368,22 @@
     Homing = true;
     HallTriggered = false;
     
-    SetPower(-0.4);
+    DisableInterrupts();
+    
+    direction = -1;
+    
+    SetPower(-0.1);
     
     while (!HallTriggered)
     {
         wait(0.1);
     }
+    
+    EnableInterrupts();
 }
 
 void HallEffectFall()
 {    
-    RGHSinInterrupt.disable_irq();
-    RGHCosInterrupt.disable_irq();
-    RGHSinFallingInterrupt.disable_irq();
-    RGHCosFallingInterrupt.disable_irq();
-    
     if (direction < 0)
     {        
         SetPower(0.0);
@@ -365,10 +395,6 @@
             position = 0.0;
         }
     }
-    RGHSinInterrupt.enable_irq();
-    RGHCosInterrupt.enable_irq();
-    RGHSinFallingInterrupt.enable_irq();
-    RGHCosFallingInterrupt.enable_irq();
 }