Thing-a-day, Day 23: Optical encoders for the Drawing Bot

For day 23, I added optical encoders to the drawing bot that was built on day 19. I also modified the program so that it uses the Servo library, which I only recently became aware of. The encoders work like a charm- they are hooked up to the interrupt input on the Arduino, and that part works fine. The problem, however, becomes that the continuous-rotation servo motors cannot be stopped fast enough, so they end up overshooting their destinations. I tried to get around this by cutting power to the servo motors when the device reaches position, however there still appears to be a good amount of mechanical energy in the system, so no dice. I’m giving up on the servos, and will switch to stepper motors for the next version. The code, which demonstrates how to use interrupts, is after break.

Update: I realised there is a bit of a bug in the source code. Direction is not taken into account when adding a new count request to the previous one. This can result in the error between moves being magnified rather than being corrected!

#include <SimpleMessageSystem.h>
#include <Servo.h>
 
/******************************************************************************************************
 *  Slightly more sophisticated Window washer drawing bot
 *
 * by Matt Mets
 * Created 19 Feb. 2008
 * Updated 23 Feb. 2008
 *  
 * This is a little driver routine to move around two continuous rotation servo motors,
 * for the purpose of moving a pen platform to draw on a whiteboard.  It seems to be loosing
 * counts sometimes.
 *
 * This code is released into the public domain.  Attribution is appreciated.
 *
 ******************************************************************************************************/
 
 
/***** Variable Definitions ***************************************************************************/
int encoderLPin = 2;        // Pins that the encoders are connected to
int encoderRPin = 3;        // (cannot be changed)
 
int motorLPin = 4;          // Pins that the motors are connected to
int motorRPin = 5;
 
int killMotorL = 6;         // Kill switches to stop the servos quickly (hopefully)
int killMotorR = 7;
 
 
int statusPin = 13;         // Status LED
 
Servo motorL, motorR;
int countLeft, countRight;
 
/***** Functions ************************************************************************************/
void setup()
{
  // Set up the two motor outputs
  motorL.attach(motorLPin);
  motorR.attach(motorRPin);
 
  // Set up the two encoder inputs
  attachInterrupt(0, doCountLeft, RISING);
  attachInterrupt(1, doCountRight, RISING);
 
  // The kill switch outputs (these control an NMOS transistor between the servo and ground)
  pinMode(killMotorL, OUTPUT);
  pinMode(killMotorR, OUTPUT);
  digitalWrite(killMotorL, LOW);
  digitalWrite(killMotorR, LOW);
 
  // And the output LED
  pinMode(statusPin, OUTPUT);
  digitalWrite(statusPin, HIGH);
 
  Serial.begin(9600);
}
 
/* Count the number of pulses seen by the left optical encoder */
void doCountLeft()
{
  countLeft--;
  if(countLeft == 0)
    digitalWrite(killMotorL, LOW);
}
 
/* Count the number of pulses seen by the right optical encoder */
void doCountRight()
{
  countRight--;
  if(countRight == 0)
    digitalWrite(killMotorR, LOW);
}
 
/* Move the motors at given speed for a duration of time
 * lMotorVal - speed of left motor (90 = stop)
 * rMotorVal - speed of right motor (90 = stop)
 * time - time to move for (100 ms intervals)
 */
void doMoveCommand(int speedLeft, int speedRight, int countsLeft, int countsRight)
{
  int position;
 
  Serial.println(countLeft);
  Serial.println(countRight);
 
  // There could be some contention here if the encoders move while these commands are running.
  // Interrupts= multiple threads= tricky!
  countLeft += countsLeft;
  countRight += countsRight;
 
  // Start the servos
  digitalWrite(killMotorL, HIGH);
  digitalWrite(killMotorR, HIGH);
 
  motorL.write(speedLeft);
  motorR.write(speedRight);
 
  // Keep running the loop until each servo has finished.
  while((countLeft > 0) || (countRight > 0))
  {
    if(countLeft <= 0) {
      digitalWrite(killMotorL, LOW);
    }
 
    if(countRight <= 0) {
      digitalWrite(killMotorR, LOW);
    }
 
    Servo::refresh();
  }
 
  delay(1000);
 
  Serial.println(countLeft);
  Serial.println(countRight);
}
 
void loop()
{
  char firstChar;
 
  if (messageBuild())
  { // Checks to see if the message is complete
    firstChar = messageGetChar(); // Gets the first word as a character
    if (firstChar = 'm')
    {
      int speedLeft = messageGetInt(); // Gets the next word an integer
      int speedRight = messageGetInt(); // Gets the next word an integer
      int countsLeft = messageGetInt();
      int countsRight = messageGetInt();
 
      doMoveCommand(speedLeft, speedRight, countsLeft, countsRight);
    }
  } 
}
This entry was posted in tech, thingaday. Bookmark the permalink.

Leave a Reply

Your email address will not be published. Required fields are marked *

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>