Digital & Embedded Systems

Tutorial 7 - Implementation

Now that the equations of motion for the differential drive robot have been introduced, the issue of implementation can be addressed. Lets us first define the coordinate system for the robot (this is the local coordinate system). We will choose the origin to lie at the center of the motor axis with the positive x-axis pointing towards the front of the robot and the y-axis lying along the motor axis. This is illustrated in the figure below.

Robot Local Coordinate System

 

For the last laboratory, you were required to program a PID controller to control the velocity of one motor. Let assume that you chose a object oriented approach and defined a structure for the PID controller. This would probably look something like

  typedef struct{

      int setpoint;
      int value;

      double Kp;
      double Ki;
      double Kd;

      int error;
      int error_sum;
      int error_last;

  } PID;

You should have also defined functions for modifying the gains, setting the setpoint, and calculating the control signal. These functions might be called

  void PIDSetGains(PID *pid, double Kp,
                   double Ki, double Kd);

  void PIDSetPoint(PID *pid, int setpoint);

  int  PIDCalc(PID *pid, int current_velocity);

Notice that the PID controller structure has no variable to hold the motor or quad handles. This is desirable because the controller is not limited to DC motor control, it can conceivably be applied to any process we want to control. However, because the PID controller doesn't explicitly control the motors, you probably have defined another, more specific, structure for the DC motors. This might look something like

  typedef struct{
       
      MotorHandle motor;
      QuadHandle  quad;

      PID ctrl;
   
  } DCVelocityCtrl;

Keeping with the object-oriented approach, you now should have defined functions to operate on this structure. The definitions for the operations most likely to be performed, initialisation, modifying the setpoint, and calculating the motor signal might have been written as

  void DCVInit(DCVelocityCtrl *ctrl,
               DeviceSemantics motor,
               DeviceSemantics quad);

  void DCVSetpoint(DCVelocityCtrl *ctrl,
                   int setpoint);

  void DCVCalc(DCVelocityCtrl *ctrl);

 

For this laboratory, you are required to extend the control to both motors in order to make the robot drive straight and turn about its origin. In order to accomplish this, while using what you have already coded, lets define another structure called 'Turtle' after the turtle graphics programs of the the late eighties. This structure will contain information about the robot's current position, its desired position and the structure will also contain the velocity controllers for the two wheels.

  typedef struct{

      int current_x;
      int current_y;
      int current_phi;

      int final_x;
      int final_y;
      int final_phi;

      DCVelocityCtrl wheel[2];

  } Turtle;

Like the PID and DCVelocityCtrl structures, you will need to write functions to operate on/with the Turtle structure. You might call these functions

  void TRTLDrive(Turtle *t, int velocity);
  void TRTLSpin(Turtle *t, int angle_vel);

These function are very similar, and only have one main difference which is the sign of the desired velocity for the motor. After you have coded and tested these two functions, you can code two more functions. The first should drive the robot to a location relative to the robots location when the function is called. The second should drive the robot to a location relative to the global coordinate system. These might be defined like

  void TRTLGo(Turtle *t, int x, int y, double phi);

  void TRTLGoTo(Turtle *t, int x,
                int y, double phi);

Remember for the Turtle, to get to another location, follow these steps

• Turn robot so that it is facing the desired location
• Drive straight to the desired location
• Turn so that orientation is at the desired orientation

You can implement a bang-bang style controller for both orientation and distance. Below is some pseudocode for the bang-bang style controller.

  Turtle trtl; // ONLY GLOBAL VARIABLE

  void IRQ_Callback(void)
  {
      int margin;
      int radius;

      /* First check for angle */
      if (current_angle < desired_angle-margin)
          TRTLSpin(positive_value);
      else if (current_angle > desired_angle+margin)
          TRTLSpin(negative_value);
      
      /* Now check for distance */
      else{
          if(distance_to_point > radius)
              TRTLDrive(positive_value);
      }
  }

This does not include everything you will need for your Turtle contoller, you will also need to determine if the robot should drive backward (Hint: apply the fact that the cosine of the angle between to vectors is equal to the dot product of the vectors divided by the magnitudes). Also all the trigonometric functions you will need can be found in <math.h>.