added speed controlled servo sweep

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
 }
 
-
-
-
-
-

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

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