From ddecaa0b2cd33de0470e006167f6cbab40f65ea1 Mon Sep 17 00:00:00 2001 From: theroboticsheep Date: Fri, 17 Apr 2015 14:52:46 -0700 Subject: [PATCH 1/3] added speed controlled servo sweep --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 26 ++++--- .../src/libraries/ROSArduinoBridge/servos.h | 38 ++++++++-- .../src/libraries/ROSArduinoBridge/servos.ino | 74 +++++++++++++++++++ 3 files changed, 120 insertions(+), 18 deletions(-) create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index ff8bfe1..466dd19 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -45,8 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#define USE_BASE // Enable the base controller code -//#undef USE_BASE // Disable the base controller code +//#define USE_BASE // Enable the base controller code +#undef USE_BASE // Disable the base controller code /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE @@ -63,8 +63,8 @@ //#define ARDUINO_ENC_COUNTER #endif -//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h -#undef USE_SERVOS // Disable use of PWM servos +#define USE_SERVOS // Enable use of PWM servos as defined in servos.h +//#undef USE_SERVOS // Disable use of PWM servos /* Serial port baud rate */ #define BAUDRATE 57600 @@ -184,11 +184,11 @@ int runCommand() { break; #ifdef USE_SERVOS case SERVO_WRITE: - servos[arg1].write(arg2); + servos[arg1].setTargetPosition(arg2); Serial.println("OK"); break; case SERVO_READ: - Serial.println(servos[arg1].read()); + Serial.println(servos[arg1].getServo().read()); break; #endif @@ -268,7 +268,7 @@ void setup() { #ifdef USE_SERVOS int i; for (i = 0; i < N_SERVOS; i++) { - servos[i].attach(servoPins[i]); + servos[i].initServo(servoPins[i], servoInitPosition[i]); } #endif } @@ -330,12 +330,14 @@ void loop() { setMotorSpeeds(0, 0); moving = 0; } +#endif +// Sweep servos +#ifdef USE_SERVOS + int i; + for (i = 0; i < N_SERVOS; i++) { + servos[i].doSweep(); + } #endif } - - - - - diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h index 4b97050..ff80420 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h @@ -1,9 +1,35 @@ -/* Define the attachment of any servos here. - The example shows two servos attached on pins 3 and 5. -*/ +#ifndef SERVOS_H +#define SERVOS_H -#define N_SERVOS 2 -Servo servos [N_SERVOS]; -byte servoPins [N_SERVOS] = {3, 5}; +#define N_SERVOS 1 +// This delay in milliseconds determines determines the pause +// between each one degree step the servo travels. Increasing +// this number will make the servo sweep more slowly. +// Decreasing this number will make the servo sweep more quickly. +#define SWEEP_COMMAND_INTERVAL 150 // ms + +byte servoPins [N_SERVOS] = { 3 }; +byte servoInitPosition [N_SERVOS] = { 90 }; // degrees + + +class SweepServo +{ + public: + SweepServo(); + void initServo(int servoPin, int initPosition); + void doSweep(); + void setTargetPosition(int position); + Servo getServo(); + + private: + Servo servo; + int currentPositionDegrees; + int targetPositionDegrees; + long lastSweepCommand; +}; + +SweepServo servos [N_SERVOS]; + +#endif diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino new file mode 100644 index 0000000..fc68f60 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino @@ -0,0 +1,74 @@ +/*************************************************************** + Servo Sweep - by Nathaniel Gallinger + + Sweep servos one degree step at a time with a user defined + delay in between steps. Supports changing direction + mid-sweep. Important for applications such as robotic arms + where the stock servo speed is too fast for the strength + of your system. + + *************************************************************/ + +#ifdef USE_SERVOS + + +// Constructor +SweepServo::SweepServo() +{ + this->currentPositionDegrees = 0; + this->targetPositionDegrees = 0; + this->lastSweepCommand = 0; +} + + +// Init +void SweepServo::initServo(int servoPin, int initPosition) +{ + this->servo.attach(servoPin); + this->currentPositionDegrees = initPosition; + this->targetPositionDegrees = initPosition; + this->lastSweepCommand = millis(); +} + + +// Perform Sweep +void SweepServo::doSweep() +{ + + // Get ellapsed time + int delta = millis() - this->lastSweepCommand; + + // Check if time for a step + if (delta > SWEEP_COMMAND_INTERVAL) { + // Check step direction + if (this->targetPositionDegrees > this->currentPositionDegrees) { + this->currentPositionDegrees++; + this->servo.write(this->currentPositionDegrees); + } + else if (this->targetPositionDegrees < this->currentPositionDegrees) { + this->currentPositionDegrees--; + this->servo.write(this->currentPositionDegrees); + } + // if target == current position, do nothing + + // reset timer + this->lastSweepCommand = millis(); + } +} + + +// Set a new target position +void SweepServo::setTargetPosition(int position) +{ + this->targetPositionDegrees = position; +} + + +// Accessor for servo object +Servo SweepServo::getServo() +{ + return this->servo; +} + + +#endif From eda206f68c2cf6a4426004787c304610f16a8fbc Mon Sep 17 00:00:00 2001 From: theroboticsheep Date: Fri, 17 Apr 2015 15:04:23 -0700 Subject: [PATCH 2/3] changed default sweep delay to zero so that servos will behave normally unless delay is increased --- ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h index ff80420..f0f404e 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h @@ -8,7 +8,7 @@ // between each one degree step the servo travels. Increasing // this number will make the servo sweep more slowly. // Decreasing this number will make the servo sweep more quickly. -#define SWEEP_COMMAND_INTERVAL 150 // ms +#define SWEEP_COMMAND_INTERVAL 0 // ms byte servoPins [N_SERVOS] = { 3 }; byte servoInitPosition [N_SERVOS] = { 90 }; // degrees From e9810841b48f0eb95362657bbe94cf1ae5823bf5 Mon Sep 17 00:00:00 2001 From: theroboticsheep Date: Sat, 18 Apr 2015 18:29:53 -0700 Subject: [PATCH 3/3] added support for rotating servos at different speeds --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 15 +++++++----- .../src/libraries/ROSArduinoBridge/servos.h | 23 +++++++++++++------ .../src/libraries/ROSArduinoBridge/servos.ino | 8 +++++-- 3 files changed, 31 insertions(+), 15 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 466dd19..5de3726 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -265,12 +265,15 @@ void setup() { #endif /* Attach servos if used */ -#ifdef USE_SERVOS - int i; - for (i = 0; i < N_SERVOS; i++) { - servos[i].initServo(servoPins[i], servoInitPosition[i]); - } -#endif + #ifdef USE_SERVOS + int i; + for (i = 0; i < N_SERVOS; i++) { + servos[i].initServo( + servoPins[i], + stepDelay[i], + servoInitPosition[i]); + } + #endif } /* Enter the main loop. Read and parse input from the serial port diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h index f0f404e..d2b6d21 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h @@ -2,29 +2,38 @@ #define SERVOS_H -#define N_SERVOS 1 +#define N_SERVOS 2 -// This delay in milliseconds determines determines the pause +// This delay in milliseconds determines the pause // between each one degree step the servo travels. Increasing // this number will make the servo sweep more slowly. -// Decreasing this number will make the servo sweep more quickly. -#define SWEEP_COMMAND_INTERVAL 0 // ms +// Decreasing this number will make the servo sweep more quickly. +// Zero is the default number and will make the servos spin at +// full speed. 150 ms makes them spin very slowly. +int stepDelay [N_SERVOS] = { 0, 0 }; // ms -byte servoPins [N_SERVOS] = { 3 }; -byte servoInitPosition [N_SERVOS] = { 90 }; // degrees +// Pins +byte servoPins [N_SERVOS] = { 3, 4 }; + +// Initial Position +byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees class SweepServo { public: SweepServo(); - void initServo(int servoPin, int initPosition); + void initServo( + int servoPin, + int stepDelayMs, + int initPosition); void doSweep(); void setTargetPosition(int position); Servo getServo(); private: Servo servo; + int stepDelayMs; int currentPositionDegrees; int targetPositionDegrees; long lastSweepCommand; diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino index fc68f60..5ba344c 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino @@ -22,9 +22,13 @@ SweepServo::SweepServo() // Init -void SweepServo::initServo(int servoPin, int initPosition) +void SweepServo::initServo( + int servoPin, + int stepDelayMs, + int initPosition) { this->servo.attach(servoPin); + this->stepDelayMs = stepDelayMs; this->currentPositionDegrees = initPosition; this->targetPositionDegrees = initPosition; this->lastSweepCommand = millis(); @@ -39,7 +43,7 @@ void SweepServo::doSweep() int delta = millis() - this->lastSweepCommand; // Check if time for a step - if (delta > SWEEP_COMMAND_INTERVAL) { + if (delta > this->stepDelayMs) { // Check step direction if (this->targetPositionDegrees > this->currentPositionDegrees) { this->currentPositionDegrees++;