Initialization

On power-up of the robot, the current EEPROM held data values are loaded and the downloaded executable is run. The initialization process is as follows:

• Set up Ports C, E and G for the UARTS

• Initialise robot variables from values held in the EEPROM • Set up the command table

• Perform default communication port setup • Add user commands

• Initialise the motors • Enable interrupts

• Run the program main in a forever loop

At the end of the initialization the program is in the main loop checking for a command to execute.

3.8. Robot Framework and PID Controller 93

3.8.2

Command Support

This provides the instructions that the robot has to perform. There are over thirty user commands supported, for which only four of are interest in this context.

The routine dispatchCommand is called in the main loop, when commandBufferMode is set to COMMAND READY by Radio Comms. DispatchCommand matches the received command against the list of commands, and when a match is found, executes the corre- sponding command routine.

• SetSpeed command sets the required speed of the robot. This is the command received from the Robot Soccer Engine to move the robot. It is executed by enter- ing the command [-{leftByte}{rightByte}]. The routine setSpeed scales the each byte value by multiplying by param V ByteScaleSpeed (=1024) and shifting

right by 8, e.g. −128 x 1024/256 = −512. The result is stored in nReqSpeedLeft

and nReqSpeedRight respectively.

• Stop command sets the speed of the robot to zero. It is executed by entering the command [s].

• Test command runs the user defined routine test in the downloaded executable. It is executed by entering the command [t].

• Param command allows the viewing and setting of parameters. To view a parameter enter [.id], to set a parameter enter [.id=nnn] and to show all parameters enter [.].

3.8.3

Communication

This provides the mechanism to transfer commands and data between the modules of the robot under interrupt. The two uarts on the robot are controlled by two communication sub processes, radio comms (UART0) and IO comms (UART1).

Radio Comms

External commands and replies are processed through the radio comms. The receive sig- nal is handled by processReceiveByte0 and the transmit signal by processTransmit- Byte0. The routine processReceiveByte0 stores the incoming command in command-

Buffer, and when completed sets the commandBufferMode to COMMAND READY for execu- tion in the main loop by the Command Support process. The routine processTransmit-

Byte0sends the contents of sendBuffer to the radio comms port. This mechanism as

well as altering the required speed, allows parameter changes to be made whilst the robot is running.

IO Comms

This subprocess controls the exchange of data between the robot program and the motor IO under interrupt. There are two data exchanges to be handled. Firstly, the request for left and right motor clix data from motor IO, and secondly the transmission of left and right motor power values to motor IO.

The motor IO sends a timeout interrupt every millisecond. The interrupt handler for the timeout counts down the parameter variable nControlRateTicks to zero from param

V PidTickRate (=5). When the countdown reaches zero, the IoState of the robot is

changed from IOSTATE IDLE to IOSTATE GetLeftHi and UDR1 is set to the command char IOCHAR GETPOS.

This initiates the exchange of left and right motor clix data through the receive signal routine processReceiveByte1. On receipt of each motor clix component the IoState is set to the next expected byte. A confirm is set in UDR1 and sent to the motor IO by the transmit signal routine processTransmitByte1. This causes the motor IO to signal the next byte is ready to be handled by processReceiveByte1. After the last confirm is sent, the IoState of the robot is set to IOSTATE GotNewPos by the transmit signal rou- tine processTransmitByte1, so that the motor control algorithm can be actioned. The current motor clix values are stored in the variables nNewClixLeft and nNewClixRight. Motor power data transfer is initiated by motor control, triggering the motor IO to call the receive signal routine processReceiveByte1. The left then right motor power values are loaded in UDR1 for transfer. The IoState of the robot is set back to IOSTATE Idle when completed.

3.8. Robot Framework and PID Controller 95

3.8.4

Motor Control

The motor control process provides the control algorithm for the robot. The routine

controlMotors is called in the main loop and is actioned when IoState is set to

