Diff: Servo.cpp

Revision:

0:7ba85d855519

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.cpp	Wed Jan 18 22:09:15 2017 +0000
@@ -0,0 +1,50 @@
+#include "Servo.h"
+
+
+
+Servo::Servo(PinName pin, double maxPulse, double minPulse) : _pwm(pin){
+    _pinName = pin;
+    _pwm.period(0.020);//20ms 周期
+    _maxPulse = maxPulse;
+    _minPulse = minPulse;
+    setAngleRange();//Max 90° min -90°に設定
+    setZeroPulse((maxPulse + minPulse)/2.0);
+    write(0.0f);//初期位置に
+}
+
+void Servo::write(double angle){
+    //error check
+    if(angle > _maxAngle)    angle = _maxAngle;
+    else if(angle < _minAngle)   angle = _minAngle;
+    //make pulse value
+    double pulse = _zeroPulse + angle * ((_maxPulse - _zeroPulse) / 90.0);
+    _pwm.pulsewidth(pulse);
+    
+    currentAngle = angle;//register set angle   
+}
+
+double Servo::read(){
+    return currentAngle;   
+}
+void Servo::setRange(double maxPulse, double minPulse){
+    _maxPulse = maxPulse;
+    _minPulse = minPulse;
+}
+
+void Servo::setZeroPulse(double zeroPulse){
+    _zeroPulse = zeroPulse; 
+}
+
+void Servo::setAngleRange(double maxAngle, double minAngle){
+    _maxAngle = maxAngle;
+    _minAngle = minAngle;
+}
+
+Servo& Servo::operator=(double dValue){
+    write(dValue);
+    return *this;   
+}
+
+double Servo::operator()(void){
+    return read();   
+}
\ No newline at end of file