Servo.cpp

00001 #include "Servo.h"
00002 
00003 const float Servo::MIN_INPUT = 0.01f;
00004 const float Servo::MAX_INPUT = 0.99f;
00005 
00006 Servo::Servo(PinName Pin) : ServoPin(Pin)
00007 {
00008     servoEnabled = false;
00009     Position = 0;
00010     Period = 0;
00011 }
00012 
00013 /**
00014  * Sets the pwm period.
00015  * @_Period period in mus.
00016  */
00017 void Servo::SetPeriod(float _Period)
00018 {
00019     Period = _Period;
00020 }
00021 
00022 /**
00023  * Sets the desired angle.
00024  * @_Input a value between 0...1.
00025  */
00026 void Servo::SetPosition(float _Input)
00027 {
00028     if (servoEnabled) {
00029         if (_Input < MIN_INPUT) _Input = MIN_INPUT;
00030         if (_Input > MAX_INPUT) _Input = MAX_INPUT;
00031         Position = static_cast<int>(_Input * static_cast<float>(Period));
00032     }
00033 }
00034 
00035 void Servo::StartPulse()
00036 {
00037     ServoPin = 1;
00038     PulseStop.attach(callback(this, &Servo::EndPulse), std::chrono::microseconds{static_cast<long int>(Position)});
00039 }
00040 
00041 void Servo::EndPulse()
00042 {
00043     ServoPin = 0;
00044 }
00045 
00046 /**
00047  * Enables the servo with start angle and period.
00048  * @_StartInput a value between 0...1.
00049  * @_Period period in mus.
00050  */
00051 void Servo::Enable(float _StartInput, int _Period)
00052 {
00053     servoEnabled = true;
00054     if (_StartInput < MIN_INPUT) _StartInput = MIN_INPUT;
00055     if (_StartInput > MAX_INPUT) _StartInput = MAX_INPUT;
00056     Period = _Period;
00057     Position = static_cast<int>(_StartInput * static_cast<float>(Period));
00058     Pulse.attach(callback(this, &Servo::StartPulse), std::chrono::microseconds{static_cast<long int>(Period)});
00059 }
00060 
00061 /**
00062  * Enables the servo with last set angle and period.
00063  */
00064 void Servo::Enable()
00065 {
00066     servoEnabled = true;
00067     Pulse.attach(callback(this, &Servo::StartPulse), std::chrono::microseconds{static_cast<long int>(Period)});
00068 }
00069 
00070 /**
00071  * Disables the servo.
00072  */
00073 void Servo::Disable()
00074 {
00075     servoEnabled = false;
00076     Pulse.detach();
00077 }
00078 
00079 /**
00080  * Returns true if Servo is enabled.
00081  * @return isEnable.
00082  */
00083 bool Servo::isEnabled()
00084 {
00085     return servoEnabled;
00086 }