/*
slowmove() - Just like write but at reduced speed.
value - Target position for the servo. Identical use as value of the function write.
speed - Speed at which to move the servo.
speed=0 - Full speed, identical to write
speed=1 - Minimum speed
speed=255 - Maximum speed
*/
void VarSpeedServo::slowmove(int value, uint8_t speed) {
// This fuction is a copy of write and witeMicroseconds but value will be saved
// in target instead of in ticks in the servo structure and speed will be save
// there too.
if (speed) {
if(value < MIN_PULSE_WIDTH) {
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if(value < 0) value = 0;
if(value > 180) value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel >= 0) && (channel < MAX_SERVOS) ) { // ensure channel is valid
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
// Set speed and direction
uint8_t oldSREG = SREG;
cli();
servos[channel].target = value;
servos[channel].speed = speed;
SREG = oldSREG;
}
}
else {
write (value);
}
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel >= 0) && (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
unsigned int status;
status = INTDisableInterrupts();
servos[channel].ticks = value;
INTRestoreInterrupts(status);
}
}
void VarSpeedServo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel >= 0) && (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value -= TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
uint8_t oldSREG = SREG;
cli();
servos[channel].ticks = value;
SREG = oldSREG;
// Extension for slowmove
// Disable slowmove logic.
servos[channel].speed = 0;
// End of Extension for slowmove
}
}
void ServoEx::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
u8 channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
// Changes to handle multiple servo group move
if (GroupMoveActiveCnt)
servos[channel].ticksPending = value;
else {
uint8_t oldSREG = SREG;
cli();
servos[channel].ticks = value;
SREG = oldSREG;
}
}
}
void Servo::writeMicroseconds(int value) {
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if (channel < MAX_SERVOS) { // ensure channel is valid
if (value < SERVO_MIN()) // ensure pulse width is valid
value = SERVO_MIN();
else if (value > SERVO_MAX())
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
servo_info[channel].ticks = value;
}
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if (value < SERVO_MIN()) // ensure pulse width is valid
value = SERVO_MIN();
else if (value > SERVO_MAX())
value = SERVO_MAX();
servos[channel].us = value;
}
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
servos[channel].ticks = value; // this is atomic on a 16bit uC, no need to disable Interrupts
}
}
void Servo::write(int value) {
if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < 0) value = 0;
if (value > 180) value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
this->writeMicroseconds(value);
}
void Servo_writeMicroseconds(byte servoIndex, int value)
{
if( (servoIndex < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - SERVO_TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
uint8_t oldSREG = SREG;
cli();
servos[servoIndex].ticks = value;
SREG = oldSREG;
}
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
CCTL0 = 0; // disable interrupt to avoid race condition
servos[channel].ticks = value;
CCTL0 = CCIE;
}
}
void SmartServo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
uint8_t oldSREG = SREG;
cli();
servos[channel].ticks = value;
SREG = oldSREG;
}
}
void Servo::writeMicroseconds(int value) {
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if (channel < MAX_SERVOS) { // ensure channel is valid
// ensure pulse width is valid
value = constrain(value, SERVO_MIN(), SERVO_MAX()) - (TRIM_DURATION);
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
CRITICAL_SECTION_START;
servo_info[channel].ticks = value;
CRITICAL_SECTION_END;
}
}
void Servo::write(int value, float speedFactor)
{
if(value < MIN_PULSE_WIDTH)
{ // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if(value < 0) value = 0;
if(value > 180) value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
value = min(SERVO_MAX_REAL(), value);
value = max(SERVO_MIN_REAL(), value);
}
this->writeMicroseconds(value);
servos[this->servoIndex].speedFactor = speedFactor;
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
#if defined(REL_GR_KURUMI)
servos[channel].ticks = value;
#else
noInterrupts();
servos[channel].ticks = value;
interrupts();
#endif
}
}
void MeServo::write(int value)
{
int delayTime = abs(value-this->read());
this->begin();
if(value < MIN_PULSE_WIDTH)
{ // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if(value < 0) value = 0;
if(value > 180) value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
this->writeMicroseconds(value);
delay(delayTime);
this->detach();
}
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 (!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);
}
/*
write(value, speed) - Just like write but at reduced speed.
value - Target position for the servo. Identical use as value of the function write.
speed - Speed at which to move the servo.
speed=0 - Full speed, identical to write
speed=1 - Minimum speed
speed=255 - Maximum speed
*/
void Servo::write(float value, uint8_t speed) {
// This fuction is a copy of write and writeMicroseconds but value will be saved
// in target instead of in ticks in the servo structure and speed will be save
// there too.
//double degrees = value;
if (speed) {
if (value < MIN_PULSE_WIDTH) {
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
// updated to use constrain instead of if, pva
value = value * 10;//make the value tem times bigger, must after the if() detection or if() will fail
value = constrain(value, 0, 1800);
value = map(value, 0, 1800, SERVO_MIN(), SERVO_MAX());
}
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel >= 0) && (channel < MAX_SERVOS) ) { // ensure channel is valid
// updated to use constrain instead of if, pva
//value = constrain(value, SERVO_MIN(), SERVO_MAX());
//Serial.println(value,DEC);
//value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
// Set speed and direction
uint8_t oldSREG = SREG;
cli();
servos[channel].target = value;
servos[channel].speed = speed;
SREG = oldSREG;
}
}
else {
write (value);
}
}
// returns current pulse width as an angle between 0 and 180 degrees
int Servo::read()
{
int value = map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
return value;
}
// return the value as degrees
int Servo::read() { return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
int Servo::read() // return the value as degrees
{
return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
}
