Diff: Servo.cpp

Revision:

2:738b08cb03ea

Parent:

1:940dffb14ff6

Child:

3:37e6c7b8fd2d

--- a/Servo.cpp	Sat Oct 21 21:32:00 2017 +0200
+++ b/Servo.cpp	Sat Oct 21 21:36:48 2017 +0200
@@ -1,98 +1,86 @@
-#ifndef _SERVO_
-#define _SERVO_
+
+
+#include "Servo.hpp"
+
+Servo::Servo(PinName pwmPin, float zeroAngle, float minAngle, float maxAngle) :
+    _servo(pwmPin)
+{
+    PW_PER_DEG = 0.0005f/45.0f;
+    ZEROPW = 0.0014f;
+
+    _minAngle = minAngle;
+    _maxAngle = maxAngle;
+    _angle = zeroAngle;
+    _servo.period_us(5000);
+    _servo.pulsewidth(0);
+    SaveZero();        
+}
 
-#include "mbed.h"
+void Servo::SetServo()
+{
+    float pw = ZEROPW+_angle*PW_PER_DEG;
+
+    pw = ( pw < MIN_PW ? MIN_PW : pw > MAX_PW ? MAX_PW : pw);
+
+    _servo.pulsewidth(pw);
+}
+
+void Servo::SaveZero()
+{
+    ZEROPW = (ZEROPW+_angle*PW_PER_DEG);
+}
+
+void Servo::Calibrate(float actualAngle)
+{
+    PW_PER_DEG = (_angle*PW_PER_DEG)/actualAngle;
+}
 
-/**
- *  A Servo controller library :)s
- *
- * @author CA Bezuidenhout
- */
-class Servo {
-public:
-    /**
-    * @param pwmPin : PWM pin of servo / orange wire
-    * @param zeroAngle : The zero angle of the servo
-    * @param minAngle : The minimum angle allowed for the servo
-    * @param maxAngle : The maximum angle allowed for the servo
-    */
-    Servo(PinName pwmPin, float zeroAngle = 0, float minAngle = -90.0f, float maxAngle = 90.0f);
-  
-    /**
-     * Saves the current position as the zero position.
-     */
-    void SaveZero();
-  
-    /**
-     * Sets the zero position without moving the servo
-     *
-     * @param angle : The angle to set as the zero position
-     */
-    void SetZero(float angle);
-  
-    /**
-     * Calibrate the servo
-     * @param actualAngle : The actual angle from zero, of the current position in degrees.
-     */
-    void Calibrate(float actualAngle);
-  
-    /**
-     * Sets the angular position of the servo
-     * @param angle : Desired angle from the zero position in degrees
-     */
-    void Set(float angle);
-  
-    /**
-     * Moves to an relative angular position from the current position
-     * @param relAngle : The relative angle to move in degrees
-     */
-    void Move(float relAngle);
-  
-    /**
-     * Disables control signal to servo
-     */
-    void Off();
+void Servo::Set(float angle)
+{
+    if( _angle < _minAngle)
+        _angle = _minAngle;
+    else if( _angle > _maxAngle )
+        _angle = _maxAngle;
+    else 
+        _angle = angle;
+
+    SetServo();
+}
+
+void Servo::Move(float relAngle)
+{
+    float newAngle = _angle + relAngle;
+
+    if( newAngle < _minAngle )
+        _angle = _minAngle;
+    else if( newAngle > _maxAngle )
+        _angle = _maxAngle;
+    else
+        _angle = newAngle;
 
-    /**
-     * Gets the current angle of the servo
-     * @returns Current angle of the servo
-     */ 
-    float GetAngle();
+    SetServo();
+}
 
-    /**
-     * Sets the minimum angle allowed for the servo
-     * @params minAngle : minimum angle allowed
-     */
-    void SetMin(float minAngle);
+void Servo::SetZero(float angle)
+{
+  ZEROPW = (ZEROPW+angle*PW_PER_DEG);
+}
 
-    /**
-     * Sets the maximum angle allowed for the servo
-     * @params maxAngle : maximum angle allowed
-     */
-    void SetMax(float maxAngle);
+void Servo::Off()
+{
+    _servo.pulsewidth(0);
+}
 
-    /**
-     * Sets the servo to minimum position
-     */
-    void GotoMin();
-
-    /**
-     * Sets the servo to maximum position
-     */
-    void GotoMax();
+float Servo::GetAngle() {
+    return _angle;   
+}
 
-private:
-    PwmOut _servo;
-    float _angle;
-    float _ref;
-    float _minAngle;
-    float _maxAngle;
-    
-    float PW_PER_DEG;
-    float ZEROPW;
-    static const float MIN_PW = 0.0005;
-    static const float MAX_PW = 0.0025f;
-  
-    void SetServo();
-};
-#endif
\ No newline at end of file
+void Servo::GotoMin() {
+    _angle = _minAngle;  
+    SetServo();
+}
+
+void Servo::GotoMax() {
+    _angle = _maxAngle;
+    SetServo();
+}
\ No newline at end of file