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 <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
Servo(int pin)37 Servo::Servo (int pin) {
38 init(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, DEFAULT_WAIT_DISABLE_PWM);
39 }
40
Servo(int pin,int minPulseWidth,int maxPulseWidth)41 Servo::Servo (int pin, int minPulseWidth, int maxPulseWidth) {
42 init(pin, minPulseWidth, maxPulseWidth, DEFAULT_WAIT_DISABLE_PWM);
43 }
44
Servo(int pin,int minPulseWidth,int maxPulseWidth,int waitAndDisablePwm)45 Servo::Servo (int pin, int minPulseWidth, int maxPulseWidth, int waitAndDisablePwm) {
46 init(pin, minPulseWidth, maxPulseWidth, waitAndDisablePwm);
47 }
48
~Servo()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 * */
setAngle(int angle)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
haltPwm()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 * */
calcPulseTraveling(int value)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
setMinPulseWidth(int width)111 Servo::setMinPulseWidth (int width) {
112 m_minPulseWidth = width;
113 }
114
115 void
setMaxPulseWidth(int width)116 Servo::setMaxPulseWidth (int width) {
117 m_maxPulseWidth = width;
118 }
119
120 void
setPeriod(int period)121 Servo::setPeriod (int period) {
122 m_period = period;
123 }
124
125 int
getMinPulseWidth()126 Servo::getMinPulseWidth () {
127 return m_minPulseWidth;
128 }
129
130 int
getMaxPulseWidth()131 Servo::getMaxPulseWidth () {
132 return m_maxPulseWidth;
133 }
134
135 int
getPeriod()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
init(int pin,int minPulseWidth,int maxPulseWidth,int waitAndDisablePwm)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