Diff: src/SimpleTLE5206.cpp

Revision:

5:bfc5c5cc161e

Parent:

2:c5fbe0cb8a97

--- a/src/SimpleTLE5206.cpp	Tue Jul 05 14:55:29 2011 +0000
+++ b/src/SimpleTLE5206.cpp	Tue Jul 05 16:08:38 2011 +0000
@@ -28,7 +28,6 @@
 
 SimpleTLE5206::SimpleTLE5206(SimpleTLE5206Output *in1, SimpleTLE5206Output *in2)
 {
-    // Start initially as GPIO and both on (no drive, no speed).
     _in1 = in1;
     _in2 = in2;
     init(0);
@@ -36,15 +35,17 @@
 
 SimpleTLE5206::SimpleTLE5206(SimpleTLE5206Output *in1, SimpleTLE5206Output *in2, int duty_cycle_hz)
 {
-    // Start initially as GPIO and both on (no drive, no speed).
     _in1 = in1;
     _in2 = in2;
+    if (duty_cycle_hz == 0) error("Division by zero!\n");
     init((uint32_t)(12000000UL / duty_cycle_hz * 2));
 }
 
 void
 SimpleTLE5206::init(uint32_t duty)
 {
+    _in1->as_pwm();
+    _in2->as_pwm();
     if (duty != 0) setDuty(duty);
     setSpeed(0.0);
 }
@@ -55,7 +56,7 @@
 
     _duty = duty;
     
-    // Skip the setup if teh duty has already been set
+    // Skip the setup if the duty has already been set
     // by a previous instance of controller.
     if (LPC_PWM1->MR0 == duty) {
         return;
@@ -79,61 +80,53 @@
     LPC_PWM1->MR5   = 0;
     LPC_PWM1->MR6   = 0;
         
-    LPC_PWM1->MCR   = 2; // MR0 resets TC.
-        
+    LPC_PWM1->MCR   = 2; // MR0 resets TC.        
     LPC_PWM1->TCR   = 9; // Enable.        
 }
 
 void
 SimpleTLE5206::setSpeed(double speed)
 {
-    uint32_t value;
+    uint32_t value1, value2;
 
     // Ensure we cap the speed to +/-1.0
     if (speed > +1.0) speed = +1.0;
     if (speed < -1.0) speed = -1.0;
+
+    value1 = _duty + 1; // Output always on.
+    value2 = _duty + 1; // Output always on.
         
-    if (speed == 0) {
-        _in1->as_gpio();
-        _in2->as_gpio();
-        _in1->write(1);    
-        _in2->write(1);    
-    }
-    else {
+    if (speed != 0.0) {
         if (speed > 0.0) {
-            // Check the outputs are configured for the direction requested. 
-            if (_in1->get_pin_type() != SimpleTLE5206Output::IsGPIO) _in1->as_gpio();                
-            if (_in2->get_pin_type() != SimpleTLE5206Output::IsPWM)  _in2->as_pwm();                
+            value2 = (uint32_t)(speed * _duty);       // Scale for requested speed.
+            if (value2 >= _duty) value2 = _duty - 1;  // Don't allow the value to overrun the duty.
+            value2 = _duty - value2;                  // Invert logic sense.
         }
         
         if (speed < 0.0) {
-            // Check the outputs are configured for the direction requested. 
-            if (_in1->get_pin_type() != SimpleTLE5206Output::IsPWM)  _in1->as_pwm();                
-            if (_in2->get_pin_type() != SimpleTLE5206Output::IsGPIO) _in2->as_gpio();  
             speed *= -1; // invert sign.              
+            value1 = (uint32_t)(speed * _duty);       // Scale for requested speed.
+            if (value1 >= _duty) value1 = _duty - 1;  // Don't allow the value to overrun the duty.
+            value1 = _duty - value1;                  // Invert logic sense.
         }
+    }
     
-        value = (uint32_t)(speed * _duty);      // Scale for requested speed.
-        if (value >= _duty) value = _duty - 1;  // Don't allow the value to overrun the duty.
-        value = _duty - value;                  // Invert logic sense.
+    switch(_in1->getPin()) {
+        case p21:            setMRx(Pwm6, value1); break;
+        case p22:            setMRx(Pwm5, value1); break;
+        case p23: case LED4: setMRx(Pwm4, value1); break;
+        case p24: case LED3: setMRx(Pwm3, value1); break;
+        case p25: case LED2: setMRx(Pwm2, value1); break;
+        case p26: case LED1: setMRx(Pwm1, value1); break;                          
+    }
         
-        switch(_in1->getPin()) {
-            case p21:            setMRx(Pwm6, value); break;
-            case p22:            setMRx(Pwm5, value); break;
-            case p23: case LED4: setMRx(Pwm4, value); break;
-            case p24: case LED3: setMRx(Pwm3, value); break;
-            case p25: case LED2: setMRx(Pwm2, value); break;
-            case p26: case LED1: setMRx(Pwm1, value); break;                          
-        }
-        
-        switch(_in2->getPin()) {
-            case p21:            setMRx(Pwm6, value); break;
-            case p22:            setMRx(Pwm5, value); break;
-            case p23: case LED4: setMRx(Pwm4, value); break;
-            case p24: case LED3: setMRx(Pwm3, value); break;
-            case p25: case LED2: setMRx(Pwm2, value); break;
-            case p26: case LED1: setMRx(Pwm1, value); break;                          
-        }
+    switch(_in2->getPin()) {
+        case p21:            setMRx(Pwm6, value2); break;
+        case p22:            setMRx(Pwm5, value2); break;
+        case p23: case LED4: setMRx(Pwm4, value2); break;
+        case p24: case LED3: setMRx(Pwm3, value2); break;
+        case p25: case LED2: setMRx(Pwm2, value2); break;
+        case p26: case LED1: setMRx(Pwm1, value2); break;                          
     }
 }