Home | History | Annotate | Download | only in servo 
      1 /*
      2  * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha (at) intel.com>
      3  * Copyright (c) 2014 Intel Corporation.
      4  *
      5  * Permission is hereby granted, free of charge, to any person obtaining
      6  * a copy of this software and associated documentation files (the
      7  * "Software"), to deal in the Software without restriction, including
      8  * without limitation the rights to use, copy, modify, merge, publish,
      9  * distribute, sublicense, and/or sell copies of the Software, and to
     10  * permit persons to whom the Software is furnished to do so, subject to
     11  * the following conditions:
     12  *
     13  * The above copyright notice and this permission notice shall be
     14  * included in all copies or substantial portions of the Software.
     15  *
     16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
     17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
     18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
     19  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
     20  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
     21  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
     22  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
     23  */
     24 
     25 #include <iostream>
     26 #include <sstream>
     27 #include <string>
     28 #include <stdexcept>
     29 #include <unistd.h>
     30 #include <stdlib.h>
     31 #include <math.h>
     32 
     33 #include "servo.h"
     34 
     35 using namespace upm;
     36 
     37 Servo::Servo (int pin) {
     38     init(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, DEFAULT_WAIT_DISABLE_PWM);
     39 }
     40 
     41 Servo::Servo (int pin, int minPulseWidth, int maxPulseWidth) {
     42     init(pin, minPulseWidth, maxPulseWidth, DEFAULT_WAIT_DISABLE_PWM);
     43 }
     44 
     45 Servo::Servo (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm) {
     46     init(pin, minPulseWidth, maxPulseWidth, waitAndDisablePwm);
     47 }
     48 
     49 Servo::~Servo () {
     50     haltPwm();
     51     mraa_pwm_close (m_pwmServoContext);
     52 }
     53 
     54 /*
     55  * X = between (MIN_PULSE_WIDTH , MAX_PULSE_WIDTH)
     56  *
     57  * X usec
     58  * _______
     59  *        |_______________________________________
     60  *                      m_period usec
     61  *
     62  * */
     63 mraa_result_t Servo::setAngle (int angle) {
     64     if (angle > m_maxAngle || angle < 0) {
     65         // C++11 std::to_string() would be nice, but...
     66         std::ostringstream str;
     67         str << m_maxAngle;
     68         throw std::out_of_range(std::string(__FUNCTION__) +
     69                                 ": angle must be between 0 and " +
     70                                 str.str());
     71 
     72         return MRAA_ERROR_UNSPECIFIED;
     73     }
     74 
     75     mraa_pwm_enable (m_pwmServoContext, 1);
     76     mraa_pwm_period_us (m_pwmServoContext, m_period);
     77     mraa_pwm_pulsewidth_us (m_pwmServoContext, calcPulseTraveling(angle));
     78 
     79     if (m_waitAndDisablePwm) {
     80         sleep(1); // we must make sure that we don't turn off PWM before the servo is done moving.
     81         haltPwm();
     82     }
     83 
     84     m_currAngle = angle;
     85     return MRAA_SUCCESS;
     86 }
     87 
     88 mraa_result_t Servo::haltPwm () {
     89     return mraa_pwm_enable (m_pwmServoContext, 0);
     90 }
     91 
     92 /*
     93  * Calculating relative pulse time to the value.
     94  * */
     95 int Servo::calcPulseTraveling (int value) {
     96     // if bigger than the boundaries
     97     if (value > m_maxAngle) {
     98         return m_maxPulseWidth;
     99     }
    100 
    101     // if less than the boundaries
    102     if (value  < 0) {
    103         return m_minPulseWidth;
    104     }
    105 
    106     // the conversion
    107     return (int) ((float)m_minPulseWidth + ((float)value / m_maxAngle) * ((float)m_maxPulseWidth - (float)m_minPulseWidth));
    108 }
    109 
    110 void
    111 Servo::setMinPulseWidth (int width) {
    112     m_minPulseWidth = width;
    113 }
    114 
    115 void
    116 Servo::setMaxPulseWidth (int width) {
    117     m_maxPulseWidth = width;
    118 }
    119 
    120 void
    121 Servo::setPeriod (int period) {
    122     m_period = period;
    123 }
    124 
    125 int
    126 Servo::getMinPulseWidth () {
    127     return m_minPulseWidth;
    128 }
    129 
    130 int
    131 Servo::getMaxPulseWidth () {
    132     return m_maxPulseWidth;
    133 }
    134 
    135 int
    136 Servo::getPeriod () {
    137     return m_period;
    138 }
    139 
    140 /**
    141  *  private mathod:  would like to use delegating constructors instead but that requires C++11
    142  */
    143 void
    144 Servo::init (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm) {
    145     m_minPulseWidth   = minPulseWidth;
    146     m_maxPulseWidth   = maxPulseWidth;
    147     m_period          = PERIOD;
    148 
    149     m_waitAndDisablePwm = waitAndDisablePwm;
    150 
    151     m_maxAngle        = 180.0;
    152     m_servoPin        = pin;
    153 
    154     if ( !(m_pwmServoContext = mraa_pwm_init (m_servoPin)) )
    155       {
    156         throw std::invalid_argument(std::string(__FUNCTION__) +
    157                                     ": mraa_pwm_init() failed, invalid pin?");
    158         return;
    159       }
    160 
    161     m_currAngle = 180;
    162 
    163     setAngle (0);
    164 }
    165