Robot Projects
Sam Wane
The PIG
The PIG
This robot was built at Staffordshire University with the help of Steve Knight (hardware). The main purpose was to build a multi-purpose robotic platform that is large enough to accomodate a laptop. The laptop will run a map-solving program, control the robot and read the sensors through its serial port, interface a webcam and run a speech synthesiser.
The problem with H-Bridge motor drivers.
A lot has been written about H-bridge drivers, and there are circuits on the
Web showing how they can be built using just P channel MOSFETS. In my experience,
this cannot be done as the diagram below explains:
Motor (a) will turn when the gate voltage is between 2-4V. The motor in (b)
will not turn as the FET cannot be guaranteed a difference of 2-4V between the
gate and source. The motor in (c) will turn as it is a P MOSFET, the source
is connected to +12V, and the gate is 2-4V lower than the source voltage.
My H-Bridge circuit
The theory
The motor turns in one direction when FETs Q1 and Q5 are conducting:
…and the other way when FETS Q4 and Q6 are conducting:
FETs Q4 and Q5 are switched to set the direction, the PWM signal switches either
Q1 or Q6 depending on whether the DIR (direction) line is low or high. The AND
gates sort out which FET the PWM signal goes to. Note the use of Q11 as an inverter
(NOT) gate.
The FETs switch on when the difference between the gate voltage and source voltage
is between 2-4V. This is derived from the 12V motor power supply and using the
potential divider networks of the 12k and 3.3k ohm resistors. This divides the
voltage to 3V. Note that if more than 12V is to be used to drive the motors,
these values must be altered.
To turn on the P MOSFETs, the voltage is pulled 3V lower than the 12V supply,
i.e. 9V is placed on the gate, the P MOSFETS turn off when the full 12V is applied
to the gate.
The Circuit
Components:
R1,R5,R8,R10 12k
R2, R6, R9, R11 3.3k
R3, R12 33k
R7, R4, R13, R14, R15 47k
R16 1k
Q1, Q6 MTP36N06V
Q4, Q5 MTP30P06V
Q3, Q10 BC212
Q2, Q7, Q8, Q9, Q11 BC108
D1,D2,D3,D4 large current diodes
Note that the PCB diagram may not be to scale!
One of the motor drivers in-situ.
If I was to build another motor controller I would keep it simple. The H-bridge above has been temperamental, and those P-FETs are so sensitive. An idea would be to use a relay for reversing motor polarity, and a FET for the motor on/off control. A FET could switch the relay, and I would include a test switch (SW1) to test the motor and driver, and LEDs to show the pulses to the motor and the relay state. I haven't built this circuit. If you do, you could try leaving the LEDs out until you are sure the prototype works ok as these could interfere with the switching voltage to the FET. Add a protection diode and capacitor across the relay. You could add a 'motor enable' relay also.
This shows the H-Bridge of one motor
These are 500 pulses per revolution and will have several uses in the program:
Here is the snippet of code used to increment the counters based on encoder rotation. Note that a 'flag' variable is used to catch the single rising edge of the encoder pulse. It works as follows: (The right encoder is connected to pin B1, and left to pin B2)
//encoder count routines
if (input(PIN_B1) && (!flagr))
{
flagr=1;
distr++;//distance travelled
ratecountr++;//this is reset each interrupt
}
if (!input(PIN_B1)) flagr=0;
if (input(PIN_B2) && (!flagl))
{
flagl=1;
distl++;
ratecountl++;
}
if (!input(PIN_B2)) flagl=0;
//end of encoder count
The PIC used is a 16F877 from Microchip. This takes care of low level tasks:
The main PIC is at the front centre back to back with the H-Bridge. The ultrasonic slave PIC can be seen above the wheel on the left.
The second PIC on the robot controls the ultrasonic sensors and relays this information back to the host PIC via a serial connection. The host PIC will then relay this data to the PC. It is thought that this second PIC will be able to control servo motors and any other I/O in the future.
The main PIC program functions thus:
All the code was written in Pic-C by CCS.
The PIC communicates to the host PC via a RS232 serial link. There are two formats for this communication:debug mode and command mode.
Debug mode
The PIC will accept 'English' words and numbers and act on them, command examples are (FD 500, BK 100, LT 90). This mode is easy to communicate through Hyperterminal, and the PIC will write in English allowing a user to gain direct command of the robot.
The PIC starts in debug mode by default.
The routine used to receive number data in ASCII format is listed here:
long get_num (void)// returns a number from characters on the
RS232
//max no is 59999, integers only. Returns zero if any illegal characters are
entered.
//Input finished with character return char(13)
{
char c;
long num;
float shift;
shift=10000;
num=0;
do
{
c=getch(); //0-48,1-49,9-57
putc(c); //echo typed character
if ((c>=48) && (c<=57))
{
if (c>54) shift=shift/10;//int can handle up to 65535,make it 59999
num=num+shift*(c-48);
shift=shift/10;
} else if (c!=13) goto breakout; //Handles anything other than numbers
}while (c!=13);
num=(long)(num/(shift*10));
printf("\n\r");
return (num);
breakout:printf("\n\rInvalid Number\n\r");
return (0);
}
And to print numbers to the terminal program (useful for displaying debug data and distances):
void term_print(long int tval)
{
int temp;
long int val;
val=tval;
if (val<0)
{
printf("-");
val=-val;
}
if (val>9999)
{
temp=48+(val/10000);
putc(temp);
val=val-((val/10000)*10000);
}
else putc(48); //put in a zero
if (val>999)
{
temp=48+(val/1000);
putc(temp);
val=val-((val/1000)*1000);
}
else putc(48); //put in a zero
if (val>99)
{
temp=48+(val/100);
putc(temp);
val=val-((val/100)*100);
}
else putc(48); //put in a zero
if (val>9)
{
temp=48+val/10;
putc(temp);
val=val-((val/10)*10);
}
else putc(48); //put in a zero
temp=48+val;
putc(temp);
// printf("\n\r");
}
Command mode
This is more economic and allows a program on a PC to talk to the PIC directly. All commands are three bytes, the first is the command, and the second and third are the LSB and MSB values to be passed.
A command from debug mode forces the mode to change to command mode.
These were bought from Robot Electronics and interfaced to the PIC using the following program. It is no use repeating the details of operation of the sensors as the Robot Electronics link explains the operation clearly. The sensor is the SRF04.
Here is a program used to red the ultrasonic sensors. It interfaces to 3 sensors (1-left, 2-middle, 3-right). It waits for an input, 1, 2 or 3, and activates the sensor accordingly. Three leds can be connected to ports D1, D2 and D3 to show a rough proximity, this proximity value can have a threshold set by sending it 255 followed by the threshold value. A value (0-255) is returned depending on the proximity of an object to the activated sensor.
Besides the ultrasonic sensors, the encoders can be used to detect if the robot has collided with anything.
A Holonomic Robot (one that steers by changing the speed/direction of the two side wheels) tends not to drive in a straight line. This is due to the two motors not quite reaching the same speeds, hence it will describe a curve.
The idea behind straight line motion is to control the speeds of the motors using PWM so that it corrects its trajectory to keep moving in a straight line. Later we will see that the trajectory of the robot itself should be compensated to get to the desired point.
Considering the angle and displacement of the robot separately, the angle a robot will have moved for a given wheel rotation is defined as:
where theta is the angle, nL is the number of encoder pulses on the left wheel, nR- same for right wheel, s-distance wheel moves with each tick (in mm), w-distance the wheels are apart.
A control routine was written into the PIC to compensate for the deviation of the angle (the error). The error value altered the PWM values direcly as in the following diagram:
and the code appears as follows:
if (distr>distl) //theta positive value, error positive
{
thetat=((((distr-distl)*s)/(3.141*W))*180);
errorb=(signed int) (0-thetat*gain);
}else
{
thetat=((((distl-distr)*s)/(3.141*W))*180);
errorb=(signed int) (0+thetat*gain);
}
if (errorb<-90) errorb=-90;
if (errorb>90) errorb=90;
errorb=errorb*4; //increase gain for feedback
set_pwm1_duty(speedl-errorb);
set_pwm2_duty(speedr+errorb);
The variables stand for the following:distl, distr - distance each wheel has moved in encoder pulses, thetat-the error angle,gain-the feedback gain (I set this to 4 after running several trials), errorb-the error signal to be subtracted from the pwm values, speedl, speedr - the original set speed for the motors (should be equal values).
Note that this code can also be used to drive the robot in a curve of a constant angle and could be used as an efficient way to change direction of the robot without it stopping.
Of course, straight line motion will still result in a positional error, the robt moving in a straight line had to correct an angle at first, so even if it moves in a straight line, the first angle it set off with will cause a deviation 'yo' in the figure below.
The robot has moved in an arc having radius 'r' in the figure below
That 'r' is calculated as
again where alpha is the angle,
n1 is the number of encoder pulses on the left wheel, n2- same for right wheel,
s-distance wheel moves with each tick (in mm), W-distance the wheels are apart.
and alpha:
