src/servo/servo.cxx

   1 /*
   2  * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@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 <unistd.h>
  27 #include <stdlib.h>
  28 #include <math.h>
  29
  30 #include "servo.h"
  31
  32 using namespace upm;
  33
  34 Servo::Servo (int pin) {
  35     mraa_result_t error = MRAA_SUCCESS;
  36
  37     m_minPulseWidth   = MIN_PULSE_WIDTH;
  38     m_maxPulseWidth   = MAX_PULSE_WIDTH;
  39     m_maxPeriod       = MAX_PERIOD;
  40
  41     m_maxAngle        = 180.0;
  42     m_servoPin        = pin;
  43     m_pwmServoContext = mraa_pwm_init (m_servoPin);
  44
  45     m_currAngle = 180;
  46
  47     setAngle (0);
  48 }
  49
  50 Servo::~Servo () {
  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  *                      20000 usec
  61  *
  62  * Max period can be only 7968750(nses) which is ~8(msec)
  63  * so the servo wil not work as expected.
  64  * */
  65 mraa_result_t Servo::setAngle (int angle) {
  66     if (m_pwmServoContext == NULL) {
  67         std::cout << "PWM context is NULL" << std::endl;
  68         return MRAA_ERROR_UNSPECIFIED;
  69     }
  70
  71     if (angle > m_maxAngle || angle < 0) {
  72         return MRAA_ERROR_UNSPECIFIED;
  73     }
  74
  75     int period = (m_maxPulseWidth - m_minPulseWidth) / m_maxAngle;
  76
  77     int cycles = (int)(100.0 * (abs (m_currAngle - angle) / m_maxAngle));
  78
  79     // int cycles = (int)(100.0 * ((float)angle / (float)m_maxAngle));
  80
  81     mraa_pwm_enable (m_pwmServoContext, 1);
  82     for (int cycle = 0; cycle < cycles; cycle++) {
  83         mraa_pwm_period_us (m_pwmServoContext, m_maxPeriod);
  84         mraa_pwm_pulsewidth_us (m_pwmServoContext, calcPulseTraveling(angle));
  85     }
  86     mraa_pwm_enable (m_pwmServoContext, 0);
  87
  88     std::cout << "angle = " << angle << " ,pulse = " << calcPulseTraveling(angle) << ", cycles " << cycles << std::endl;
  89
  90     m_currAngle = angle;
  91 }
  92
  93 /*
  94  * Calculating relative pulse time to the value.
  95  * */
  96 int Servo::calcPulseTraveling (int value) {
  97     // if bigger than the boundaries
  98     if (value > m_maxAngle) {
  99         return m_maxPulseWidth;
 100     }
 101
 102     // if less than the boundaries
 103     if (value  < 0) {
 104         return m_minPulseWidth;
 105     }
 106
 107     // the conversion
 108     return (int) ((float)m_minPulseWidth + ((float)value / m_maxAngle) * ((float)m_maxPulseWidth - (float)m_minPulseWidth));
 109 }
 110
 111 void
 112 Servo::setMinPulseWidth (int width) {
 113     m_minPulseWidth = width;
 114 }
 115
 116 void
 117 Servo::setMaxPulseWidth (int width) {
 118     m_maxPulseWidth = width;
 119 }
 120
 121 void
 122 Servo::setMaxPeriod (int width) {
 123     m_maxPeriod = width;
 124 }
 125
 126 int
 127 Servo::getMinPulseWidth () {
 128     return m_minPulseWidth;
 129 }
 130
 131 int
 132 Servo::getMaxPulseWidth () {
 133     return m_maxPulseWidth;
 134 }
 135
 136 int
 137 Servo::getMaxPeriod () {
 138     return m_maxPeriod;
 139 }