diff --git a/src/servo/servo.cxx b/src/servo/servo.cxx index 500734ba..cde79c7d 100644 --- a/src/servo/servo.cxx +++ b/src/servo/servo.cxx @@ -32,22 +32,19 @@ using namespace upm; Servo::Servo (int pin) { - mraa_result_t error = MRAA_SUCCESS; + init(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, DEFAULT_WAIT_DISABLE_PWM); +} - m_minPulseWidth = MIN_PULSE_WIDTH; - m_maxPulseWidth = MAX_PULSE_WIDTH; - m_maxPeriod = MAX_PERIOD; +Servo::Servo (int pin, int minPulseWidth, int maxPulseWidth) { + init(pin, minPulseWidth, maxPulseWidth, DEFAULT_WAIT_DISABLE_PWM); +} - m_maxAngle = 180.0; - m_servoPin = pin; - m_pwmServoContext = mraa_pwm_init (m_servoPin); - - m_currAngle = 180; - - setAngle (0); +Servo::Servo (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm) { + init(pin, minPulseWidth, maxPulseWidth, waitAndDisablePwm); } Servo::~Servo () { + haltPwm(); mraa_pwm_close (m_pwmServoContext); } @@ -57,10 +54,8 @@ Servo::~Servo () { * X usec * _______ * |_______________________________________ - * 20000 usec + * m_period usec * - * Max period can be only 7968750(nses) which is ~8(msec) - * so the servo wil not work as expected. * */ mraa_result_t Servo::setAngle (int angle) { if (m_pwmServoContext == NULL) { @@ -72,22 +67,27 @@ mraa_result_t Servo::setAngle (int angle) { return MRAA_ERROR_UNSPECIFIED; } - int period = (m_maxPulseWidth - m_minPulseWidth) / m_maxAngle; - - int cycles = (int)(100.0 * (abs (m_currAngle - angle) / m_maxAngle)); - - // int cycles = (int)(100.0 * ((float)angle / (float)m_maxAngle)); - mraa_pwm_enable (m_pwmServoContext, 1); - for (int cycle = 0; cycle < cycles; cycle++) { - mraa_pwm_period_us (m_pwmServoContext, m_maxPeriod); - mraa_pwm_pulsewidth_us (m_pwmServoContext, calcPulseTraveling(angle)); + mraa_pwm_period_us (m_pwmServoContext, m_period); + mraa_pwm_pulsewidth_us (m_pwmServoContext, calcPulseTraveling(angle)); + + if (m_waitAndDisablePwm) { + sleep(1); // we must make sure that we don't turn off PWM before the servo is done moving. + haltPwm(); } - mraa_pwm_enable (m_pwmServoContext, 0); m_currAngle = angle; } +mraa_result_t Servo::haltPwm () { + if (m_pwmServoContext == NULL) { + std::cout << "PWM context is NULL" << std::endl; + return MRAA_ERROR_UNSPECIFIED; + } + + return mraa_pwm_enable (m_pwmServoContext, 0); +} + /* * Calculating relative pulse time to the value. * */ @@ -117,8 +117,8 @@ Servo::setMaxPulseWidth (int width) { } void -Servo::setMaxPeriod (int width) { - m_maxPeriod = width; +Servo::setPeriod (int period) { + m_period = period; } int @@ -132,6 +132,26 @@ Servo::getMaxPulseWidth () { } int -Servo::getMaxPeriod () { - return m_maxPeriod; +Servo::getPeriod () { + return m_period; +} + +/** + * private mathod: would like to use delegating constructors instead but that requires C++11 + */ +void +Servo::init (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm) { + m_minPulseWidth = minPulseWidth; + m_maxPulseWidth = maxPulseWidth; + m_period = PERIOD; + + m_waitAndDisablePwm = waitAndDisablePwm; + + m_maxAngle = 180.0; + m_servoPin = pin; + m_pwmServoContext = mraa_pwm_init (m_servoPin); + + m_currAngle = 180; + + setAngle (0); } diff --git a/src/servo/servo.h b/src/servo/servo.h index c832d9ef..1449682e 100644 --- a/src/servo/servo.h +++ b/src/servo/servo.h @@ -28,12 +28,14 @@ namespace upm { -#define MIN_PULSE_WIDTH 600 -#define MAX_PULSE_WIDTH 2500 -#define MAX_PERIOD 7968 +#define MIN_PULSE_WIDTH 600 +#define MAX_PULSE_WIDTH 2500 +#define PERIOD 20000 -#define HIGH 1 -#define LOW 0 +#define HIGH 1 +#define LOW 0 + +#define DEFAULT_WAIT_DISABLE_PWM 0 /** * @brief C++ API for servo libraries @@ -53,6 +55,26 @@ class Servo { */ Servo (int pin); + /** + * Instantiates a servo object + * + * @param pin servo pin number + * @param minPulseWidth minimum pulse width, in microseconds + * @param maxPulseWidth maximum pulse width, in microseconds + */ + Servo (int pin, int minPulseWidth, int maxPulseWidth); + + /** + * Instantiates a servo object + * + * @param pin servo pin number + * @param minPulseWidth minimum pulse width, in microseconds + * @param maxPulseWidth maximum pulse width, in microseconds + * @param waitAndDisablePwm if 1, PWM will be enabled only during setAngle() execution + * for a period of 1 second, and then turned back off. If 0, PWM will remain on afterward. + */ + Servo (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm); + /** * Servo object destructor. */ @@ -66,6 +88,11 @@ class Servo { */ mraa_result_t setAngle (int angle); + /** + * Halts PWM for this servo and allows it to move freely. + */ + mraa_result_t haltPwm (); + /** * Return name of the component * @@ -93,9 +120,9 @@ class Servo { /** * Set maximum period width * - * @param width maximum PWM period width + * @param period PWM period width */ - void setMaxPeriod (int width); + void setPeriod (int period); /** * Return minimum pulse width @@ -116,7 +143,7 @@ class Servo { * * @return maximum PWM period width */ - int getMaxPeriod (); + int getPeriod (); protected: int calcPulseTraveling (int value); @@ -129,7 +156,12 @@ class Servo { int m_minPulseWidth; int m_maxPulseWidth; - int m_maxPeriod; + int m_period; + + int m_waitAndDisablePwm; + + private: + void init (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm); }; }