Revision 2:d6a018232650, committed 2011-05-19

Comitter:

AjK

Date:

Thu May 19 22:46:09 2011 +0000

Parent:

1:fbb8acf19e77

Child:

3:1232505e7206

Commit message:

1.2 See CHANGELOG.h

Changed in this revision

--- a/inc/ChangeLog.h	Mon May 02 18:42:16 2011 +0000
+++ b/inc/ChangeLog.h	Thu May 19 22:46:09 2011 +0000
@@ -1,12 +1,17 @@
-/*
-
-SimpleRCservos change log history.
-==================================
-
-1.1     2/May/2011
-    * Fixed some documentation errors.
-    
-1.0     2/May/2011
-    * Initial release.
-
+/*
+
+SimpleRCservos change log history.
+==================================
+
+1.2    19/May/2011
+
+    * Added the SimpleServoControl helper class.
+      See http://mbed.org/forum/helloworld/topic/2303
+      
+1.1     2/May/2011
+    * Fixed some documentation errors.
+    
+1.0     2/May/2011
+    * Initial release.
+
 */
\ No newline at end of file

--- a/inc/SimpleRCservos.h	Mon May 02 18:42:16 2011 +0000
+++ b/inc/SimpleRCservos.h	Thu May 19 22:46:09 2011 +0000
@@ -1,247 +1,247 @@
-/*
-    Copyright (c) 2011 Andy Kirkham
- 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
- 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
- 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-#ifndef AJK_SIMPLERCSERVOS_H
-#define AJK_SIMPLERCSERVOS_H
-
-#include "LPC17xx.h"
-#include "SimpleRCmbed.h"
-
-namespace AjK {
-
-class SimpleRCservos {
-
-public:
-    enum Servo { NoServo0, Servo1, Servo2, Servo3, Servo4, Servo5, Servo6, NumOfServos  };
-    
-protected:
-    PinName _pinName;
-    
-    uint32_t _duty;
-    
-    uint32_t _mid[NumOfServos];
-    
-    double _limitMax[NumOfServos];
-    
-    double _limitMin[NumOfServos];
-    
-    uint32_t _msMin[NumOfServos];
-    
-    uint32_t _msMax[NumOfServos];
-            
-    void init(uint32_t duty);
-       
-    void setRangeMin(Servo ch, uint32_t u) { _msMin[ch] = u * 24; }
-    
-    void setRangeMax(Servo ch, uint32_t u) { _msMax[ch] = (u * 24) - _msMin[ch]; }
-        
-    void setMRx(Servo ch, uint32_t value);
-    
-    double limit(Servo ch, double degrees);
-                
-public:
-
-    /** Constructor
-     */
-    SimpleRCservos() { init(480000UL); setDuty(); }
-
-    /** setDuty
-     * 
-     * Set's the duty, or period, of the PWM pulses.
-     * The default is 20ms and the parameter passed in is the 
-     * number of "clock ticks" that represents the period.
-     * 480000 gives rise to 20ms by default. It's rare that
-     * you would alter this value. Remember, with the PWM
-     * peripheral there is only a single, common,  period for 
-     * all six PWM outputs.
-     *
-     * @param duty The number of "clock ticks" for the duty cycle.
-     */
-    void setDuty(uint32_t duty = 480000);
-    
-    /** enable1
-     * 
-     * Enable PWM channel 1.
-     *
-     * @param PinName p26(P2_0) or LED1(P1_18)
-     */
-    void enable1(PinName pin = p26);
-    
-    /** enable2
-     * 
-     * Enable PWM channel 2.
-     *
-     * @param PinName p25(P2_1) or LED2(P1_20)
-     */
-    void enable2(PinName pin = p25);
-    
-    /** enable3
-     * 
-     * Enable PWM channel 3.
-     *
-     * @param PinName p24(P2_2) or LED3(P1_21)
-     */
-    void enable3(PinName pin = p24);
-    
-    /** enable4
-     * 
-     * Enable PWM channel 4.
-     *
-     * @param PinName p23(P2_3) or LED4(P1_23)
-     */
-    void enable4(PinName pin = p23);
-    
-    /** enable5
-     *
-     * Enable PWM channel 5.
-     *
-     * @param PinName p22(P2_4) or P1_24
-     */
-    void enable5(PinName pin = p22);
-    
-    /** enable6
-     *
-     * Enable PWM channel 6.
-     *
-     * @param PinName p21(P2_5) or P1_26
-     */
-    void enable6(PinName pin = p21);
-    
-    /** enable
-     *
-     * Enable the specified channel. The default pin on port 2 is assumed.
-     *
-     * @param Servo The channel to enable, Servo1 to Servo6
-     */
-    void enable(Servo ch);
-    
-    /** setLimitMin
-     * 
-     * Set the minimum logical limit for the specified channel.
-     *
-     * @param Servo channel
-     * @param double The lower logical limit.
-     */
-    void setLimitMin(Servo ch, double d) { _limitMin[ch] = d; }
-    
-    /** getLimitMin
-     * 
-     * Get the minimum logical limit for the specified channel.
-     *
-     * @param Servo channel
-     * @return double The lower logical limit.
-     */
-    double getlimitMin(Servo ch)   { return _limitMin[ch]; }
-    
-    /** setLimitMax
-     * 
-     * Set the maximum logical limit for the specified channel.
-     *
-     * @param Servo channel
-     * @param double The upper logical limit.
-     */
-    void setLimitMax(Servo ch, double d) { _limitMax[ch] = d; }
-    
-    /** getLimitMax
-     * 
-     * Get the maximum logical limit for the specified channel.
-     *
-     * @param Servo channel
-     * @return double The upper logical limit.
-     */
-    double getLimitMax(Servo ch)   { return _limitMax[ch]; }
-    
-    /** setLimits
-     *
-     * Set the logical upper and lower limits for channel.
-     *
-     * @param Servo channel
-     * @param double The minimum logical limit
-     * @param double The maximum logical limit
-     */
-    void setLimits(Servo ch, double min, double max) { 
-        _limitMin[ch] = min; 
-        _limitMax[ch] = max; 
-    }
-    
-    /** getRangeMin
-     * 
-     * Set the minimum physical limit for the specified channel.
-     *
-     * @param Servo channel
-     * @param double The lower phyiscal limit in microseconds
-     */
-    uint32_t getRangeMin(Servo ch, uint32_t u) { return _msMin[ch] / 24; }
-    
-    /** getRangeMax
-     * 
-     * Set the maximum physical limit for the specified channel.
-     *
-     * @param Servo channel
-     * @param uint32 The upper phyiscal limit in microseconds
-     */
-    uint32_t getRangeMax(Servo ch, uint32_t u) { return (_msMax[ch] / 24) + _msMin[ch]; }
-    
-    /** setRange
-     * 
-     * Set the minimum and maximum physical limits for the specified channel.
-     *
-     * @param Servo channel
-     * @param uint32 The lower phyiscal limit in microseconds
-     * @param uint32 The upper phyiscal limit in microseconds
-     */
-    void setRange(Servo ch, uint32_t min, uint32_t max) { 
-        setRangeMin(ch, min);
-        setRangeMax(ch, max);
-    }
-    
-    /** position
-     *
-     * Set the desired position of the servo.
-     * This value should be between the lower and upper logical limits.
-     *
-     * Under normal circumstances this fuction returns the same position is 
-     * was supplied. However, if the supplied position is outside the logical
-     * system limits we return the value we actually went to. This is almost
-     * always the value of the logical limit that is set for that direction.
-     *
-     * @param Servo channel
-     * @param double the logical position to move to.
-     * @return double The actual calculated position moved to.
-     */
-    double position(Servo ch, double degrees);
-    
-    /** neutral
-     *
-     * Tell the servo to move to it's neutral position (1.5ms pulse width)
-     *
-     * @param Servo channel
-     */
-    void neutral(Servo ch);
-    
-}; // class RCservo ends.
-
-}; // namespace AjK ends.
-
-using namespace AjK;
-
-#endif /* AJK_SIMPLERCSERVOS_H */
+/*
+    Copyright (c) 2011 Andy Kirkham
+ 
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+ 
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+ 
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+#ifndef AJK_SIMPLERCSERVOS_H
+#define AJK_SIMPLERCSERVOS_H
+
+#include "LPC17xx.h"
+#include "SimpleRCmbed.h"
+
+namespace AjK {
+
+class SimpleRCservos {
+
+public:
+    enum Servo { NoServo0, Servo1, Servo2, Servo3, Servo4, Servo5, Servo6, NumOfServos  };
+    
+protected:
+    PinName _pinName;
+    
+    uint32_t _duty;
+    
+    uint32_t _mid[NumOfServos];
+    
+    double _limitMax[NumOfServos];
+    
+    double _limitMin[NumOfServos];
+    
+    uint32_t _msMin[NumOfServos];
+    
+    uint32_t _msMax[NumOfServos];
+            
+    void init(uint32_t duty);
+       
+    void setRangeMin(Servo ch, uint32_t u) { _msMin[ch] = u * 24; }
+    
+    void setRangeMax(Servo ch, uint32_t u) { _msMax[ch] = (u * 24) - _msMin[ch]; }
+        
+    void setMRx(Servo ch, uint32_t value);
+    
+    double limit(Servo ch, double degrees);
+                
+public:
+
+    /** Constructor
+     */
+    SimpleRCservos() { init(480000UL); setDuty(); }
+
+    /** setDuty
+     * 
+     * Set's the duty, or period, of the PWM pulses.
+     * The default is 20ms and the parameter passed in is the 
+     * number of "clock ticks" that represents the period.
+     * 480000 gives rise to 20ms by default. It's rare that
+     * you would alter this value. Remember, with the PWM
+     * peripheral there is only a single, common,  period for 
+     * all six PWM outputs.
+     *
+     * @param duty The number of "clock ticks" for the duty cycle.
+     */
+    void setDuty(uint32_t duty = 480000);
+    
+    /** enable1
+     * 
+     * Enable PWM channel 1.
+     *
+     * @param PinName p26(P2_0) or LED1(P1_18)
+     */
+    void enable1(PinName pin = p26);
+    
+    /** enable2
+     * 
+     * Enable PWM channel 2.
+     *
+     * @param PinName p25(P2_1) or LED2(P1_20)
+     */
+    void enable2(PinName pin = p25);
+    
+    /** enable3
+     * 
+     * Enable PWM channel 3.
+     *
+     * @param PinName p24(P2_2) or LED3(P1_21)
+     */
+    void enable3(PinName pin = p24);
+    
+    /** enable4
+     * 
+     * Enable PWM channel 4.
+     *
+     * @param PinName p23(P2_3) or LED4(P1_23)
+     */
+    void enable4(PinName pin = p23);
+    
+    /** enable5
+     *
+     * Enable PWM channel 5.
+     *
+     * @param PinName p22(P2_4) or P1_24
+     */
+    void enable5(PinName pin = p22);
+    
+    /** enable6
+     *
+     * Enable PWM channel 6.
+     *
+     * @param PinName p21(P2_5) or P1_26
+     */
+    void enable6(PinName pin = p21);
+    
+    /** enable
+     *
+     * Enable the specified channel. The default pin on port 2 is assumed.
+     *
+     * @param Servo The channel to enable, Servo1 to Servo6
+     */
+    void enable(Servo ch);
+    
+    /** setLimitMin
+     * 
+     * Set the minimum logical limit for the specified channel.
+     *
+     * @param Servo channel
+     * @param double The lower logical limit.
+     */
+    void setLimitMin(Servo ch, double d) { _limitMin[ch] = d; }
+    
+    /** getLimitMin
+     * 
+     * Get the minimum logical limit for the specified channel.
+     *
+     * @param Servo channel
+     * @return double The lower logical limit.
+     */
+    double getlimitMin(Servo ch)   { return _limitMin[ch]; }
+    
+    /** setLimitMax
+     * 
+     * Set the maximum logical limit for the specified channel.
+     *
+     * @param Servo channel
+     * @param double The upper logical limit.
+     */
+    void setLimitMax(Servo ch, double d) { _limitMax[ch] = d; }
+    
+    /** getLimitMax
+     * 
+     * Get the maximum logical limit for the specified channel.
+     *
+     * @param Servo channel
+     * @return double The upper logical limit.
+     */
+    double getLimitMax(Servo ch)   { return _limitMax[ch]; }
+    
+    /** setLimits
+     *
+     * Set the logical upper and lower limits for channel.
+     *
+     * @param Servo channel
+     * @param double The minimum logical limit
+     * @param double The maximum logical limit
+     */
+    void setLimits(Servo ch, double min, double max) { 
+        _limitMin[ch] = min; 
+        _limitMax[ch] = max; 
+    }
+    
+    /** getRangeMin
+     * 
+     * Set the minimum physical limit for the specified channel.
+     *
+     * @param Servo channel
+     * @param double The lower phyiscal limit in microseconds
+     */
+    uint32_t getRangeMin(Servo ch, uint32_t u) { return _msMin[ch] / 24; }
+    
+    /** getRangeMax
+     * 
+     * Set the maximum physical limit for the specified channel.
+     *
+     * @param Servo channel
+     * @param uint32 The upper phyiscal limit in microseconds
+     */
+    uint32_t getRangeMax(Servo ch, uint32_t u) { return (_msMax[ch] / 24) + _msMin[ch]; }
+    
+    /** setRange
+     * 
+     * Set the minimum and maximum physical limits for the specified channel.
+     *
+     * @param Servo channel
+     * @param uint32 The lower phyiscal limit in microseconds
+     * @param uint32 The upper phyiscal limit in microseconds
+     */
+    void setRange(Servo ch, uint32_t min, uint32_t max) { 
+        setRangeMin(ch, min);
+        setRangeMax(ch, max);
+    }
+    
+    /** position
+     *
+     * Set the desired position of the servo.
+     * This value should be between the lower and upper logical limits.
+     *
+     * Under normal circumstances this fuction returns the same position is 
+     * was supplied. However, if the supplied position is outside the logical
+     * system limits we return the value we actually went to. This is almost
+     * always the value of the logical limit that is set for that direction.
+     *
+     * @param Servo channel
+     * @param double the logical position to move to.
+     * @return double The actual calculated position moved to.
+     */
+    double position(Servo ch, double degrees);
+    
+    /** neutral
+     *
+     * Tell the servo to move to it's neutral position (1.5ms pulse width)
+     *
+     * @param Servo channel
+     */
+    void neutral(Servo ch);
+    
+}; // class RCservo ends.
+
+}; // namespace AjK ends.
+
+using namespace AjK;
+
+#endif /* AJK_SIMPLERCSERVOS_H */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/SimpleServoControl.h	Thu May 19 22:46:09 2011 +0000
@@ -0,0 +1,119 @@
+/*
+    Copyright (c) 2011 Andy Kirkham
+ 
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+ 
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+ 
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+// Derived from forum post http://mbed.org/forum/helloworld/topic/2303
+
+#ifndef AJK_SIMPLESERVOCONTROL_H
+#define AJK_SIMPLESERVOCONTROL_H
+
+#include "mbed.h"
+#include "SimpleRCservos.h"
+
+namespace AjK {
+
+/** SimpleServoControl
+ *
+ * Very simple servo controller class.
+ * 
+ * @see http://mbed.org/forum/helloworld/topic/2303
+ */
+class SimpleServoControl {
+
+protected:
+    double _current_position;
+    double _desired_position;
+    double _step;
+    int    _poll_interval;
+    SimpleRCservos::Servo _motor;
+    SimpleRCservos *_servos;
+    Ticker _servo_poll;
+    
+public:
+
+    // Constructor.
+    SimpleServoControl(SimpleRCservos *servos, SimpleRCservos::Servo motor, double min = -90.0, double max = +90.0) 
+    { 
+        _servos = servos;
+        _motor = motor; 
+        _servos->setLimits(_motor, min, max); // define logical limits.
+        _servos->enable(_motor); // Enable the PWM outout.
+        _current_position = 0.0;
+        _desired_position = 0.0;
+        _step = 1.0;
+        _poll_interval = 10000; // 100ms.
+        _servo_poll.attach_us(this, &SimpleServoControl::poll, _poll_interval); 
+    }
+    
+    void poll(void) 
+    {
+        if (_desired_position > _current_position) {
+            _current_position += _step;
+            // Don't allow the servo to oscillate around _desired_position.
+            if (_desired_position < _current_position) { 
+                _current_position = _desired_position; 
+            }
+            _servos->position(_motor, _current_position);                        
+        }
+        else if (_desired_position < _current_position) {
+            _current_position -= _step;
+            // Don't allow the servo to oscillate around _desired_position.
+            if (_desired_position > _current_position) {
+                _current_position = _desired_position; 
+            }
+            _servos->position(_motor, _current_position);            
+        }
+    }
+    
+    void position(double position = 90.0)  // spins the servo 90º to the left
+    {
+        _desired_position = position;
+    }
+    
+    void setStep(double d) 
+    { 
+        _step = d; 
+    }
+    
+    double getStep(void) 
+    { 
+        return _step; 
+    } 
+    
+    void setPollInterval(int i) 
+    { 
+        _poll_interval = i;
+        _servo_poll.detach();
+        _servo_poll.attach_us(this, &SimpleServoControl::poll, _poll_interval); 
+    }
+    
+    int getPollInterval(void) 
+    { 
+        return _poll_interval; 
+    }
+    
+};
+
+}; // namespace AjK ends.
+
+using namespace AjK;
+
+#endif

--- a/src/SimpleRCservos.cpp	Mon May 02 18:42:16 2011 +0000
+++ b/src/SimpleRCservos.cpp	Thu May 19 22:46:09 2011 +0000
@@ -1,226 +1,247 @@
-
-
-#include "mbed.h"
-#include "SimpleRCservos.h"
-
-namespace AjK {
-
-void 
-SimpleRCservos::init(uint32_t duty)
-{
-    _duty = duty;
-    
-    for (int i = 0; i < NumOfServos; i++) {
-        _limitMin[i] = -1.0;
-        _limitMax[i] = +1.0;
-        
-        // The following would give 1ms to 2ms
-        setRange((Servo)i, 1000, 2000);
-        
-        // The following would give 900us to 2.1ms as required Hitec HS-322HD servo module.        
-        //setRange((Servo)i, 900, 2100);
-        
-        // The following would give 600us to 2.4ms
-        //setRange((Servo)i, 600, 2400);
-        
-        _mid[i] = duty * 0.075f; // 1.5ms
-        
-    }
-}
-
-void
-SimpleRCservos::setDuty(uint32_t duty)
-{
-    // Ensure powered up (default is 1)  
-    LPC_SC->PCONP |= (1UL << 6);  
-        
-    // CCLK/4 = 24MHz
-    LPC_SC->PCLKSEL0 &= ~(3UL << 12);
-    
-    // Reset.
-    LPC_PWM1->TCR   = 2; 
-    
-    _duty = duty;
-    
-    LPC_PWM1->PR    = 0;
-    LPC_PWM1->MR0   = _duty;
-    LPC_PWM1->MR1   = 0;
-    LPC_PWM1->MR2   = 0;
-    LPC_PWM1->MR3   = 0;
-    LPC_PWM1->MR4   = 0;
-    LPC_PWM1->MR5   = 0;
-    LPC_PWM1->MR6   = 0;
-        
-    LPC_PWM1->MCR   = 2; // MR0 resets TC.
-        
-    LPC_PWM1->TCR   = 9; // Enable.        
-}
-
-double
-SimpleRCservos::position(Servo ch, double pos)
-{
-    pos = limit(ch, pos);    
-    double a = pos - _limitMin[ch];
-    double set = a / (_limitMax[ch] - _limitMin[ch]);
-    uint32_t ps = _msMax[ch] * set;
-    setMRx(ch, ps + _msMin[ch]);   
-    return pos; 
-}
-
-void
-SimpleRCservos::neutral(Servo ch)
-{
-    setMRx(ch, _mid[ch]);   
-}
-
-double 
-SimpleRCservos::limit(Servo ch, double pos)
-{
-    if (pos >= _limitMin[ch] && pos <= _limitMax[ch]) return pos;
-    if (pos < _limitMin[ch]) return _limitMin[ch];
-    if (pos > _limitMax[ch]) return _limitMax[ch];
-    return 0.0; // Keep the compiler happy.
-}
-
-void 
-SimpleRCservos::setMRx(Servo ch, uint32_t value) 
-{
-    switch(ch) {
-        case Servo1: LPC_PWM1->MR1 = value; break;
-        case Servo2: LPC_PWM1->MR2 = value; break;
-        case Servo3: LPC_PWM1->MR3 = value; break;
-        case Servo4: LPC_PWM1->MR4 = value; break;
-        case Servo5: LPC_PWM1->MR5 = value; break;
-        case Servo6: LPC_PWM1->MR6 = value; break;        
-    }
-    LPC_PWM1->LER |= (1UL << ch);
-}
-
-void
-SimpleRCservos::enable(Servo ch)
-{
-    switch(ch) {
-        case 1: enable1(); break;
-        case 2: enable2(); break;
-        case 3: enable3(); break;
-        case 4: enable4(); break;
-        case 5: enable5(); break;
-        case 6: enable6(); break;
-    }
-}
-
-void
-SimpleRCservos::enable1(PinName pin)
-{
-    setMRx(Servo1, _mid[1]);
-    
-    switch(pin) {
-        case P2_0:    
-            LPC_PINCON->PINSEL4 &= ~(3UL << 0); // Mbed p26 P2.0 clr bits
-            LPC_PINCON->PINSEL4 |=  (1UL << 0); // Mbed p26 P2.0 set bits
-            break;
-        case P1_18: // Mbed LED1
-            LPC_PINCON->PINSEL3 &= ~(3UL << 4); // Mbed LED2 P1.18 clr bits
-            LPC_PINCON->PINSEL3 |=  (2UL << 4); // Mbed LED2 P1.18 set bits
-            break;
-    }
-    
-    LPC_PWM1->PCR |= (1UL << 9);
-}
-
-void
-SimpleRCservos::enable2(PinName pin)
-{
-    setMRx(Servo2, _mid[2]);
-    
-    switch(pin) {
-        case P2_1:
-            LPC_PINCON->PINSEL4 &= ~(3UL << 2); // Mbed p25 P2.1 clr bits
-            LPC_PINCON->PINSEL4 |=  (1UL << 2); // Mbed p25 P2.1 set bits
-            break;
-        case P1_20: // Mbed LED2
-            LPC_PINCON->PINSEL3 &= ~(3UL << 8); // Mbed LED2 P1.20 clr bits
-            LPC_PINCON->PINSEL3 |=  (2UL << 8); // Mbed LED2 P1.20 set bits
-            break;
-    }
-    
-    LPC_PWM1->PCR |= (1UL << 10);
-}
-
-void
-SimpleRCservos::enable3(PinName pin)
-{
-    setMRx(Servo3, _mid[3]);
-    
-    switch(pin) {
-        case P2_2:
-            LPC_PINCON->PINSEL4 &= ~(3UL << 4); // Mbed p24 P2.2 clr bits
-            LPC_PINCON->PINSEL4 |=  (1UL << 4); // Mbed p24 P2.2 set bits
-            break;
-        case P1_21: // Mbed LED3
-            LPC_PINCON->PINSEL3 &= ~(3UL << 10); // Mbed LED3 P1.21 clr bits
-            LPC_PINCON->PINSEL3 |=  (2UL << 10); // Mbed LED3 P1.21 set bits
-            break;
-    }
-    
-    LPC_PWM1->PCR |= (1UL << 11);
-}
-
-void
-SimpleRCservos::enable4(PinName pin)
-{
-    setMRx(Servo4, _mid[4]);
-    
-    switch(pin) {
-        case P2_3:
-            LPC_PINCON->PINSEL4 &= ~(3UL << 6); // Mbed p23 P2.3 clr bits
-            LPC_PINCON->PINSEL4 |=  (1UL << 6); // Mbed p23 P2.3 set bits
-            break;
-        case P1_23: // Mbed LED4
-            LPC_PINCON->PINSEL3 &= ~(3UL << 14); // Mbed LED4 P1.23 clr bits
-            LPC_PINCON->PINSEL3 |=  (2UL << 14); // Mbed LED4 P1.23 set bits
-            break;
-    }
-    
-    LPC_PWM1->PCR |= (1UL << 12);
-}
-
-void
-SimpleRCservos::enable5(PinName pin)
-{
-    setMRx(Servo5, _mid[5]);
-    
-    switch(pin) {
-        case P2_4: // Mbed p22
-            LPC_PINCON->PINSEL4 &= ~(3UL << 8); // Mbed p22 P2.4 clr bits
-            LPC_PINCON->PINSEL4 |=  (1UL << 8); // Mbed p22 P2.4 set bits
-            break;
-        case P1_24:            
-            LPC_PINCON->PINSEL3 &= ~(3UL << 16); // P1.24 clr bits
-            LPC_PINCON->PINSEL3 |=  (2UL << 16); // P1.24 set bits
-            break;
-    }
-    
-    LPC_PWM1->PCR |= (1UL << 13);
-}
-
-void
-SimpleRCservos::enable6(PinName pin)
-{
-    setMRx(Servo6, _mid[6]);
-    
-    switch(pin) {
-        case P2_5: // Mbed p21
-            LPC_PINCON->PINSEL4 &= ~(3UL << 10); // Mbed p21 P2.5 clr bits
-            LPC_PINCON->PINSEL4 |=  (1UL << 10); // Mbed p21 P2.5 set bits
-            break;
-        case P1_26:
-            LPC_PINCON->PINSEL3 &= ~(3UL << 20); // P1.26 clr bits
-            LPC_PINCON->PINSEL3 |=  (2UL << 20); // P1.26 set bits
-            break;
-    }
-    
-    LPC_PWM1->PCR |= (1UL << 14);
-}
-
-}; // namespace AjK ends.
+/*
+    Copyright (c) 2011 Andy Kirkham
+ 
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+ 
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+ 
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+
+#include "mbed.h"
+#include "SimpleRCservos.h"
+
+namespace AjK {
+
+void 
+SimpleRCservos::init(uint32_t duty)
+{
+    _duty = duty;
+    
+    for (int i = 0; i < NumOfServos; i++) {
+        _limitMin[i] = -1.0;
+        _limitMax[i] = +1.0;
+        
+        // The following would give 1ms to 2ms
+        setRange((Servo)i, 1000, 2000);
+        
+        // The following would give 900us to 2.1ms as required Hitec HS-322HD servo module.        
+        //setRange((Servo)i, 900, 2100);
+        
+        // The following would give 600us to 2.4ms
+        //setRange((Servo)i, 600, 2400);
+        
+        _mid[i] = duty * 0.075f; // 1.5ms
+        
+    }
+}
+
+void
+SimpleRCservos::setDuty(uint32_t duty)
+{
+    // Ensure powered up (default is 1)  
+    LPC_SC->PCONP |= (1UL << 6);  
+        
+    // CCLK/4 = 24MHz
+    LPC_SC->PCLKSEL0 &= ~(3UL << 12);
+    
+    // Reset.
+    LPC_PWM1->TCR   = 2; 
+    
+    _duty = duty;
+    
+    LPC_PWM1->PR    = 0;
+    LPC_PWM1->MR0   = _duty;
+    LPC_PWM1->MR1   = 0;
+    LPC_PWM1->MR2   = 0;
+    LPC_PWM1->MR3   = 0;
+    LPC_PWM1->MR4   = 0;
+    LPC_PWM1->MR5   = 0;
+    LPC_PWM1->MR6   = 0;
+        
+    LPC_PWM1->MCR   = 2; // MR0 resets TC.
+        
+    LPC_PWM1->TCR   = 9; // Enable.        
+}
+
+double
+SimpleRCservos::position(Servo ch, double pos)
+{
+    pos = limit(ch, pos);    
+    double a = pos - _limitMin[ch];
+    double set = a / (_limitMax[ch] - _limitMin[ch]);
+    uint32_t ps = _msMax[ch] * set;
+    setMRx(ch, ps + _msMin[ch]);   
+    return pos; 
+}
+
+void
+SimpleRCservos::neutral(Servo ch)
+{
+    setMRx(ch, _mid[ch]);   
+}
+
+double 
+SimpleRCservos::limit(Servo ch, double pos)
+{
+    if (pos >= _limitMin[ch] && pos <= _limitMax[ch]) return pos;
+    if (pos < _limitMin[ch]) return _limitMin[ch];
+    if (pos > _limitMax[ch]) return _limitMax[ch];
+    return 0.0; // Keep the compiler happy.
+}
+
+void 
+SimpleRCservos::setMRx(Servo ch, uint32_t value) 
+{
+    switch(ch) {
+        case Servo1: LPC_PWM1->MR1 = value; break;
+        case Servo2: LPC_PWM1->MR2 = value; break;
+        case Servo3: LPC_PWM1->MR3 = value; break;
+        case Servo4: LPC_PWM1->MR4 = value; break;
+        case Servo5: LPC_PWM1->MR5 = value; break;
+        case Servo6: LPC_PWM1->MR6 = value; break;        
+    }
+    LPC_PWM1->LER |= (1UL << ch);
+}
+
+void
+SimpleRCservos::enable(Servo ch)
+{
+    switch(ch) {
+        case 1: enable1(); break;
+        case 2: enable2(); break;
+        case 3: enable3(); break;
+        case 4: enable4(); break;
+        case 5: enable5(); break;
+        case 6: enable6(); break;
+    }
+}
+
+void
+SimpleRCservos::enable1(PinName pin)
+{
+    setMRx(Servo1, _mid[1]);
+    
+    switch(pin) {
+        case P2_0:    
+            LPC_PINCON->PINSEL4 &= ~(3UL << 0); // Mbed p26 P2.0 clr bits
+            LPC_PINCON->PINSEL4 |=  (1UL << 0); // Mbed p26 P2.0 set bits
+            break;
+        case P1_18: // Mbed LED1
+            LPC_PINCON->PINSEL3 &= ~(3UL << 4); // Mbed LED2 P1.18 clr bits
+            LPC_PINCON->PINSEL3 |=  (2UL << 4); // Mbed LED2 P1.18 set bits
+            break;
+    }
+    
+    LPC_PWM1->PCR |= (1UL << 9);
+}
+
+void
+SimpleRCservos::enable2(PinName pin)
+{
+    setMRx(Servo2, _mid[2]);
+    
+    switch(pin) {
+        case P2_1:
+            LPC_PINCON->PINSEL4 &= ~(3UL << 2); // Mbed p25 P2.1 clr bits
+            LPC_PINCON->PINSEL4 |=  (1UL << 2); // Mbed p25 P2.1 set bits
+            break;
+        case P1_20: // Mbed LED2
+            LPC_PINCON->PINSEL3 &= ~(3UL << 8); // Mbed LED2 P1.20 clr bits
+            LPC_PINCON->PINSEL3 |=  (2UL << 8); // Mbed LED2 P1.20 set bits
+            break;
+    }
+    
+    LPC_PWM1->PCR |= (1UL << 10);
+}
+
+void
+SimpleRCservos::enable3(PinName pin)
+{
+    setMRx(Servo3, _mid[3]);
+    
+    switch(pin) {
+        case P2_2:
+            LPC_PINCON->PINSEL4 &= ~(3UL << 4); // Mbed p24 P2.2 clr bits
+            LPC_PINCON->PINSEL4 |=  (1UL << 4); // Mbed p24 P2.2 set bits
+            break;
+        case P1_21: // Mbed LED3
+            LPC_PINCON->PINSEL3 &= ~(3UL << 10); // Mbed LED3 P1.21 clr bits
+            LPC_PINCON->PINSEL3 |=  (2UL << 10); // Mbed LED3 P1.21 set bits
+            break;
+    }
+    
+    LPC_PWM1->PCR |= (1UL << 11);
+}
+
+void
+SimpleRCservos::enable4(PinName pin)
+{
+    setMRx(Servo4, _mid[4]);
+    
+    switch(pin) {
+        case P2_3:
+            LPC_PINCON->PINSEL4 &= ~(3UL << 6); // Mbed p23 P2.3 clr bits
+            LPC_PINCON->PINSEL4 |=  (1UL << 6); // Mbed p23 P2.3 set bits
+            break;
+        case P1_23: // Mbed LED4
+            LPC_PINCON->PINSEL3 &= ~(3UL << 14); // Mbed LED4 P1.23 clr bits
+            LPC_PINCON->PINSEL3 |=  (2UL << 14); // Mbed LED4 P1.23 set bits
+            break;
+    }
+    
+    LPC_PWM1->PCR |= (1UL << 12);
+}
+
+void
+SimpleRCservos::enable5(PinName pin)
+{
+    setMRx(Servo5, _mid[5]);
+    
+    switch(pin) {
+        case P2_4: // Mbed p22
+            LPC_PINCON->PINSEL4 &= ~(3UL << 8); // Mbed p22 P2.4 clr bits
+            LPC_PINCON->PINSEL4 |=  (1UL << 8); // Mbed p22 P2.4 set bits
+            break;
+        case P1_24:            
+            LPC_PINCON->PINSEL3 &= ~(3UL << 16); // P1.24 clr bits
+            LPC_PINCON->PINSEL3 |=  (2UL << 16); // P1.24 set bits
+            break;
+    }
+    
+    LPC_PWM1->PCR |= (1UL << 13);
+}
+
+void
+SimpleRCservos::enable6(PinName pin)
+{
+    setMRx(Servo6, _mid[6]);
+    
+    switch(pin) {
+        case P2_5: // Mbed p21
+            LPC_PINCON->PINSEL4 &= ~(3UL << 10); // Mbed p21 P2.5 clr bits
+            LPC_PINCON->PINSEL4 |=  (1UL << 10); // Mbed p21 P2.5 set bits
+            break;
+        case P1_26:
+            LPC_PINCON->PINSEL3 &= ~(3UL << 20); // P1.26 clr bits
+            LPC_PINCON->PINSEL3 |=  (2UL << 20); // P1.26 set bits
+            break;
+    }
+    
+    LPC_PWM1->PCR |= (1UL << 14);
+}
+
+}; // namespace AjK ends.