Code for PID Controller Experiment
/*
This is an experimental proportional plus integral controller for a
modification of Dave Baum's Tankbot equipped with rotational sensors, one
for each of the two motors that drive the Tankbot's treads. The design is
based on the controller described in Chapter 7 of "Mobile Robots:
Inspiration to Implementation" by Joseph Jones and Anita Flynn.
*/
/*
We begin by providing names for the various outputs, inputs, and internal
timers that we'll be using in the program.
*/
#define LEFT OUT_A
#define RIGHT OUT_C
#define LEFT_ROTATION SENSOR_1
#define BUMP SENSOR_2
#define RIGHT_ROTATION SENSOR_3
#define LEFT_TIMER 0
#define RIGHT_TIMER 1
/*
There are some irritating little details concerning what the constants and
sensor readings mean that we have to deal with before we get into writing
actual code for the controller.
The SAMPLE_PERIOD constant will determine how often we sample the
observable parameters and adjust the controllable ones. In the ideal
situation, we sample continuously; however, this is anything but the ideal
situation and given that we have a digital computer running at a finite clock
speed we have to sample at discrete intervals. As a general rule of thumb,
a good approximation to a continuous controller can be obtained by choosing
a sampling period of one-tenth to one-fifth of the shortest time constant
or cycle period of the highest frequency component of the dynamical system.
In the case of the Tankbot, roughly speaking, the shortest time constant
would correspond to the time it takes the Tankbot to change its heading,
say, 5-10 degrees, at full speed, which I estimate to be around 500ms.
This is a very rough estimate however and I expect that it is high.
However, if we were to set our sampling period to be 50 to 100ms as
recommended then we would have trouble measuring this period accurately
using the RCX and NQC.
In NQC, the internal timers return values in units of 100ms resolution,
hence, e.g., if n = Timer(0) and n == 100, then 10 seconds has elapsed
since Timer(0) was last reset to zero. The wait function takes as its only
argument an integer value interpreted in 10ms units, hence Wait(100) waits for
1 second. SAMPLE_PERIOD is in 10ms units.
*/
#define SAMPLE_PERIOD 100 /* The sample period is 1 second. */
/*
Determining appropriate units for velocity, time and distance takes a
little care. The rotation sensor meaures in increments of 22.5 degrees,
counting off 16 clicks (units) for a full rotation.
A quick experiment indicates that one rotation of the rear (driving) wheel
of the Tankbot results in the track (and therefore the Tankbot itself under
ideal conditions) moving approximately 8-9cm (as a further check, the
diameter of the wheel plus the thickness of the tread is about 3cm and so
one rotation should drive the robot apprroxmately 3cm * Pi). Since two
rotations of the drive wheel result in five rotations of the axle of the
rotation sensor, if the robot travels 2 * 7 = 14cm then the rotation sensor
will count off 5 * 16 = 80 clicks so that one click of the rotation sensor
should correspond to approximately 14 / 80 = .175cm.
So, if the Tankbot is traveling at around 10cm/sec and we count the number
clicks over a SAMPLE_PERIOD of one second, it should be around 60. We'll
also assume that the TARGET_VELOCITY and BIAS are specified in terms of
0.175cm/sec units, so that if we want the Tankbot to move at around 10cm per
second we should set TARGET_VELOCITY to around 60.
*/
#define TARGET_VELOCITY 5
#define BIAS 0
/*
There are three separate PID controllers in this design: one
proportional-only controller (a PID controller without the I or the D
components) for each of the two drive motors (these controllers use the
K-PROPORTIONAL constant) and one integral-only controller (a PID controller
without the P or the D components) for the steering (this controller uses
the K_INTEGRAL constant). Since all numbers in NQC code have to be represented
as signed integers, we represent each controller constant as the ratio of two
integers, the DIVIDEND and the DIVISOR, and first multiply by the DIVIDEND
and then divide by the DIVISOR.
*/
#define K_PROPORTIONAL_DIVIDEND 4
#define K_PROPORTIONAL_DIVISOR 5
#define K_INTEGRAL_DIVIDEND 0 /* Turn off the integral controller. */
#define K_INTEGRAL_DIVISOR 10
int LEFT_VELOCITY ;
int RIGHT_VELOCITY ;
int LEFT_POWER = 0;
int RIGHT_POWER = 0;
int INTEGRAL = 0;
task main() {
/* Initialize the sensor inputs. */
SetSensor( LEFT_ROTATION, SENSOR_ROTATION ) ;
SetSensor( RIGHT_ROTATION, SENSOR_ROTATION ) ;
SetSensor( BUMP, SENSOR_TOUCH ) ;
SetOutput( LEFT, OUT_ON ) ; // One of OUT_ON, OUT_OFF, or OUT_FLOAT
SetOutput( RIGHT, OUT_ON ) ;
SetDirection( LEFT, OUT_FWD ) ; // One of OUT_FWD or OUT_REV.
SetDirection( RIGHT, OUT_REV ) ;
SetPower( LEFT, 0 ) ; SetPower( RIGHT, 0 ) ;
/* Play a sound to indicate that the controller is starting. */
PlaySound( SOUND_UP ) ;
CreateDatalog( 100 ) ;
ClearSensor( BUMP ) ;
until ( BUMP == 1 ) {
/* Calculate the velocity of the left and right drive motors. */
ClearSensor( LEFT_ROTATION ) ; ClearTimer( LEFT_TIMER ) ;
ClearSensor( RIGHT_ROTATION ) ; ClearTimer( RIGHT_TIMER ) ;
Wait( SAMPLE_PERIOD ) ;
LEFT_VELOCITY = LEFT_ROTATION / Timer( LEFT_TIMER ) ;
RIGHT_VELOCITY = RIGHT_ROTATION / Timer( RIGHT_TIMER ) ;
/* Beep to indicate the end of a sample period. */
PlaySound( SOUND_CLICK ) ;
/* Maintain a running sum of the bias plus the difference in velocity. */
INTEGRAL = INTEGRAL + LEFT_VELOCITY + BIAS - RIGHT_VELOCITY ;
/* Calculate the new power level for the left motor. */
AddToDatalog( RIGHT_VELOCITY ) ;
LEFT_POWER +=
K_PROPORTIONAL_DIVIDEND *
( ( TARGET_VELOCITY - LEFT_VELOCITY ) -
( K_INTEGRAL_DIVIDEND * INTEGRAL / K_INTEGRAL_DIVISOR ) ) /
K_PROPORTIONAL_DIVISOR ;
if ( LEFT_POWER < 0 ) LEFT_POWER = 0 ;
else if ( LEFT_POWER > 7 ) LEFT_POWER = 7 ;
/* Calculate the new power level for the right motor. */
RIGHT_POWER +=
K_PROPORTIONAL_DIVIDEND *
( ( TARGET_VELOCITY - RIGHT_VELOCITY ) +
( K_INTEGRAL_DIVIDEND * INTEGRAL / K_INTEGRAL_DIVISOR ) ) /
K_PROPORTIONAL_DIVISOR ;
if ( RIGHT_POWER < 0 ) RIGHT_POWER = 0 ;
else if ( RIGHT_POWER > 7 ) RIGHT_POWER = 7 ;
AddToDatalog( RIGHT_POWER ) ;
/* Update the power for the two motors. */
SetPower( LEFT, LEFT_POWER ) ;
SetPower( RIGHT, RIGHT_POWER ) ;
}
}