Setup
Upon entry of the program an initial set up function is performed, this calls functions responsible for Motor, sensor and LED set up. Primarily these functions ensure that the correct register bits are set to allow input or output. Where necessary, timers and reference voltages are also set to ensure correct function of the Atmega32u4’s PWM and ADC pins. Before entering the main loop a series of variables are instantiated and pointers created. These variables include values for the PID loop, an array to store sensor values and an array to store the map of the course.
Main Loop
Once the main loop has been entered the flow of the program continues as seen in. By holding the button on the main BOB down a reading of the central 4 sensors is started. The sensors are initially held over the white line and the reading across each IR collector is recorded. These values are then averaged and a range of ‘white’ values saved. This value is then used in later statements to compare with real time readings from the sensors to determine if a white or black line is being detected. This is not required for the following of the line but is useful for detecting edge markers and stopping the robot if the line is lost.
The Atmega32u4 can only read one ADC pin at a time which must be directed to read the pin that is connected to the sensor. This is done through the setting of the ADMUX register. Once this has been done the analogue value can be converted to an 8-bit reading.
As the sensors were all placed in switch mode, only an 8-bit register was needed to be used to read the sensors. This reduced the time taken and memory required to read all the sensors. Whilst a digital read of the pins voltage could have been performed instead, it was chosen to still use an analogue read so that if the lighting situation changed drastically the PID control loop could still be performed.
The reference voltage of the ACD was set to the internal one of 2.56 V and auto trigger mode was turned off. This prevented unnecessary conversions and computational time from being wasted.
Detect Current Status
As a safety feature, a check that the device is over a white line is performed. This operates by checking each of the 4 middle sensors values.
| for every sensor:
if sensor value == white: return 1: return 0 |
If any of the central sensors detects that it is over white, then the feedback control loop can be run. Otherwise both motors are brought to a complete stop and a re-find line function is run (This feature was removed from the final product). This was not a direct requirement of the final product but let components be tested without risk of the device falling of the track and becoming damaged if it left the line.
Process PID Loop
To control the position of the robot relative to the line a PID feedback control loop is used. This feedback loop allows the position of the robot to be tightly controlled relative to a desired position, which is the middle sensors centred over the white line.
Where kp, ki and kd are constants. These constants were found by using a provided Matlab simulator and then through trial and error on the actual device. This was required as the simulator used did not consider the size and weight of the robot or the performance of the motors. To tune the PID the robot was run on the track with both ki and kd set to 0. The value of kp was increased until the robot could follow the line but had great oscillation.
| error = line position – device position
integral = integral + error derivate = error – lastError turnAmount = (kp * error) + (ki * integral) + (kd * derivate)
motor1Speed = turnAmount + baseSpeed motor2Speed = -turnAmount + baseSpeed
lastError = error |
The value of kp was then decreased slightly, whilst giving kd a non-zero value. Kd was continuously increased until the oscillation was kept to a minimum. Finally, a very small ki value was used to make the tracking of the line even smoother. It was seen that correct kp and kd values were far more important for this case than ki was.
To find the error the raw ADC values of each of the 4 sensors was read and adjusted so that detection on the outer sensors would increase the error amount proportionally.
By reading the raw ADC value the ambient light did not matter as this would be reflected relatively across all sensors. Similarly, it saved significantly on computational time as 4 individual checks to determine if a sensor was seeing white or black could be replaced with one sum.
Before setting each of the motors to their appropriate speeds a few additional checks occur. Firstly, the error calculated is added to a sum of all previous errors for that section of the track. This is used later to process the track when mapping. The first 90 values of a section are skipped and then the next 135 are used in mapping. This accounts for the sensors being in front of the device and ensures that recordings only start once the entire robot is on a section of track. Once the course has been mapped the PID loop responds to the mapping of the course.
| if currentSection is Straight, Crossroad or End of a slow zone
Indicate with LED if nextSection is not the start of a slow zone increase both motor speeds else cap both motor speeds |
Both motors are capped in the section immediately preceding a slow zone to allow more accurate control during the slow zone.
When testing it was observed that the device would commonly leave the course on tight corners. Initially it was assumed that this was due to the turn amount being too small. However, despite increasing the values of the PID controller this would still occur. Using EEPROM data and LED feedback, it became apparent that when the motors were set between -10% and 10% of their speed would not have enough torque to spin. More so, the motors would continue rolling forwards due to the momentum of the overall robot, preventing it from making tight turns. For this reason, a hardcoded check is made that if the required speed percentage falls within this range the motors will be set to -11%. This causes a stall of the motor, causing it to come to a complete stop and allow the device to make a tighter turn.
Once all these checks have occurred, both motor speeds are set. The motors are controlled using Pulse Width Modulation, PWM, which allows voltage in varying width pulses to be applied across the motors, resulting in the motors running at different speeds. These pulses are controlled by the onboard timers of the Atmega32u4 and a created function that takes an integer input from 0 to 100%.
Process Wing Sensors
The wing sensors of the device are used for advanced mapping of the course and reacting to certain areas, such as the start stop markers and slow zones. This data is processed separately from the PID control loop to ensure that individual sections work as desired and allow the device to run without these advanced features if required. To process data from the wing sensors rising and falling edge checks were performed:
Rising Edge
|
Falling Edge
|
By considering the placement of the device on the track it was determined that the right-wing sensor is required to detect the start and stop markers. However, this will also detect a white value when the device drives over a crossroad. For this reason, no action in response to the right-wing sensor is set to occur until the falling edge of detection, as it left the marker. This allowed a check of the left wing to occur during the rising and high edge, to see if it detects white. If it does then it is clear that the robot is driving over a crossroad and this is recorded in the map. Otherwise it must have detected a start or stop marker.
A Boolean is used to determine whether it is a start or stop marker. As the robot starts between two markers the first one it sees will toggle the Boolean, whereas the second one will cause the robot to come to a stop for 3 seconds and reset the value. When stopped, a counter that measures the position of the robot relative to a saved map of the course is also reset, ready for the next lap of the course.
The left-wing sensor implements a rising edge method to detect white edge markers and record a section of track to a saved map. It uses the sum of errors to calculate an average error across the previous section. If the average error is greater than a certain amount this indicates that the robot, on average, in that section, was turning. Slow zones are detected by identifying two straight, non-crossroad, sections in a row.
The mapping code is designed to continuously run, this ensures that if a section of the track is incorrectly labelled in one run of the track it can be relabelled on the next lap. The exception to this is the slow zone, which cannot be remapped once detected. To handle the slow zone consistently the device comes to a complete stop for 2 seconds. This cause a false average error for the section being considered by the mapping algorithm. Each time a rising edge is detected by the left-wing sensor an LED is also toggled to indicate this detection to observers. After processing the last section of track the error sum is reset to 0 and a counter indicating the robots current position, relative to the map, incremented.
| if current position is not a slow start
if average error is greater than threshold, or less than – threshold set current position as curve else if previous section was straight set current position as slow start else if previous section was slow start set current position as slow end else set current position as straight |
A deliberate decision was made to perform this data processing on a rising edge instead of a falling one. The primary one being to ensure that the robot could react to changing track conditions as fast as possible. As it is a physical component, whilst calculations are occurring, it will continue to move. By placing the edge detection on a rising edge, it can be insured that the robot will react to a different section of track as early as possible, preventing any overshooting of the track.
As the final product requirement was for an average speed through the slow zone it was decided to bring the device to a complete stop. This decision was made after great deliberation and consideration of the mechanical system. Due to the larger wheels being used, the slowest speed the motors could run at was faster than the average speed across the slow zone. Although smaller wheels were tested, which solved this issue, they came with their own range of issues,. The only consistent way to achieve an average speed was to come to a complete stop in the slow zone for an appropriate amount of time before continuing.
This reaction is triggered during the processing of the left-wing sensor instead of inside the PID loop. This was done to ensure that the stop code would only run once, as actions for the left-wing sensor are performed on a rising edge. Whilst this could have been done in the main PID loop with a Boolean value this would have required more variables and additional handling of data to occur.