I have been playing with servos, and I needed two features that the servo libraries do not provide; servo trim and servo reversing. Each physical dis-assembly and reassembly puts the servos at a slightly different position, so servo trimming is required. And it would be very desirable to have the ability to reverse servos to have them operate in the same direction, regardless of servo orientation.
I know that I can write code in the main program to deal with this, but I would prefer that these be encapsulated within the servo functionality.
So, I modified the servo libraries so that each servo has 2 additional properties, reverse and trim, and two additional methods, setreverse() and settrim().
This will (in my understanding) make each servo about 3 bytes longer, and each servowrite() call a few instructions longer.
Code included. I tried to include comments with my initials (vjh) describing each change.
I did test this and am getting the results I desire.
Questions I have:
- Am I insane for changing the servo library?
- Do you see anything I have done that will be a 'gotcha'?
My setup and loop
#include <math.h>
#include <Servo.h>
//prototypes
void waitforcharacter(int terminationchar);
Servo ServoA; //initialize
Servo ServoB; //initialize
Servo ServoC; //initialize
char buffer[6];
int received;
uint8_t reverse;
int trim;
void setup()
{
int i;
ServoA.attach(7, 700, 2000);
ServoB.attach(8, 700, 2300);
ServoC.attach(9, 1100, 2300);
Serial.begin(9600);
trim = false;
reverse=0;
}
void loop()
{
Serial.begin(9600);
ServoA.writeMicroseconds(1500); ServoB.writeMicroseconds(1500); ServoC.writeMicroseconds(1500);
Serial.println ("1500");
waitforcharacter(92); //wait for a backslash to be entered
ServoA.writeMicroseconds(1700); ServoB.writeMicroseconds(1700); ServoC.writeMicroseconds(1700);
Serial.println ("1700");
waitforcharacter(92); //wait for a backslash to be entered
ServoA.writeMicroseconds(1900); ServoB.writeMicroseconds(1900); ServoC.writeMicroseconds(1900);
Serial.println ("1900");
waitforcharacter(92); //wait for a backslash to be entered
if (reverse)
{
reverse=false;
trim=100;
Serial.println ("switching ServoA to be reversed and have a trim of 100");
ServoA.setreverse(reverse);
ServoA.settrim(trim);
}
else
{
reverse=true;
trim=-100;
Serial.println ("switching ServoA to be normal and have a trim of -100");
ServoA.setreverse(reverse);
ServoA.settrim(trim);
}
}
My servo.h
/*
Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
vjh snip comments for forum posting space
*/
#ifndef Servo_h
#define Servo_h
#include <inttypes.h>
#define Servo_VERSION 2 // software version of this library
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached
#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds
#if defined(__AVR_ATmega1280__)
#define MAX_SERVOS 48 // the maximum number of servos (valid range is from 1 to 48)
#else
#define MAX_SERVOS 12 // this library supports up to 12 on a standard Arduino
#endif
#define INVALID_SERVO 255 // flag indicating an invalid servo index
typedef struct {
uint8_t nbr :6 ; // a pin number from 0 to 63
uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false
} ServoPin_t ;
typedef struct {
ServoPin_t Pin;
unsigned int ticks;
uint8_t reverse; // vjh property added to handle reversing flag 0 is normal and 1 is reversed
int trim; // vjh property added to handle trim
} servo_t;
class Servo
{
public:
Servo();
uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
void detach();
void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds
void writeMicroseconds(int value); // Write pulse width in microseconds
int read(); // returns current pulse width as an angle between 0 and 180 degrees
int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
bool attached(); // return true if this servo is attached, otherwise false
void setreverse(int value); // vjh add method to handle servo reverse flag 0 is normal and 1 is reversed
void settrim(int value); // vjh add method to handle servo trim
private:
uint8_t servoIndex; // index into the channel data for this servo
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH
};
#endif
Mp servo.cpp (relevant pieces only, the file is too large for a single post)
uint8_t Servo::attach(int pin, int min, int max)
{
if(this->servoIndex < MAX_SERVOS ) {
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
this->max = (MAX_PULSE_WIDTH - max)/4;
servos[this->servoIndex].reverse = false; //vjh
servos[this->servoIndex].trim = 0; //vjh
// initialize the timer if it has not already been initialized
servoTimer_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if(isTimerActive(timer) == false)
initISR(timer);
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
}
return this->servoIndex ;
}
void Servo::setreverse(int value)
{ //vjh add method to set the reverse 0 for normal, 1 for reverse
servos[this->servoIndex].reverse= value;
}
void Servo::settrim(int value)
{ //vjh add method to set the trim + or - millis
servos[this->servoIndex].trim= 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 (servos[channel].reverse)
value = map(value,600,2400,2400,600); //vjh if reverse flag is set, swap the value
value += servos[channel].trim; //vjh add the servo trim to the value
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = (value-TRIM_DURATION) * TICKS_PER_uS; // convert to ticks after compensating for interrupt overhead
uint8_t oldSREG = SREG;
cli();
servos[channel].ticks = value;
SREG = oldSREG;
}
}
