OOPic Robotics Lesson F: Closed Loop Wheel Position Control using Virtual Circuits

Web	www.PhilBot.com

My other sites

OurCoolHouse

Phil's Resume

Web Portfolio

[Home] [Projects] [Hardware] [Software] [Books] [Links] [Downloads]

BOB: Software: Lesson F - Closed Loop Wheel Position Control using Virtual Circuits. [Back to BOB Software] [Back to BOB index]

This is the first lesson that focuses heavily on Virtual Circuits (VC's).

VC's are a cool feature of the OOPic that enables high priority actions to take place independently of the main program loop. VC objects are usually more to do with processing, and less to do with hardware interfacing. A classic minimal VC object is the oWire object. It is used to connect one digital flag to another. You might use an oWire to connect one digital input (eg: a switch) to a different digital output (eg: an LED). From then onwards, whenever the switch input changed, the LED would automatically follow. This would happen regardless of whatever was happening in the main program.

In BOB's case, I thought it would be handy to be able to tell the motor drive to move a certain number of "clicks", and then go on with the program and let the drive take care of itself. All I'd need then is a way to know when the drive had done it's job. So I started assembling a list of likely Objects that I could use to do what I wanted. I already had the oDCMotor2 and oQEncoder under control, so I just needed some way to be able to stop the motor automatically. I figured I could use one of the oCompare objects to monitor the encoder clicks and trigger the motor to stop when the correct number had occurred.

Based on my prior experimentation I have come to realize that Object Memory is scarce (just 86 bytes) so all effort should be made to minimize the number of bytes used for any function.. I started out thinking that I could have a "Setpoint" object that the oCompare compared with the current oQEncoder value, but I then I realized that a "cheaper" way would be to preload the encoder with the desired movement value and let it count down to zero (saving 3 bytes per wheel). An oCompare0 object could then be used to compare the value with zero. OK, so once we know the wheel has turned far enough, how do we stop the motor? My solution was to use the oEvent object. This cool object acts like an interrupt that can run any piece of code when it's triggered. The code that gets run is a function whose name is based on the name of the oEvent object. eg: if the event object is called "LeftAutoStop" then the the interrupt function MUST be called "LeftAutoStop_Code".

OK, so here's how it breaks down. Let's say I want to move the left wheel forward 10 inches (or 50 clicks of the wheel encoder) at speed 60. I call a function to preload the Left encoder with -50 and then set the motor speed to 60. I then go along with my business. In the background, the motor will start running and the encoder will start counting UP to zero. When it hits zero, the oCompare0 Object will signal a "Between" condition which will trigger the LeftAutoStop oEvent to call the LeftAutoStop_Code which will shut down the motor drive. Bingo. Closed loop position control using Virtual Circuits. One reason for using Virtual Circuits here is that they operate many hundreds of times faster than the main program code, so we can be assured that they will immediately detect the "end of motion" condition. The other reason is that you probably want your program to focus on other actions like remembering where you are, or locating targets or avoiding objects or some other crazy thing.

As always, in this program (PBot_BOB-F_VC6.osc) I start using the I/O definitions described on the main BOB [Software page]. Then I need to add some special values that I'll use when issuing movement commands. I like to give these values names (using Const) and define them at the top of the program so I can change all occurrences in one easy location: Here are the constants I've defined:

//-----------------------------------------------------------------
// Constants, used for default values
//-----------------------------------------------------------------
Const   LEFT_WHEEL          =  1;       // Used when passing motion setpoints
Const   RIGHT_WHEEL         =  2;
Const   BOTH_WHEELS         =  3;

Const   NEUTRAL             =  0;       // Used to set wheel direction
Const   FORWARD             =  1;
Const   REVERSE             =  2;

Const   SPIN_90             =  9;       // Determined by experimentation
Const   INCHES_10           = 50;       // Determined by experimentation

I use all the usual Hardware Objects (see [previous lessons] for the details) but now I need to define a bunch of Virtual Circuit Objects:

//-----------------------------------------------------------------
// Virtual Circuit objects
//-----------------------------------------------------------------
oCompare0   LeftMatch       = New oCompare0;    // Used to detect end-of-motion
oWire       LeftConnect     = New oWire;        // Used to trigger Stop Event
oEvent      LeftAutoStop    = New oEvent;       // Motion Stop Object

oCompare0   RightMatch      = New oCompare0;    // Used to detect end-of-motion
oWire       RightConnect    = New oWire;        // Used to trigger Stop Event
oEvent      RightAutoStop   = New oEvent;       // Motion Stop Object

Once I start using Virtual Circuits, I add a new SetupVC( ) function just for them. It helps me to keep my initialization code clear in my mind, plus as I try different VC's I don't accidentally modify the IO setup. Here's the SetupVC( ) function for lesson F:

//-----------------------------------------------------------------
//  SetupVC()
//  Configure all the Virtual Circuit Objects here
//-----------------------------------------------------------------
Void    SetupVC(Void)
{
    // --------------------------------
    // Setup the Closed Loop VC's
    // --------------------------------
    LeftMatch.Input.Link(LeftClicks);               // Setpoint comparison
    LeftMatch.Fuzziness     = 0;                    // Tolerated error
    LeftMatch.Operate       = cvTrue;               // Run
    LeftConnect.Input.Link(LeftMatch.Between);      // Connect Match to Stop Event
    LeftConnect.Output.Link(LeftAutoStop.Operate);

    RightMatch.Input.Link(RightClicks);             // Setpoint comparison
    RightMatch.Fuzziness    = 0;                    // Tolerated error
    RightMatch.Operate      = cvTrue;               // Run// Tollerated error
    RightConnect.Input.Link(RightMatch.Between);    // Connect Match to Stop Event
    RightConnect.Output.Link(RightAutoStop.Operate);
}

Let's look at what these assignments mean, by running down the connections for the left wheel:
LeftMatch is an oCompare0 object. We link it's input to LeftClicks, which is the left wheel oQEncoder object. Then we set the Fuzziness to 0, which means we want an exact value match (with Zero). Then we set it operating. LeftConnect is an oWire object which is used to connect two flags together. We set the LeftConnect.Input to LeftMatch.Between (which is the "equals zero" flag) and the output to RightAutoStop.Operate. This means that when Between is True, the RightAutoStop_CODE function will be called.

So now lets look at how we start and stop the motor.

All wheel motions are initiated using the SetWheelPosition( ) function. We tell it: Which Wheel, What direction, How far and at What speed. Here's the code:

//-----------------------------------------------------------------
//  EVENT SETUP SUBROUTINES
//-----------------------------------------------------------------
Sub Void SetWheelPosition(Byte Wheel, Byte Direction, Word Clicks, Byte Speed)
{
    If (Direction == FORWARD)
        Clicks = 0 - Clicks;
    Else
        Speed = 0 - Speed ;

    // Which Wheel ?
    If ((Wheel & LEFT_WHEEL) == LEFT_WHEEL)
    {
        LeftClicks = Clicks;            // Set the click countdown
        LeftConnect.Operate = cvTrue ;  // Turn On event trigger
        LeftMotor.Brake = cvOff  ;      // Release the Brake
        LeftMotor = Speed ;             // Start the rotation
        LeftRunning.Set ;               // Signal Running
    }

    If ((Wheel & RIGHT_WHEEL) == RIGHT_WHEEL)
    {
        RightClicks = Clicks;           // Set the click countdown
        RightConnect.Operate = cvTrue ; // Turn On event trigger
        RightMotor.Brake = cvOff  ;     // Release the Brake
        RightMotor = Speed ;            // Start the rotation
        RightRunning.Set;               // Signal Running
    }
}

Notice that the sequence for starting a wheel in motion is:

Preload the Encoder,
Turn on the Stop Event object,
Turn off the brake,
Set the speed,
Turn on the Running flag.

You can imaging that the two AutoStop_CODE functions would do the opposite;

//-----------------------------------------------------------------
//  EVENT SUBROUTINES
//  These are called by the two oEvent objects when
//  their Operate inputs are set.
//-----------------------------------------------------------------

Sub Void LeftAutoStop_Code(Void)
{
    LeftMotor = - LeftMotor  ;          // Reverse the motor drive
    LeftMotor = 0  ;                    // Stop the motor drive
    LeftMotor.Brake = cvOn  ;           // Brake the motor drive
    LeftConnect.Operate = cvFalse ;     // Disable the event trigger
    LeftRunning.Clear ;                 // Signal NotRunning
}

Sub Void RightAutoStop_Code(Void)
{
    RightMotor = - RightMotor  ;        // Reverse the motor drive
    RightMotor = 0  ;                   // Stop the motor drive
    RightMotor.Brake = cvOn  ;          // Brake the motor drive
    RightConnect.Operate = cvFalse ;    // Disable the event trigger
    RightRunning.Clear ;                // Signal NotRunning
}

The only slightly odd thing here is that before turning off the motor drive, I reverse it momentarily. This is to help stop the motor a little bit faster.

Now all that's left is for the main program to do something logical with this feature. One thing that wheel feedback is useful for is precision turning. If you know how far to turns the wheels to do a reliable 90 degree turn, it's a lot easier to navigate something like a maze. So I made my test program just drive around in a square. I had to experiment to see how many clicks I needed to do a 90 Degree spin, but now I have the value, I can just load it into my program as a constant. Here's the main code:

//-----------------------------------------------------------------
//  BOB Main Program
//-----------------------------------------------------------------
Void Main(Void)
{
    // Always set up the required IO and VC's first.
    SetupIO();
    SetupVC();

    // Wait for 5 second "Start Up" to elapse
    While(ActionTimer.NonZero);

    // Drive in a square
    While (cvTrue)
    {
        // Turn 90 degrees clockwise
        SetWheelPosition(LEFT_WHEEL,  FORWARD,  SPIN_90,  50);
        SetWheelPosition(RIGHT_WHEEL, REVERSE,  SPIN_90,  50);
        While (WheelsMoving()) ;
        WaitTenths(5);

        // Drive forward 10 Inches
        SetWheelPosition(BOTH_WHEELS, FORWARD, INCHES_10,  50);
        While (WheelsMoving()) ;
        WaitTenths(5);
    }
}

You can see here that after setting the wheels in motion, I enter a While loop, waiting for the WheelsMoving( ) function to return false. I didn't have to do this, it's just that in this case I had nothing better to do. I could have maybe started playing a tune, and between each note, called WheelsMoving( ) to see if I needed to start a new movement.

While I was documenting this program I thought of several different ways I could have coded it. I could have used a regular oCounter object instead of a oQEncoder because the oCounter can increment an external oWord object which has it's own NonZero flag. This way I could eliminate the oCompareO object and maybe save myself a few bytes of Object space. Anyway, if anyone sees a "more efficient" way to code a Closed Loop Position Control function, send me your code.

Next, in [Lesson G] I will create a Closed Loop SPEED control for the wheels using Virtual Circuits.