|
|
|
|
|
My other sites |
|
|
|
|||
| [Home] [Projects] [Hardware] [Software] [Books] [Links] [Downloads] | |||
|
|
|||
|
|
|||
We all know that the real reason for having a robot is to go places and do stuff. Well, maybe not always, but a robot with no mobility just isn't that much fun :)
This lesson will show how to control BOB's Twin Motor Gearbox using the Quad Half H-bridge driver chip from Texas Instruments. The chip's official part number is SN754410, and it has enough pins to control two small DC motors with proportional forward and reverse speeds. Each motor uses two of the Half H-Drivers to produce a full H-Bridge. I've included a portion of BOB's schematic here for reference. (Remember the full schematic is available on the [download page] ).
Each motor uses three OOPic I/O lines. Two to control the motor direction & braking, and one to control the Motor speed. The speed is controlled by driving the "Enable" line with a Pulse Width Modulated (PWM) signal. The width of the High Pulse controls how long the motor is turned on for each cycle. The longer it's on, the faster the motor will go.
It's worth noting that this type of speed control falls under the general heading of "Open Loop" control, because the PWM ratio is set by the program and then the motor runs at whatever speed results from the combination of battery voltage, PWM ratio and robot drag/friction. With Open Loop control it's quite possible that for a given PWM setting. the resultant speed may vary from wheel to wheel, and from robot to robot. This is fine for Sumo, but may not be optimal for line following or room navigation. See later lessons for examples of Closed Loop Control.
As always, in this program (PBot_BOB-C_VC6.osc)
I start using the I/O definitions described on the main BOB [Software
page].
I'm going to need the Motor definitions, and the Proximity Sensor Definitions. These are:
In this lesson I want to verify that the individual wheel drives are working, then I'll drive around trying not to bump into things :)
Here is the code I use to declare the various Objects that I need for this Lesson:
//-----------------------------------------------------------------
// 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
// Digital Proximity sensors
oDIO1 LeftEye = New oDIO1; // Left Prox Sensor
oDIO1 RightEye = New oDIO1; // Right Prox Sensor
Notice that I'm using the oDCMotor2 object for each wheel. This Object is compatible with several different H-Bridge chips like the L293, L298 and SN754410
In my programs, I always use the SetupIO( ) function configure the OOPic
"Objects" required to interface to the hardware.
Here is the object Setup code
for this lesson:.
//-----------------------------------------------------------------
// SetupIO()
// Configure all the IO Objects here
//-----------------------------------------------------------------
Void SetupIO( Void )
{
// --------------------------------
// setup the ActionTimer Timer for 1/10th Second ticks
// Preload counter with 5 second delay
// --------------------------------
ActionTimer.PreScale = CLOCK_PRESCALE;
ActionTimer.ClockIn.Link(ooPIC.Hz60);
ActionTimer = FIVE_SECONDS;
ActionTimer.Operate = cvTrue;
// --------------------------------
// setup the Wheel Drivers
// --------------------------------
LeftMotor.Value = 0;
LeftMotor.PreScale = SLOW_PWM;
LeftMotor.Mode = cvOn; // Active Braking
LeftMotor.Brake = cvOff;
LeftMotor.IOLine1 = IO_MOTOR_LEFT_FWD;
LeftMotor.IOLine2 = IO_MOTOR_LEFT_REV;
LeftMotor.IOLineP = IO_MOTOR_LEFT_PWM;
LeftMotor.Unsigned = cvFalse;
LeftMotor.Period = PWM_PERIOD;
LeftMotor.Operate = cvTrue;
RightMotor.Value = 0;
RightMotor.PreScale = SLOW_PWM;
RightMotor.Mode = cvOn; // Active Braking
RightMotor.Brake = cvOff;
RightMotor.IOLine1 = IO_MOTOR_RIGHT_FWD;
RightMotor.IOLine2 = IO_MOTOR_RIGHT_REV;
RightMotor.IOLineP = IO_MOTOR_RIGHT_PWM;
RightMotor.Unsigned = cvFalse;
RightMotor.Period = PWM_PERIOD;
RightMotor.Operate = cvTrue;
// --------------------------------
// setup the Eye proximity sensors
// --------------------------------
LeftEye.IOLine = IO_PROXIMITY_LEFT;
LeftEye.Direction = cvInput;
RightEye.IOLine = IO_PROXIMITY_RIGHT;
RightEye.Direction = cvInput;
}
As always, the first thing I do is configure the ActionTimer object. It's setup to count down to zero in 1/10th of a second.
Next I configure the Left and Right Motor objects. In addition to just assigning the IO lines, there are some other important settings. PreScale and Period go together to define the frequency and resolution of the PWM signal. The two selections I've used provide a 3.1 kHz PWM signal with 100 steps of resolution. I've also set Unsigned to false which means the the object uses the sign of Value to determine which direction to run. I've also set Active Breaking ON (using Mode), and then turned off the brake.
Finally I configure the left and right "eyes" as digital inputs.
In this lesson, I also define two new utility functions: Drive and Break. Any program that moves BOB around will need to do these basic actions a lot, so it makes sense to put them into functions right away.
Drive( ) is used to set the speeds of both motors. It can also be told to hold those speeds for a set amount of time before returning. This just avoids having to pepper the main code with a lot of different time delays. Drive is also responsible for taking off the break before starting to move. Setting the drive speeds to zero will stop BOB.
If you just set the speed of BOB's motors to zero, he will roll gracefully to a stop. But for some events (like reaching a drop-off) this might not be fast enough, so Brake( ) is used to bring BOB to a rapid stop. This function reverses the motors for a tenth of a second before calling for a speed of zero.
Now for the actual program. First it calls SetupIO( ), then it waits for the 5 second timer to expire, then BOB is put through some simple maneuvers to show that each motor control is working as expected. Finally the program enters an infinite loop polling the Proximity sensors. This loop is a basic obstacle avoidance process. If nothing is seen in front of BOB, then he drives forward. If BOB sees something blocking his way in BOTH eyes he backs up and spins around. If something appears in ONLY ONE eye, then BOB will veer away form that object.
When designing the initial set of wheel test maneuvers, it is important to isolate the various tests so there is no confusion as to whether the correct IO assignments have been made. For example, instead of driving both wheels forward and then back, I do them one at a time. This will highlight right away if the wheels are flipped (left with Right), or if a direction is reversed on either wheel. Then some dual-wheel maneuvers can be run to ensure that everything is working together.
//-----------------------------------------------------------------
// BOB Main Program
//-----------------------------------------------------------------
Void Main(Void)
{
// Always setup the required IO and VC's first.
SetupIO();
// Wait for 5 second "Start Up" to elapse
While(ActionTimer.NonZero);
// Run Basic Wheel Tests
Drive( 50, 0, 20); // Left forward for 2 seconds
Brake();
Drive(-50, 0, 20); // Left Reverse for 2 seconds
Brake();
Drive( 0, 50, 20); // Right forward for 2 seconds
Brake();
Drive( 0,-50, 20); // Right Reverse for 2 seconds
Brake();
Drive( 100,-100, 10); // Fast spim to Right for 1 second
Brake();
Drive(-100, 100, 10); // Fast spim to Left for 1 second
Brake();
// Stop and wait two seconds then start forward
Brake();
WaitTenths(20);
Drive( 50, 50, 10); // Forward
// Run forever finding and avoiding walls
While (cvTrue)
{
// Go forward if the path is clear
If (LeftEye == CLEAR & RightEye == CLEAR)
{
Drive(60, 60, 0);
}
// Reverse and spin if the path is blocked in both eyes
Else If (LeftEye == OBSTACLE & RightEye == OBSTACLE)
{
Brake();
Drive(-50, -50, 5);
Drive( 50, -50, 6);
}
// Spin to the right if the left is blocked
Else If (LeftEye == OBSTACLE)
{
Drive( 50, -50, 0);
}
// Spin to the left if the right is blocked
Else If (RightEye == OBSTACLE)
{
Drive(-50, 50, 0);
}
}
}
To test the program, I put BOB in the middle of a small room and let him go.
Don't forget the full source code for this example is available on the [Download] page (PBot_BOB-C_VC6.osc).
Next, in [Lesson D] I will add the Line sensors to the program and turn BOB into Sumo-BOB.
Web content is copyright © PhilBot.com
2005, Deep Creek Lake, MD.
Contact: Phil Malone 301.387.2331, webmaster@PhilBot.com