///
/// \brief Writes a value to a servo instance on it's attached pin
/// \details This will start a pulse on the attached pin that will cause a servo to
/// to go to the given angle unless it is a continuous rotation servo.
/// \param [in] value - the angle for the servo to turn to
///
void Servo::write(int value)
{
ULONG pulseMicroseconds;
if (!attached())
{
ThrowError(HRESULT_FROM_WIN32(ERROR_INVALID_STATE), "Error when calling write, servo is not attached.");
return;
}
// Value is in degrees from 0-180, convert it to microSeconds
if (value <= 0)
{
pulseMicroseconds = _min;
}
else if (value >= 180)
{
pulseMicroseconds = _max;
}
else
{
pulseMicroseconds = ( (((_max - _min) * value) + 90) / 180 ) + _min;
}
writeMicroseconds(pulseMicroseconds);
}
//=============================================================================
// FUNCTION: void write(uint16_t value)
//
// DESCRIPTION: Sets the servo angle in degrees.
// invalid angle that is valid as pulse in microseconds is
// treated as microseconds.
//
// INPUT: value - position for the servo to move to
//
// RETURNS: Nothing
//
//=============================================================================
void Servo8Bit::write(uint16_t value)
{
//make sure we have a valid servo number. If it's invalid then exit doing nothing.
if(myServoNumber == invalidServoNumber) return;
//for now, only accept angles, and angles that are between 0 and 200 degrees
if( value > 180 )
{
//treat this number as microseconds
writeMicroseconds( value );
}
else
{
//treat this number as degrees
uint16_t servoPulseLengthInUs = map(value, 0, 180, myMin, myMax);
writeMicroseconds( servoPulseLengthInUs );
}
}//end write
void Servo::write(int value)
{
if (value < 0)
value = 0;
else if (value > 180)
value = 180;
value = map(value, 0, 180, MIN_PULSE, MAX_PULSE);
writeMicroseconds(value);
}
void Servo::set188hz()
{
if (!this->is188hz)
{
// only changes if is different freq
this->is188hz = true;
// cypress - I2C
writeMicroseconds(DEFAULT_PULSE_WIDTH);
}
}
void Servo::write(int value)
{
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < MIN_PULSE_WIDTH) {
// assumed to be 0-180 degrees servo
value = max(0, min(180, value));
value = map(value, 0, 180, _minUs, _maxUs);
}
writeMicroseconds(value);
}
uint8_t Servo::attach(int pin, int min, int max)
{
// need to validate the pin
uint8_t list_index = 0;
bool is_valid_pin = false;
// let's check the boundaries
if (min < MIN_PULSE_WIDTH)
min = MIN_PULSE_WIDTH;
if (max > MAX_PULSE_WIDTH)
max = MAX_PULSE_WIDTH;
trace_debug("%s pin:%d min:%d max:%d\n", __func__, pin, min, max);
for (list_index = 0; list_index < MAX_NUMBER_OF_SERVOS; list_index++) {
if (pinData[list_index].pin == pin) {
is_valid_pin = true;
break;
}
}
if (!is_valid_pin) {
trace_error("invalid pin");
return INVALID_SERVO;
}
if (this->index < MAX_NUMBER_OF_SERVOS) {
// set as active
pinData[list_index].isActive = true;
this->pin = pin;
this->min = min;
this->max = max;
this->isAttached = true;
#ifdef SERVO_PWM_WITH_I2C
this->is188hz = true;
pinMode(pin, OUTPUT);
analogWrite(pin, 1);
writeMicroseconds(DEFAULT_PULSE_WIDTH);
#else
// sysfs
this->is188hz = false;
this->setPeriod(PWM_50Hz);
this->setDutyCycle(DEFAULT_PULSE_WIDTH * 1000);
this->enablePin();
#endif
}
trace_debug("%s Attached ok on pin:%d min:%d max:%d\n", __func__,
pin, this->min, this->max);
return this->index;
}
void Servo::write(int value)
{
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < MIN_PULSE_WIDTH) {
// assumed to be 0-180 degrees servo
value = constrain(value, 0, 180);
// writeMicroseconds will contrain the calculated value for us
// for any user defined min and max, but we must use default min max
value = improved_map(value, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}
writeMicroseconds(value);
}
///
/// \brief Writes a value to a servo instance on it's attached pin
/// \details This will start a pulse on the attached pin that will cause a servo to
/// to go to the given angle unless it is a continuous rotation servo.
/// \param [in] value - the angle for the servo to turn to
///
void Servo::write(int value)
{
if (!attached())
{
ThrowError("Error when calling write, servo is not attached.\n");
return;
}
// value is in angles and needs to be converted to microSeconds
if (value <= 0)
{
writeMicroseconds(_min);
}
else if (value >= 180)
{
writeMicroseconds(_max);
}
else
{
double alternateValue = (double) value / 180 * (_max - _min) + _min;
writeMicroseconds((int) alternateValue);
}
}
void Servo::write(int value)
{
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < MIN_PULSE_WIDTH)
{
if (value < 0)
value = 0;
else if (value > 180)
value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
writeMicroseconds(value);
}
void Servo::write(int16_t value)
{
// treat values less than min pulse width as angles in degrees
if (value < MIN_PULSE_WIDTH)
{
if (value < 0)
value = 0;
if(value > 180)
value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
writeMicroseconds(value);
}
void Servo::write(int value)
{
if (servoIndex >= MAX_SERVOS) return;
if (value >= MIN_PULSE_WIDTH) {
writeMicroseconds(value);
return;
} else if (value > 180) {
value = 180;
} else if (value < 0) {
value = 0;
}
if (servoIndex >= MAX_SERVOS) return;
servo_ticks[servoIndex] = map(value, 0, 180, min_ticks, max_ticks);
}
void Servo::write(int val)
{
// according to Arduino reference lib, if this angle will
// be bigger than 200, it should be considered as microsenconds
if (val < MIN_PULSE_WIDTH) {
// yeah.. user is passing angles
if (val < 0)
val = 0;
else if (val > 180)
val = 180;
trace_debug("it is an angle:%d this->min:%d this->max:%d\n",
val, this->min, this->max);
writeMicroseconds(map(val, MIN_ANGLE, MAX_ANGLE, this->min,
this->max));
}
else {
trace_debug("it is microseconds:%d\n", val);
// actually angle on this case it is microsencods
writeMicroseconds(val);
}
}
void Servo::write(int value)
{
if (!isThreadCreated) {
rtw_create_thread(&g_servo_task, (os_pthread)servo_handler, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
//flipper.attach(servo_handler, 0.02);
isThreadCreated = true;
}
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < MIN_PULSE_WIDTH)
{
if (value < 0)
value = 0;
else if (value > 180)
value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
writeMicroseconds(value);
}
