upm/src/servo/servo.cxx
Kirill Luchikhin d15bf22536 Fix some dosctrings errors and trailing whitespaces
Signed-off-by: Kirill Luchikhin <kirill.luchikhin@intel.com>
2014-07-29 21:46:48 +04:00

140 lines
3.7 KiB
C++

/*
* Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
* Copyright (c) 2014 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <iostream>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include "servo.h"
using namespace upm;
Servo::Servo (int pin) {
mraa_result_t error = MRAA_SUCCESS;
m_minPulseWidth = MIN_PULSE_WIDTH;
m_maxPulseWidth = MAX_PULSE_WIDTH;
m_maxPeriod = MAX_PERIOD;
m_maxAngle = 180.0;
m_servoPin = pin;
m_pwmServoContext = mraa_pwm_init (m_servoPin);
m_currAngle = 180;
setAngle (0);
}
Servo::~Servo () {
mraa_pwm_close (m_pwmServoContext);
}
/*
* X = between (MIN_PULSE_WIDTH , MAX_PULSE_WIDTH)
*
* X usec
* _______
* |_______________________________________
* 20000 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) {
std::cout << "PWM context is NULL" << std::endl;
return MRAA_ERROR_UNSPECIFIED;
}
if (angle > m_maxAngle || angle < 0) {
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_enable (m_pwmServoContext, 0);
std::cout << "angle = " << angle << " ,pulse = " << calcPulseTraveling(angle) << ", cycles " << cycles << std::endl;
m_currAngle = angle;
}
/*
* Calculating relative pulse time to the value.
* */
int Servo::calcPulseTraveling (int value) {
// if bigger than the boundaries
if (value > m_maxAngle) {
return m_maxPulseWidth;
}
// if less than the boundaries
if (value < 0) {
return m_minPulseWidth;
}
// the conversion
return (int) ((float)m_minPulseWidth + ((float)value / m_maxAngle) * ((float)m_maxPulseWidth - (float)m_minPulseWidth));
}
void
Servo::setMinPulseWidth (int width) {
m_minPulseWidth = width;
}
void
Servo::setMaxPulseWidth (int width) {
m_maxPulseWidth = width;
}
void
Servo::setMaxPeriod (int width) {
m_maxPeriod = width;
}
int
Servo::getMinPulseWidth () {
return m_minPulseWidth;
}
int
Servo::getMaxPulseWidth () {
return m_maxPulseWidth;
}
int
Servo::getMaxPeriod () {
return m_maxPeriod;
}