int setAngle(int angle, int pin) {
if (angle > m_maxAngle || angle < 0) {
return MRAA_ERROR_UNSPECIFIED;
}
if (pin == pin3) {
mraa_pwm_enable(m_pwmServoContext, 1);
mraa_pwm_period_us(m_pwmServoContext, m_period);
mraa_pwm_pulsewidth_us(m_pwmServoContext, calcPulseTraveling(angle));
}
if (pin == pin9) {
mraa_pwm_enable(m_pwmServoContext1, 1);
mraa_pwm_period_us(m_pwmServoContext1, m_period);
mraa_pwm_pulsewidth_us(m_pwmServoContext1, calcPulseTraveling(angle));
}
if (m_waitAndDisablePwm) {
sleep(1);
haltPwm();
}
m_currAngle = angle;
return MRAA_SUCCESS;
}
VexMotorController::~VexMotorController()
{
haltPwm();
mraa_pwm_close(m_pwmServoContext);
}
