Psuedo Code 201 Project

driveDistance function

A function that uses a P-controller within a do-while loop to make the robot drive straight, forward or
backwards, a user specified distance. The function takes two inputs first input is the required drive
distance in millimetres, and the second is the proportional constant, k p , found by tuning.

Set initial encoder count to zero.

Find the error , distance from target, via the equation

Send the control effort as saturated power to the motors.

Delay for a small interval so that physical change can occur before the processor
checks the error again.

Do this while error is not equal to zero.

A function that converts the current encoder count of a wheel encoder, and returns the equivalent
distance travelled in millimetres. The single input for this function is the encoder count of a specified
wheel encoder. It is known that 360 encoder counts is equal to one encoder shaft revolution, the
robots gear ratio between wheel and encoder shaft is 3 :5 and the wheel diameter is 103 mm
.

Read the encoder count of given wheel.

1 3
Pass this value through the equation Distance ( mm )=count 103 .
360 5
Return the distance variable.

lineFollow function

This function is used to drive along a line of any input colour. Three inputs are taken, two to define
the colour range of the line to be followed and a third to specify the power level of both motors.
Although the project requires the robot to trace a brown line, the logic used allows any colour line to
be followed, given that the colour range is known. The upper and lower limits used for the brown
line on the course are 2000 and 2300 respectively.

While the input line colour is not detected by any of the sensors, robot turns clockwise on
the spot until any sensor detects the required colour. When this occurs, the motors are
stopped.

If the left sensor detected the line colour, the robot turns clockwise, until the middle sensor
reads the line (i.e. the middle sensor is over the line). While the middle light sensor is over
the line, robot drives straight.

The same logic is applied should the right sensor detect the line first. Turn anticlockwise until
the middle light sensor is over the line.
 The robot will drive forward so long as the left or right sensors do not detect the line, and
the middle sensor does. If the left or right sensors do detect the line, the robot implements
appropriate on the spot rotation, to centre the robot back over the line.

armPosition function

A function takes the arm encoder count as a single input, and returns the current arm position as an
angle in degrees. For this function to run accurately the arm must be calibrated to a known reference
point, such that calculations can be made from the input angle and converted to the corresponding
encoder count value. The horizontal arm position is used the zero reference.

armAngle function

A function implemented to lift or lower the robot arm to a specified angle. The angle takes two
inputs, one being the angle specified by the user and the second being the power at which the user
wants the robot arm to move with. Power level is significant for this function because if the arm is
required to perform a delicate movement, then less power can be used. Similarly, power can be
increased if speed is a priority. For example, lowering the can down the target requires a lower speed
to prevent the can from swaying. armPosition is utilised as a helper function to determine the
current position of the arm, such that appropriate motion can be implemented.

User input is passed through appropriate equation that converts the required angle into the number
of encoder counts.

Compare target angle to current angle via the helper function:

If the given angle is less than the current angle, negative power is sent to the arm motor until
the encoder count reaches the calculated count value.

Set arm motor to zero when current and target angles match.

Else If the given angle is greater than the current position, a positive power value is sent to
the arm motor until the encoder count reaches the calculated count value.

Set arm motor to zero when current and target angles match.

Else, do nothing.

Use armPosition helper function to compare current angle to the target angle, the difference
between the two is the error .

Find control effort by controll effort=k p error .

Saturate the controll effort , and send as power to the arm motor.
Delay for a small period so that physical change can occur.

Do this while armPosition is not equal to the target angle.

turnAngle function

This function takes three inputs, one is the angle the robot will turn given in degrees the second is
the power each motor will use to perform the turn and the third is whether the robot is required to
turn on the spot or about one of its rear wheels. Power levels will be saturated to ensure they are
within the maximum and minimum power limits. A positive angle value corresponds to a clockwise
turn, and negative value, a counter clockwise turn. The arclength formula S=r , where r is
radius of the turning circle and theta is the input angle, will be used to calculate the required
distance travelled by either wheel. When not turning on the spot, the left rear wheel is the outer
wheel for a clockwise turn, and for a counter clockwise turn, the right rear is the outer. The
magnitude or absolute value of the input power level will be used to ensure a negative angle
corresponds to a counter clockwise turn and vice versa. The countToDistance helper function is
called within this function to check if the required distance has been reached.

degrees
Convert the input angle from degrees to radians, where radians= .
180
If not turning on the spot, radius is the distance between the centre of each wheel.

Pass the input angle through the arclength equation to determine the distance to be
travelled by the outer wheel.

Set outer wheel encoder count to zero.

If input angle is greater than zero, send magnitude of the saturated input power to left
motor, while encoder count is less than distance, S . Set left motor power to zero, so
robot is stationary.

Else if input angle is less than zero, send magnitude of the saturated input power to right
motor, while encoder count is less than distance, S . Stop the right motor

Else do nothing.

If turning on the spot, radius is the distance between the centre of the wheel to the centre line of the
robot.

Pass the input angle through the arclength equation to determine the distance to be
travelled by both wheels.

Set both wheel encoder counts to zero.

If input angle is greater than zero, send magnitude of the saturated input power to left
motor, and send magnitude of the saturated input power multiplied by negative one to the
right motor.

Do this while the mean of both encoder counts is less than distance, S .
Set both motors power to zero, so robot is stationary when distance is reached.

If input angle is less than zero, do the same as above, however send negative power to the
left and positive power to the right, while the encoder count mean is less than distance,
S .
Set both motors power to zero, so robot is stationary when distance is reached.

Else do nothing.