Ardros – Controller and Drive System

Ardros (Arduino & ROS) is an experimental robot platform that I am working on to get familiar with ROS and particularly with the navigation stack that comes with ROS. My previous post focused on the Python code running on the PC that acts as a gateway to the Arduino controller. In the following I  cover the (differential) drive system and the Arduino controller code.

The main components that make up the drive system are:

The controller code is available from Google code (http://code.google.com/p/drh-robotics-ros/source/browse/#svn%2Ftrunk) in the sub folder ‘Arduino’. This post discusses the state as of Subversion revision 58. I expect that the code will further evolve over time.

The main file is \Arduino\Robot\Robot.pde. It leverages a couple of libraries (source code in \Arduino\libraries):

  • Messenger: For handling messages sent from the PC through the USB link
  • TimeInfo: Helper class for keeping track of time
  • RobotParams: Holds the parameter values that are required to determine the distance traveled / turned from the encoder ticks. The parameters are: WheelDiameter, TrackWidth, CountsPerRevolution, DistancePerCount, RadiansPerCount
  • OdometricLocalizer: It is responsible for determining the pose (x, y position and heading) of the robot based on the encoder counts.
  • BatteryMonitor: A simple wrapper class that encapsulates the scaling of an analog input value to get the current battery voltage. It also determines whether the battery voltage is too low. This information is used by the speed controller (see below) and causes it to stop the motors if the voltage is too low.
  • SpeedController: It uses three PI (proportional & integral) controllers to translate commanded velocity (velocity in x direction and angular velocity) to motor control values for the left and right motors. The speed controller implements a slightly modified version of the coupled PI control structure discussed here. The document illustrates a structure that only uses a P (proportional) controller that links the two motors and ensures that they are kept in sync:
    CoupledPIControlls
    In my code I use a PI (proportional & integral) controller instead. I.e., Kp_turn is accompanied by a Ki_turn. In my case it was not necessary to implement the boxes labeled ‘friction_comp’.
    The speed controller also implements a command timeout. If no commands are received for more than the specified timeout then the controller stops the motors.

After going through setup() the code in the loop() function of \Arduino\Robot\Robot.pde first checks whether all components have been initialized.

void loop()
{
  ReadSerial();

  unsigned long milliSecsSinceLastUpdate = millis() - _TimeInfo.LastUpdateMillisecs;
  if(milliSecsSinceLastUpdate >= c_UpdateInterval)
  {
    //Serial.println(milliSecsSinceLastUpdate);
    // time for another update
    _TimeInfo.Update();
    if (_IsInitialized)
    {
      DoWork();
    }
    else
    {
      RequestInitialization();
    }
  }
}

If at least some of the components have not yet been initialized then initialization is requested by sending corresponding messages to the computer:

void RequestInitialization()
{
    _IsInitialized = true;

    if (!_RobotParams.IsInitialized)
    {
      _IsInitialized = false;

      Serial.print("InitializeDriveGeometry"); // requesting initialization of the parameters of the differential drive needed for odometry calculations
      Serial.print("\n");
    }

    if (!_SpeedController.IsInitialized)
    {
      _IsInitialized = false;

      Serial.print("InitializeSpeedController"); // requesting initialization of the speed controller
      Serial.print("\n");
    }

    if (!_BatteryMonitor.IsInitialized)
    {
      _IsInitialized = false;

      Serial.print("InitializeBatteryMonitor"); // requesting initialization of the battery monitor
      Serial.print("\n");
    }
}

The code running on the PC needs to react to these requests by sending down the initialization parameters which are handled by the Messenger class:

void OnMssageCompleted()
{
 if (_Messenger.checkString("s"))
 {
 SetSpeed();
 return;
 }

 if (_Messenger.checkString("DriveGeometry"))
 {
 InitializeDriveGeometry();
 return;
 }

 if (_Messenger.checkString("SpeedControllerParams"))
 {
 InitializeSpeedControllerParams();
 return;
 }

 if (_Messenger.checkString("BatteryMonitorParams"))
 {
 InitializeBatteryMonitor();
 }

 // clear out unrecognized content
 while(_Messenger.available())
 {
 _Messenger.readInt();
 }
}

Only after all components are initialized, odometry information is sent up and velocity commands are acted upon in the DoWork() function:

void DoWork()
{
  _OdometricLocalizer.Update(_LeftEncoderTicks, _RightEncoderTicks);
  _BatteryMonitor.Update();
  _SpeedController.Update(_BatteryMonitor.VoltageIsTooLow);
  IssueCommands();

  Serial.print("o"); // o indicates odometry message
  Serial.print("\t");
  Serial.print(_OdometricLocalizer.X, 3);
  Serial.print("\t");
  Serial.print(_OdometricLocalizer.Y, 3);
  Serial.print("\t");
  Serial.print(_OdometricLocalizer.Heading, 3);
  Serial.print("\t");
  Serial.print(_OdometricLocalizer.V, 3);
  Serial.print("\t");
  Serial.print(_OdometricLocalizer.Omega, 3);
  Serial.print("\n");

  Serial.print("b\t"); // o indicates battery info message
  Serial.print(_BatteryMonitor.BatteryVoltage, 3);
  Serial.print("\t");
  Serial.print(_BatteryMonitor.VoltageIsTooLow);
  Serial.print("\n");
}

void IssueCommands()
{
  float normalizedRightMotorCV, normalizedLeftMotorCV;

  normalizedRightMotorCV = _SpeedController.NormalizedLeftCV;
  normalizedLeftMotorCV = _SpeedController.NormalizedRightCV;

  /*
  Serial.print("Speed: ");
  Serial.print(_SpeedController.DesiredVelocity);
  Serial.print("\t");
  Serial.print(_SpeedController.DesiredAngularVelocity);
  Serial.print("\t");
  Serial.print(_SpeedController.LeftError);
  Serial.print("\t");
  Serial.print(_SpeedController.RightError);
  Serial.print("\t");
  Serial.print(_SpeedController.TurnError);
  Serial.print("\t");
  Serial.print(normalizedRightMotorCV);
  Serial.print("\t");
  Serial.print(normalizedLeftMotorCV);
  Serial.print("\n");
  */

  float rightServoValue = mapFloat(normalizedRightMotorCV, -1, 1, 90.0 - c_MaxMotorCV, 90.0 + c_MaxMotorCV);     // scale it to use it with the servo (value between 0 and 180)
  float leftServoValue = mapFloat(normalizedLeftMotorCV, -1, 1, 90.0 - c_MaxMotorCV, 90.0 + c_MaxMotorCV);     // scale it to use it with the servo (value between 0 and 180)

  _RightServo.write(rightServoValue);     // sets the servo position according to the scaled value (0 ... 179)
  _LeftServo.write(leftServoValue);     // sets the servo position according to the scaled value (0 ... 179)
}

Hopefully this somewhat explains the flow of data. In order to be able to participate in the ROS navigation stack the key aspects are that

  1. Velocity commands are accepted and acted upon. They arrive at the controller as messages prefixed with ‘s:’ for ‘speed’.
  2. Odometry information is sent back. This includes pose data (x, y, heading) as well as speed data (translational velocity and rotational velocity).

We are now in the position to do dead reckoning – but this will need to wait for the next blog entry.

Disclaimer: It turns out that the Arduino is taxed to its very limit by my controller program. This is caused by the combination of

  • The very high number of interrupts originating from the high resolution quadrature encoders mounted directly to the motor shafts.
    For details see my blog entry Quadrature encoder too fast for Arduino.
  • The trigonometric calculations performed to obtain the odometry data
  • The bidirectional communication between the controller and the PC.

As a result the faster the motors run the more wheel encoder transitions are skipped. It looks like the interrupts are suppressed in this overload scenario. As long as the robot moves slowly (< 1m/s in my case) the detected number of wheel encoder transitions is accurate enough. I plan to upgrade to a more powerful controller soon.

12 thoughts on “Ardros – Controller and Drive System”

  1. Dr. Rainer.

    Would you mind verifying how your controller and drive system are wired?
    Right Motor: values > 90 are forward
    Right Encoder: forward direction increments
    Left Motor: values > 90 are also forward (Polarity reversed on motor)
    Left Encoder: forward direction increments

    Thanks
    – James

  2. DR Rainer
    really nice work on design,have most of the parts and ordered a maple leaf board for another project ,but looking to try your first,do have the code for the LEAF maple board

  3. Dr. Rainer.
    Would you mind explaining what the speed controller variables “velocityPParam”, “velocityIParam”, ” turnIParam “, and ” turnPParam ” stands for?

  4. Respected Sir,

    I am getting this following errors while compiling the Robot.pde in Arduino IDE…..can you kindly guide me what is the problem here and how can I solve this….

    sketch_sep03a.cpp:1:22: error: WProgram.h: No such file or directory
    In file included from sketch_sep03a.cpp:3:
    /root/sketchbook/libraries/TimeInfo/TimeInfo.h: In constructor ‘TimeInfo::TimeInfo()’:
    /root/sketchbook/libraries/TimeInfo/TimeInfo.h:26: error: ‘micros’ was not declared in this scope
    /root/sketchbook/libraries/TimeInfo/TimeInfo.h: In member function ‘void TimeInfo::Update()’:
    /root/sketchbook/libraries/TimeInfo/TimeInfo.h:34: error: ‘micros’ was not declared in this scope
    /root/sketchbook/libraries/TimeInfo/TimeInfo.h:35: error: ‘millis’ was not declared in this scope
    In file included from sketch_sep03a.cpp:4:
    /root/sketchbook/libraries/OdometricLocalizer/OdometricLocalizer.h: In member function ‘void OdometricLocalizer::Update(long int, long int)’:
    /root/sketchbook/libraries/OdometricLocalizer/OdometricLocalizer.h:61: error: ‘cos’ was not declared in this scope
    /root/sketchbook/libraries/OdometricLocalizer/OdometricLocalizer.h:62: error: ‘sin’ was not declared in this scope
    In file included from sketch_sep03a.cpp:5:
    /root/sketchbook/libraries/SpeedController/SpeedController.h: In member function ‘void SpeedController::Initialize(float, float, float, float, float)’:
    /root/sketchbook/libraries/SpeedController/SpeedController.h:115: error: ‘millis’ was not declared in this scope
    /root/sketchbook/libraries/SpeedController/SpeedController.h: In member function ‘void SpeedController::CommandVelocity(float, float)’:
    /root/sketchbook/libraries/SpeedController/SpeedController.h:124: error: ‘millis’ was not declared in this scope
    /root/sketchbook/libraries/SpeedController/SpeedController.h: In member function ‘void SpeedController::Update(bool)’:
    /root/sketchbook/libraries/SpeedController/SpeedController.h:137: error: ‘millis’ was not declared in this scope
    /root/sketchbook/libraries/SpeedController/SpeedController.h:159: error: ‘constrain’ was not declared in this scope
    In file included from sketch_sep03a.cpp:6:
    /root/sketchbook/libraries/BatteryMonitor/BatteryMonitor.h: In member function ‘void BatteryMonitor::Update()’:
    /root/sketchbook/libraries/BatteryMonitor/BatteryMonitor.h:49: error: ‘analogRead’ was not declared in this scope

  5. Hi abhijit

    I have this similar issue. If you are using Arduino IDE > 1.0 ,then you need to replace Wprogram.h by Arduino.h. Try and give feedback

    Regards
    Lentin

  6. Dr.Rainer,
    first off i would like to thank you for your amazing blog!its more like a treasure than a blog for me!im actually intending to build my own robot with the ardros and a kinect..im currently building the hardware..i was wondering if you have a link where to get the pittman gearhead motors you used?or if they can be replaced with other types of D.C motors?your suggestions would really help.
    Thanks in advance,
    Aws

  7. Hi Aws,

    Thanks for the kind words. I got the motors from surplus and they sold out quickly. However, the motors can easily be replaced with others. Make sure that your motor comes with a quadrature encoder and is geared down to a speed that works with your wheel size.

    Thanks,
    Rainer

  8. Dr. Rainer,

    I am trying to implement this kind of ros implementation on my bachelor project that i already finished. My robot also use differntial wheel drive and have a pc motherboard mounted and connected to arduino to control the dc motor and read the encoder output.

    I already try to read and understand your robot.pde. I need to change the program a little bit because of the difference in the hardware. What i want to ask is about your encoder. What i understand is that you are using pittmann encoder with 8820 DC Motor Series with the compatible encoder from pittman. I already checked the datasheet and the type of the encoder available. It is written on the datasheet that the encoder is 2 channel, with A and B signals or it could be 2 channel and one index. As i understand, the index signal will give one pulse for each revolution. What i do not understand is in your robot.pde you have 3 pins definition for each encoder:

    // Quadrature encoders
    // Left encoder
    #define c_LeftEncoderInterrupt 4
    #define c_LeftEncoderPinA 19
    #define c_LeftEncoderPinB 25

    which means that you use the index signal for the interrupt, which concludes in the interrupt is called per one revolution, am i right? On my Hardware i use three channel encoder with A, B, and Z. I need to modify the program to compensate this difference. Thank you in advance.

    Regards,

    Alfredo Yohanes

  9. Dr. Rainer,

    I just realized that the definition of the interrupt is the number of the interrupt on arduino mega which matched the number of Pin A of each encoder. don’t bother for my last comments. Is there any reason why do you put the encoder pin on those interrupts, do you use the first two interrupt for something else?
    Thank you,

    Alfredo Yohanes

  10. Dr. Rainer,

    I already read your program and see that you use the pin 2 and 3 to connect to your dc motor controller, do not bother my previous comment again, thanks!!

Leave a Reply

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