IOSTATE GotNewPosby the IO Comms sub process. The IoState is set to IOSTATE Idle

and the motor clix are validated. The control algorithm is then called to calculate the re- quired motor power for each motor.

Finally the transfer of motor power data is initiated. The IoState of the robot is set to IOSTATE SendLeft and UDR1 loaded with the command char IOCHAR SETPWM, the receive is handled by IO comms.

3.8.5

Control Algorithm

The suffixes Left and Right are implied in the variable names. The control algorithm currently implemented is a three term PID controller.

The algorithm starts by calculating the rate of change of the motor clix, iClixRate as

nNewClix- nOldClix. If this rate is below a threshold of RATE MAX then iClixRate is

stored in nClixRate otherwise the previous nClixRate is used in the control algorithm. This is important since the motors can report spurious clix values, and there is no indicator if the Motor Clix counter goes through the 65535 integer boundary and resets. Using the previous value minimises the controller bumping. The nNewClix are then stored in

nOldClix for next time. The nTargetSpeed is then loaded. This can be the scaled

demand nReqSpeed from the setSpeed command, or some function of nReqSpeed.

Control Algorithm Calculation Function Call

For each motor, the function calcSpeed nPwm is called with input parameters nTargetSpeed and nClixRate, and output parameters OldSpeed and ErrorDist. The function returns a motor power value, nPwm.

Speed and Motor Power Calculations

The following calculations are performed to calculate the power to be put onto the motor • Current speed of the robot

nSpeed= nClixRate * param(V PidSpeedgain(=4))

• Change in speed — negative for increasing speed

nSpeedChange= OldSpeed - nSpeed

• Error in speed — positive for too slow

nSpeedError= nTargetSpeed - nSpeed

• Error in distance - positive when behind the target

nErrorDist= ErrorDist + nSpeedError

In order to prevent integral windup nErrorDistance is limited between ± param

V PidMaxIerr(=2048), which is half a wheel turn. nSpeed and nErrorDistance are stored in OldSpeed and ErrorDist for use next time.

• Open loop power value

nOpenLoopCalc= param(V PidOpOffs(=0)) +

param(V PidOpGain(=3))* nTargetSpeed

• Speed error correction

nSpeedErrorCalc= param( V PidP) * nSpeedError.

• Accumulative error correction

nErrorDistCalc= param(V PidI) * nErrorDist.

• Differential correction

nSpeedChangeCalc= param(V PidD) * nSpeedChange.

• Total calculated correction

nCalc= nOpenLoopCalc + nSpeedErrorCalc +

nErrorDistCalc+ nSpeedChangeCalc.

The correction is then shifted right by param(V PidDownshift(=7)), limited to lie between -127 and 127 and then stored in the return variable nPwm.

3.9. Summary 97

3.9

Summary

In this chapter the environment that the real world experiments was discussed. Descrip- tions of the pitch, cameras and the robot used in the real world experiments was provided. The software and hardware required to operate the robot was reviewed. The mechanism to identify and separate the robots using coloured caps was given. A description of how the robot operated internally was given. Details of the real world problems encountered in controlling and operating the robots was provided together with the lack of spherical aberration correction and the corrective action applied. An overview of the Fuzzy Logic system used to develop the type-1 and dual surface and average type-2 controllers was described. The DC motor model used in the simulation was defined and subsequently coded.

Development of Baseline PID Controller

4.1

Introduction

This chapter is concerned with the development of a Proportional, Integral and Differen- tial controller, commonly known as a PID controller. The controller used is the indepen- dent PID controller as described in Chapter 3. The purpose of developing the controller was to establish a baseline controller against which the type-1 and type-2 fuzzy logic con- trollers could be compared. In order to remove as much variation as possible from the investigation the same robot was used throughout the following experiments. A series of experiments were carried out with the objective of tuning the independent PID controller. The tuning experiments were to establish a set of P, I and D parameters which would allow the robot to perform across the range of wheel speed demand inputs.