|
|
|
|
|
My other sites |
|
|
|
|||
| [Home] [Projects] [Hardware] [Software] [Books] [Links] [Downloads] | |||
|
|
|||
|
|
|||
This is my most complicated Virtual Circuit (VC) program so far. It uses a range of VC objects to monitor the wheel speed, determine if it's too fast or slow, and then automatically adjust the motor drive signal. To run two motors it's cutting very close to the maximum Object Memory limit, although this is aggravated by the fact that the oMathC (Clocked Math object) doesn't seem to work.
To prepare for this program I had to determine some things:
I did some experimentation with BOB and came up with these answers.
So now I had my general operating environment nailed down, so I could start assembling a VC to do the job. A full closed loop control system might use a PID controller. This uses three feedback terms (Proportional, Integral & Differential) to control the drive signal. Most controllers usually only need two of these terms, and for the sake of simplicity mine is going to only use one term. I'm going to just use the I (Integral) term. The reason that I need to use an Integral gain term is because I need to increase or decrease the Motor drive based on the speed error. Once the speed is correct, the drive needs to stay the same. This is how the I on a PID works.
I started thinking about how I could measure the speed "error". Since the only feedback I have is the Encoder counts, I need to see how many counts occur over the cycle time to calculate speed, then I need to see how this compares with the Set-point speed, to calculate an error. My mind went back to [Lesson F] where I used the encoder reaching zero to tell me when to stop. I wondered if I could do a similar thing to determine error. It occurred to me that if I knew how many clicks I expected to get per cycle at a given speed, I could preset the encoder with that number at the start of each cycle and have the encoder count down instead of up. If the actual speed was correct, it would count down to zero at the end of the cycle. If the speed was incorrect, then whatever was left in the encoder at the end of the cycle would actually be the speed error.
For example... I know that at half speed I expect to get 5 clicks per cycle (1/4 Sec), so at the start of each cycle I preload the encoder with 5. Now at the end of the cycle, if the wheels are turning at half speed, the encoder would have counted down to 0... no error. But if the wheel was actually turning at 1/4 speed, the encoder would only have counted down to 2 or 3. This is the speed error. I can now use this value (with some gain applied) to increase the motor drive to increase the wheel speed. Slick eh?
OK, so what objects can I use, and what other constants are involved?
As always, in this program (PBot_BOB-G_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:
//-----------------------------------------------------------------I use all the usual Hardware Objects:
//-----------------------------------------------------------------
// User Objects. Use these to access BOB hardware.
//-----------------------------------------------------------------
oCountDownO ActionTimer = New oCountDownO; // User Timer
// Wheel motor driver Objects
oDCMotor2 LeftMotor = New oDCMotor2; // Driver For the Left Wheel
oDCMotor2 RightMotor = New oDCMotor2; // Driver for the Right Wheel
// Wheel Optical Encoder Objects
oQencode LeftClicks = New oQencode; // Value for Left Encoder
oQencode RightClicks = New oQencode; // Value for Right Encoder
But I also needed to define a NEW bunch of Virtual Circuit Objects:
//-----------------------------------------------------------------
// Virtual Circuits. Use these to manipulate BOB hardware.
//-----------------------------------------------------------------
oClock PID_Clock = New oClock; // Set to run at PID Rate
oBusIC SetSpeedLeft = New oBusIC; // Holds the desired clicks
oMath PIDLeft = New oMath; // Updates the Drive Signal
oBusIC DriveLeft = New oBusIC; // Holds the motor output
oCompare0 LimitLeft = New oCompare0; // Indicates out of limit condition
oWire LimitLeftWire = New oWire; // Transfers outoflimit condition
oBusIC SetSpeedRight = New oBusIC; // Holds the desired clicks
oMath PIDRight = New oMath; // Updates the Drive Signal
oBusIC DriveRight = New oBusIC; // Holds the motor output
oCompare0 LimitRight = New oCompare0; // Indicates out of limit condition
oWire LimitRightWire = New oWire; // Transfers outoflimit condition
The first thing that's new with these VC's is that many of them are "clocked". This means that I can control when they perform their action. I'm going to use a common clock for all of them, and I use both the negative or positive edge of the clock to indicate the start and end of the cycle respectively. I use PID_Clock to generate the common clock rate that corresponds to the Controller cycle rate.
SetSpeedLeft is used to hold the speed set-point and PIDLeft is used to add the current error to the current motor drive signal. DriveLeft is used to clock the new drive value into the motor controller.
A common problem with PID's that use Integral gain like ours is something called "wind-up". This is caused by a situation where no matter how high the drive goes, the controller cannot reach the set-point. When this happens, the integral term just gets bigger and bigger. Then when it comes time to change speed or direction, the Integral term has to slowly un-wind itself before the output changes. To prevent this, PID's have anti-wind-up logic. In our case we will test to see if the drive signal is going to get above the maximum effective vale, and if so, we turn off the integral term.
LimitLeft is used to detect that the new drive signal will be out of range, and LimitLeftWire is used to inhibit DriveLeft from updating the motor drive.
Here's the SetupVC( ) function that configures these VC's:
//-----------------------------------------------------------------
// SetupVC()
// Configure all the Virtual Circuits here
//-----------------------------------------------------------------
Void SetupVC( Void )
{
// PID Clocks
PID_Clock.Mode = 1; // Pulse High
PID_Clock.Rate = PID_RATE; // Desired divider
PID_Clock.Operate = cvTrue; // Run the PID clock
// Left Wheel PID
SetSpeedLeft.ClockIn.Link(PID_Clock); // Use PID Clock
SetSpeedLeft.InvertC = cvTrue; // Transfer on negative clock edge
SetSpeedLeft.Output.Link(LeftClicks); // Clock Data into Encoder
SetSpeedLeft.Operate = cvTrue; // Enable VC
PIDLeft.Input1.Link(LeftClicks); // Read Encoder Value
PIDLeft.Input2.Link(LeftMotor); // Read Motor Value
PIDLeft.Output.Link(DriveLeft); // Send Sum to Potential Drive Value
PIDLeft.Mode = cvAdd; // Add the inputs
PIDLeft.Operate = cvTrue; // Enable VC
LimitLeft.Input.Link(DriveLeft); // Test Potential Drive Signal
LimitLeft.Fuzziness = PID_LIMIT; // Set invalid width
LimitLeft.Operate = cvTrue; // Enable VC
DriveLeft.ClockIn.Link(PID_Clock); // Use PID Clock
DriveLeft.Output.Link(LeftMotor); // Clock Data into Motor
LimitLeftWire.Input.Link(LimitLeft.Between); // Transfer "Between" to enable Drive
LimitLeftWire.Output.Link(DriveLeft.Operate);
LimitLeftWire.Operate = cvTrue; // Enabled
// Right Wheel PID
SetSpeedRight.ClockIn.Link(PID_Clock); // Use PID Clock
SetSpeedRight.InvertC = cvTrue; // Transfer on negative clock edge
SetSpeedRight.Output.Link(RightClicks); // Clock Data into Encoder
SetSpeedRight.Operate = cvTrue; // Enable VC
PIDRight.Input1.Link(RightClicks); // Read Encoder Value
PIDRight.Input2.Link(RightMotor); // Read Motor Value
PIDRight.Output.Link(DriveRight); // Send Sum to Potential Drive Value
PIDRight.Mode = cvAdd; // Add the inputs
PIDRight.Operate = cvTrue; // Enable VC
LimitRight.Input.Link(DriveRight); // Test Potential Drive Signal
LimitRight.Fuzziness = PID_LIMIT; // Set invalid width
LimitRight.Operate = cvTrue; // Enable VC
DriveRight.ClockIn.Link(PID_Clock); // Use PID Clock
DriveRight.Output.Link(RightMotor); // Clock Data into Motor
LimitRightWire.Input.Link(LimitRight.Between); // Transfer "Between" to enable Drive
LimitRightWire.Output.Link(DriveRight.Operate);
LimitRightWire.Operate = cvTrue; // Enabled
}
The super tricky thing here is that the oMath object is continually calculating what the New Motor drive might be, and writing it into the input side of the oBusIC object. At the end of the cycle (positive clock edge) the oCompare object looks at this new value and decides if it is safe to write it into the motor. The OK state is signaled by the oCompare's between output. Between is wired to the operate member of the oBusIC object, so only if the new value is OK, is it written into the motor on the next positive clock edge.
All that's left is to get things rolling. I created a function called SetSpeed( ) that takes a few standardized parameters and scales them into good values to start the PID with. For example, I permit a speed input of +/- 128. Inside Set Speed this is tested against zero (the stop condition) and if a speed is called for, it converts this to a Clicks per Cycle number in the range of +/- 9. The math is messy, but that's what it's doing. Likewise, an initial value for the motor drive is calculated to get things started quickly (effectively preloading the integrator). I calculate an initial motor drive value in the range of 20 - 56. Here's SetSpeed( )
//-----------------------------------------------------------------
// Set Speed(LeftSpeed, RightSpeed, Time)
// This function drives both wheels at the indicated speed.
// The motion is held for "Time" tenth of a second
// No time limit is placed if Time = 0
//
// Speed range is +/- 128
// Time Range is 0 - 255
//-----------------------------------------------------------------
Void SetSpeed(Byte LeftSpeed, Byte RightSpeed, Byte DriveTime)
{
// Set Left Speed
If (LeftSpeed != 0)
{
// Turn off brake
LeftMotor.Brake = cvOff;
// Do Setup based in direction of motion.
If (LeftSpeed > 128) // Reverse speed
{
// Move speed range to (-1 to -9) and set initial drive signal
LeftSpeed = ((255 - LeftSpeed) >> 4) + 1;
LeftMotor.Value = - ((LeftSpeed << 2) + MOTOR_DEAD_BAND) ;
SetSpeedLeft = - LeftSpeed ;
}
Else
{
// Move speed range to (1 to 9) and set initial drive signal
LeftSpeed = (LeftSpeed >> 4) + 1;
LeftMotor.Value = (LeftSpeed << 2) + MOTOR_DEAD_BAND ;
SetSpeedLeft = LeftSpeed ;
}
}
Else
{
// Stop motor and feedback
LeftMotor.Value = 0 ;
SetSpeedLeft = 0 ;
LeftMotor.Brake = cvOn;
}
// Set Right Speed
If (RightSpeed != 0)
{
// Turn off brake
RightMotor.Brake = cvOff;
// Do Setup based in direction of motion.
If (RightSpeed > 128) // Reverse speed
{
// Move speed range to (-1 to -9) and set initial drive signal
RightSpeed = ((255 - RightSpeed) >> 4) + 1;
RightMotor.Value = - ((RightSpeed << 2) + MOTOR_DEAD_BAND) ;
SetSpeedRight = - RightSpeed ;
}
Else
{
// Move speed range to (1 to 9) and set initial drive signal
RightSpeed = (RightSpeed >> 4) + 1;
RightMotor.Value = (RightSpeed << 2) + MOTOR_DEAD_BAND ;
SetSpeedRight = RightSpeed ;
}
}
Else
{
// Stop motor and feedback
RightMotor.Value = 0 ;
SetSpeedRight = 0 ;
RightMotor.Brake = cvOn;
}
// Continue action for requested time period in tenths.
If (DriveTime > 0)
{
ActionTimer = DriveTime;
While(ActionTimer.NonZero);
}
}
My Main program just puts BOB through some simple motions to illustrate how to set the speeds.
//-----------------------------------------------------------------
// BOB Main Program
//-----------------------------------------------------------------
Void Main(Void)
{
// Always setup the required IO and VC's first.
SetupIO();
SetupVC();
// Wait for 5 second "Start Up" to elapse
While(ActionTimer.NonZero) ;
// Do a range of motions
SetSpeed( 10, 10, 100); // Straight forward real slow for 10 seconds
SetSpeed( 30, -30, 20); // Spin right for 2 seconds
SetSpeed( -80, -80, 100); // Back up real fast for 10 seconds
SetSpeed( 0, 0, 0); // Stop;
While(1); // Wait.
}
The most important thing that came out of this lesson was the new ability to control BOB at VERY slow speeds. Until now it was necessary to run the motors at a level that was guaranteed to overcome any gear friction or drag on the ground. Now with the Closed Loop control I can set a very slow speed and the controller will quickly ramp up the drive until BOB moves, and then it will reduce the power to maintain the desired crawl. Also if I push against BOB's motion with my hand, the drive signal ramps up to overcome the force I'm applying. Real closed loop control!!!
Web content is copyright © PhilBot.com
2005, Deep Creek Lake, MD.
Contact: Phil Malone 301.387.2331, webmaster@PhilBot.com