alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fix Servo library so that it correctly moves to the given angle.

Browse Source 
Signed-off-by: John Van Drasek <john.r.van.drasek@intel.com>
This commit is contained in:
Chris Sarbora 2015-02-12 16:28:13 -08:00 committed by John Van Drasek
parent 73be07df74
commit e0e9405634
2 changed files with 89 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/servo/servo.cxx
View File

@ -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);
}

50
src/servo/servo.h
View File

@ -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);
};
}