Projects
GravBot: A Maze-Running Ball Collector Robot
Summary
GravBot is an autonomous differential-drive robot that won first place in the final competition for ME 545: Mechatronics, a graduate-level course at Penn State, during Spring 2026. It is designed to navigate an unknown maze, detect and collect balls, recover from navigation failures, and stop at the maze exit. The project integrated mechanical design, sensing, motor control, computer vision, and finite state machine-based decision logic into a complete robotic system. The platform used IR distance sensors for wall following, wheel encoders for repeatable motion primitives, a Raspberry Pi camera system for real-time ball detection, and an Arduino-based controller for navigation, recovery, and actuation.
A key focus of the project was developing a modular system architecture that separated perception, decision-making, and low-level control. Computer vision was handled on the Raspberry Pi using OpenCV-based color detection, while the Arduino executed the finite state machine, encoder-based motion control, IR sensor filtering, ball collection logic, and recovery behaviors. The robot incorporated a roller-based ball intake mechanism, encoder-guided turning and recovery motions, stall detection, wall-loss recovery, and exit detection. This project provided hands-on experience in autonomous robotics, embedded control, sensor integration, computer vision, and mechatronic system design, while demonstrating the importance of robust recovery logic in real-world robotic systems.
GravBot winning moment of the final competition!
GravBot Demonstration Videos
GravBot Gripping Mechanism
GravBot Wall Following and Ball Collection
1. System Overview
GravBot is a custom two-wheel differential-drive robot built on the chassis shown in Fig.1, augmented with a roller-based ball-intake mechanism and an on-board Raspberry Pi vision system. The robot uses two JGA25-371 DC gearmotors with built-in hall-effect quadrature encoders, driven by a Pololu Dual MC33926 motor driver shield mounted on an Arduino Uno. A passive caster wheel supports the rear of the chassis. Two analog IR range sensors (Sharp GP2Y0A02YK) are used for environmental sensing: a front-facing sensor detects obstacles directly ahead, and a side-facing sensor mounted at 45° on the left side of the robot measures the perpendicular distance to the wall being followed. A pair of DC motors actuates the front intake rollers via a motor driver (module model: MD10A, motor driver IC: MC33926), and an inductive break-beam sensor mounted above the storage ramp at the entry to the robot’s storage space confirms that a captured ball has fully entered the bin. The system was powered with a 12 V battery and a power bank. Fig. 1 shows the assembled platform and labels the major hardware components.
The robot uses a two-processor architecture in which the main control architecture is implemented on the Arduino. The Arduino runs the navigation finite state machine, motor velocity loops, IR sensing and filtering, ball collection logic, recovery behavior, rollers, and break-beam logic on a 100 ms control loop, with a 10 ms subloop for IR sampling. The Raspberry Pi serves as a dedicated vision module, running OpenCV-based processing on a USB camera to detect tennis balls in the field of view. When a ball is detected, the Pi sends the ball information, including its pixel position, to the Arduino over a 115200-baud serial link. The Arduino then uses this vision data together with its sensor readings to decide the robot's next control action.
2. Mechanical Design and Ball Collection Mechanism
The mechanical design was developed to support stable navigation, reliable ball intake, and onboard storage. The robot uses a rectangular differential-drive chassis with two driven wheels at the front and a 15 mm caster wheel at the rear. The chassis is divided into a front functional section, which carries the sensors and collection mechanism, and a rear storage section, which stores the collected balls.
2.1 Chassis
The chassis was designed as a rectangular frame with a front intake section and a rear storage bin. The storage section was sized to hold all balls in the maze, with a width of 250 mm and a length of 200 mm, while remaining small enough to fit within the minimum path width of 2 ft. The robot uses two driven wheels at the front and a 15 mm caster wheel at the rear. The rear caster wheel was attached using a bracket that gave the robot a slight backward lean of no more than about 5 degrees. This small tilt helped collected balls naturally roll toward the rear storage area after entering the robot.
The front section included an 88.6 mm wide ball entry opening, providing enough clearance for a ball of approximately 64 mm diameter. The sensors and camera were mounted using 3D-printed brackets. One IR sensor was placed on the left side at 45 degrees, while another was mounted at the front center. The camera bracket positioned the USB camera about 30 degrees below the horizontal, allowing the robot to see balls close to the intake. Although the bracket also allowed for a servo-mounted camera, servoed vision was not implemented in the final system due to time constraints.
2.2. Ball-Collection Mechanism
Two collection mechanisms were considered: a servo-actuated claw (Fig. 2a) and a counterrotating roller intake (Fig. 2b). The roller intake concept in Fig. 2b was selected because it could collect balls across the front opening without requiring precise alignment. The claw design was less suitable because it would have required more precise alignment during collection and reliable servo torque to move the ball past the ramp.
The final mechanism used two DC-motor-driven robot car wheels as rollers, mounted at the front of the chassis (Fig. 2b). The wheels rotate inward to pull the ball through the front opening and shoot it up the ramp into the storage section. The rubber covering on the wheels provided grip and slight compliance, helping the rollers apply force to the ball without requiring exact contact geometry.
The ramp was placed just behind the front opening to guide the ball from the intake into storage. Instead of using a straight ramp, it was shaped to begin nearly flat near the floor and then gradually curve upward, becoming slightly concave near the end so that the ball could transition smoothly back toward the horizontal base of the robot. The front edge of the ramp was positioned about 2.5 mm above the ground to allow the ball to enter without obstruction while avoiding contact friction with the floor. A break-beam sensor was placed at the transition between the front collection region and the storage section to detect when a ball passed into storage.
3. Sensing, Filtering, and Vision
3.1. IR Sensors and Break Beam
Both Sharp IR sensors were sampled every 10 ms using a non-blocking timer. Each raw 10-bit ADC reading was first passed through a fourth-order low-pass Butterworth filter, implemented as two cascaded biquad sections, and then through a six-sample moving-average filter to further reduce ADC quantization noise. The filtered ADC value was converted to distance in centimeters using a calibrated Sharp IR model of the form 𝑑 = 𝐴/(𝑎𝑑𝑐 + 𝐵) + 𝐶. The constants 𝐴, 𝐵, and 𝐶 were identified separately for each sensor using least-squares fitting with ground-truth distance measurements collected across the operating range.
A key issue observed during testing was the non-monotonic behavior of the Sharp IR sensors at very short distances. Below a certain range, the sensor output begins to “curl back,” meaning the estimated distance may increase even as the robot moves closer to an obstacle. This behavior made the distance reading unreliable at close range and required an additional safety condition to prevent the robot from driving into a wall, as discussed in Section 5.4.
The break-beam sensor also uses an infrared emitter and receiver, but it was read as a digital signal rather than an analog distance measurement. The sensor output is low when an object blocks the beam and high otherwise. Because of this digital behavior, the readings were reliable and did not require additional filtering or calibration.
3.2. Computer vision for ball detection
Ball detection runs on the Raspberry Pi using OpenCV. Each captured frame is converted to HSV colorspace, thresholded to extract the colored region corresponding to a tennis ball, morphologically cleaned, and reduced to a single bounding-box centroid via contour analysis. The pixel coordinates of the centroid (x, y) are then linearly mapped to the range 0–254 and packed into a 3-byte serial frame: a 0xFF sync header followed by the x byte and the y byte. Values 0– 254 are used so that 0xFF can never appear in the payload, which keeps frame synchronization unambiguous. The Pi sends a packet only when a ball is detected; the Arduino interprets continuous silence (more than 300 ms without a packet) as 'no ball visible' and falls back to wall following.
The y pixel coordinate effectively encodes how close the ball is to the robot. Due to camera perspective, closer balls appear lower in the image. This property is used by the firmware as a simple proxy for range: once y exceeds a threshold (the ball is in the lower half of the frame), the robot considers the ball close enough (about 1 ft in the real world) and starts the ball collection behavior.
4. Low-Level Motion Control
The robot’s motion control was organized into two layers: high-level behavior selection and lowlevel motor execution. The finite state machine decided what type of motion was needed, such as wall following, ball tracking, pivoting at a corner, or creeping away from an obstacle. That decision was then executed using either closed-loop wheel-speed control or open-loop motion primitives, depending on whether the priority was maintaining direction or completing a fixed movement.
For direction-sensitive motion, the robot used independent PI velocity controllers on the left and right drive wheels. This was used for behaviors such as wall following and ball tracking, where the robot needed to continuously adjust its heading. The high-level controller selected target RPM values for the two wheels, and the PI controllers converted those desired speeds into PWM motor commands. Wheel RPM was estimated from encoder counts over each 100 ms control interval. Since the two sides of the drivetrain were not identical, each wheel used its own calibration and tuning. One revolution corresponded to approximately 342 encoder counts on the left wheel and 347 counts on the right wheel.
For fixed motions, such as turning by a desired angle or moving a short distance, the robot used open-loop primitives instead of PI speed control. In these cases, the goal was not to maintain an exact RPM, but to complete a specific movement. The encoder counts were recorded at the start of the motion, a constant PWM command was applied, and the motion ended once the target encoder counts were reached. The count targets depended on the type of motion being executed and were tuned separately for each wheel to account for differences between the two drivetrain sides.
5. Top-Level Control Finite State Machine
The robot's behavior is governed by a top-level FSM (Figure 3) with seven states: WALL_FOLLOW, TURNING, WALL_LOST, BALL_OVERRIDE, STALL_RECOVERY, EXIT, and STOPPED. WALL_FOLLOW is the default state; the others are entered in response to specific sensor or system events and always return either to WALL_FOLLOW or to STOPPED. Some of the states contain their own nested sub-FSMs which are shown in Appendix A.
Figure 3. Top-level finite state machine.
5.1. Wall following
The primary behavior maintains a constant perpendicular distance of 20 cm from the left wall. The error between the measured left distance and the 20 cm set-point is converted into a steering correction by a proportional-style law (gain 1.0, output saturated to ±5 RPM); this correction is added to and subtracted from a base forward speed to produce the per-wheel set-points. The base speed is normally 20 RPM, but is reduced to 7 RPM whenever the front IR reads less than 30 cm so that the robot has more time to react to the corner that is presumably approaching. If the front IR reading falls below 30 cm the controller exits to TURNING; if the left IR reading exceeds 50 cm the wall has been lost and the controller exits to WALL_LOST.
5.2. Inward corners
Corners that bend in towards the robot are handled by a fixed-geometry pivot. The robot stops, snapshots both encoder counts, and drives the left wheel forward and the right wheel backward at equal PWM until each wheel has accumulated the target count for a 90° pivot. After the pivot completes the FSM returns to WALL_FOLLOW with a short entry delay to let the IR filters reconverge before the next steering decision is made.
5.3. Outer corners and wall recovery
Outer corners are more difficult because the robot loses the wall and has no reference to track. Wall loss is declared when the left IR exceeds 50 cm. The robot then enters a four-phase nested FSM:
- Reverse phase: The robot reverses at low speed until either the left wall is re-detected within 30 cm or a 5 s timeout expires. This step ensures that the robot is aligned with the end of the wall before the open-loop creep phase begins.
- Creep phase: The robot drives forward by a fixed encoder count to clear the corner before turning, preventing the inner wheel from clipping the wall during the arc.
- Arc phase: The robot executes a left-turning arc by commanding the outer (right) wheel to 6 RPM and the inner (left) wheel to 2 RPM, sweeping around the corner. The arc continues until the robot measures a distance less than 30 cm on the left. Furthermore, if a ball is visible during the arc and is in the lower half of the camera frame, the rollers are switched on so the ball can be ingested passively as the robot sweeps past; the rollers turn off as soon as the break beam fires or the arc completes.
The reverse-creep-arc geometry was tuned experimentally on the actual maze, exploiting the constraint that the maze passages are at least 24 inches wide. A 10 s arc timeout is enforced as a final safety net: if the wall has not been reacquired within that window the robot enters STOPPED rather than continuing to spin in place.
5.4. Ball Collection Override
When the camera reports a ball (a serial packet has arrived within the last 300 ms) the FSM transitions from WALL_FOLLOW to BALL_OVERRIDE, suspending wall following entirely. The override runs its own three-phase sub-FSM:
- Track phase: The robot moves forward at 7 RPM and steers using the ball's x coordinate as the heading error for PD control. Steering correction is saturated at ±3 RPM to avoid jittering and abrupt motion. The y coordinate of the ball center increases as the robot gets closer to the ball since the center approaches the lower frame boundary. Hence, this phase is completed when the ball is no longer visible, assuming that the ball center transitioned out of the image frame.
- Collect phase: The rollers are switched on and the robot creeps forward at 4 RPM. The collect phase exits when the break beam fires (ball captured) or after a 5 s timeout (ball missed).
- Collected phase: The rollers are switched off and the robot pauses for 2 s. The firmware then samples the front IR and decides whether a reverse is needed.
The post-collection reverse handles the IR curl-back issue described in Section 3.1. After collection, the robot may be very close to a front wall, where the IR sensor can falsely report a safer distance. To resolve this, the firmware records the initial front IR reading and reverses while monitoring how the reading changes. If the reading rises, the robot stops once it has increased by 5 cm. If the reading first falls, indicating the curl-back region, the robot continues reversing until the reading returns to the normal 30 cm stop distance. A 5 s timeout bounds the behavior if the IR signal remains ambiguous.
5.5. Stall detection and recovery
During any PID-driven forward motion, both wheel encoders are monitored independently. If a wheel's count has not advanced for 500 ms while its target RPM is non-zero, that wheel is declared stalled and the FSM transitions to STALL_RECOVERY. Recovery consists of an open-loop reverse with a yaw bias away from the stuck wheel. If the left wheel stalled, the right wheel gets full reverse and the left only half, swinging the chassis off the obstacle, and vice versa. After a 1 s biased reverse the FSM returns to whichever state was interrupted. A separate slip-stall threshold catches the case where the encoder is incrementing but at a rate far below the commanded speed, which the bare 'count unchanged' check would otherwise miss on slippery surfaces.
5.6. Maze exit
The maze exit is identified by both IR sensors reporting unusually large distances simultaneously: the front IR exceeding 200 cm and the left IR exceeding 100 cm together indicate that the robot has entered an open area with no wall within range. To suppress false positives caused by transient sensor dropouts, the exit condition must hold continuously for 1.5 s before the FSM commits to the EXIT state. EXIT performs a final short forward creep so that the chassis fully clears the maze threshold before the robot stops and the run ends.
5.7. State-entry delay
A short entry delay is applied when the FSM enters a new top-level state. This was added because state transitions often occur immediately after abrupt actions such as stopping, pivoting, reversing, or recovering from a stall, and the IR readings may briefly reflect filter lag, vibration, or the previous motion rather than the robot’s new condition. The delay therefore acts like a simple transition debounce: it gives the robot and sensor estimates time to settle before another sensorbased transition is allowed. This improves reliability by keeping the states isolated (that is, conditions from a previous state do not influence the next state).
6. Batteries and Power Management
The robot was powered using a Zeee 3S LiPo battery pack rated at 11.1 V nominal, 5200 mAh, and 50C continuous discharge. This battery supplied the high-current loads, including the drive motors through the motor shield and the roller intake motors through the roller motor driver. The Raspberry Pi was powered separately using a USB power bank, while the Arduino was powered through the Raspberry Pi USB connection. The Arduino then provided the 5 V logic rail for the IR sensors, encoders, and break-beam sensor. The low-current 5 V logic and sensor connections were distributed through a breadboard, while the motor power path was kept separate from the breadboard. The system also shared a common ground so that the Arduino, motor drivers, sensors, and Raspberry Pi communication signals had a consistent electrical reference. Figure 4 shows the circuit diagram of the system, including the motor power path, sensor connections, logic wiring, and serial communication link.
Figure 4. Wiring diagram of the GravBot system.
The final power architecture was chosen after testing revealed reliability issues when the Raspberry Pi was powered from the same LiPo-based system. In particular, the Raspberry Pi showed a low-voltage warning immediately after the Arduino was connected, suggesting that the additional load or wiring path caused a voltage drop. Intermittent resets were also observed during testing, and these may have been worsened when the rollers were active, since roller startup or stall conditions can draw larger transient currents. To reduce the risk of voltage drops affecting the vision system, the Raspberry Pi was moved to a separate USB power bank while the LiPo battery remained dedicated to the motor loads. This separation improved reliability by isolating the sensitive computing and sensing electronics from the noisier, higher-current motor subsystem.
7. Performance and Discussion
GravBot was tested in both practice and official competition mazes using the conservative tuning discussed in Section 5. Navigation was reliable across the tested layouts: the robot consistently performed wall following, ball tracking, right turns, wall-lost recovery, and stall recovery. The encoder-counted open-loop motions were especially effective because pivots, creeps, and recovery motions remained repeatable even when the maze layout changed.
The navigation strategy was designed around the known maze constraints, so several behaviors depended on defined minimum path widths, wall lengths, and corner geometry. The robot handled different corner types as long as the corner angle relative to the followed wall was at least 90 degrees. Its speed was intentionally conservative to reduce collisions, sensor delay issues, and missed transitions, but it still completed full runs in about 2 to 3 minutes, well under the 4 minute requirement.
Ball collection was reliable under favorable placement conditions. In competition, the robot collected all five balls placed along its wall-following path, where the balls were within about 2 ft of a wall. The main difficulty occurred near corners: if a ball was not aligned with the turning arc, the robot could bump it toward the wall and later repeatedly attempt collection. A give-up condition was added for balls detected too close to the wall, but due to IR false readings and limited testing, this change interfered with normal collection and caused the first mission failure.
The main vision detector used fixed HSV thresholding for green balls and produced no major false positives under intended conditions. However, attempts to extend detection to multiple colors using Canny-based shape detection introduced false positives. This was problematic because the collection logic treats a previously detected but no longer visible ball as ready for collection. A practical improvement would be to verify shape-based detections with color thresholding before the Raspberry Pi sends a ball-detected signal to the Arduino.
Future extensions include using the camera to detect walls and the maze exit, reducing dependence on large-distance IR thresholds, adding a servo-actuated arm for more flexible object collection, and integrating an audio module for spoken status updates. These additions are compatible with the existing Raspberry Pi and Arduino architecture.
8. Conclusion
This project demonstrated a complete autonomous ball-collecting maze robot using IR wall sensing, encoder-based motion, Raspberry Pi vision, and FSM control. GravBot performed well according to the competition requirements, successfully executing wall following, corner navigation, stall recovery, exit detection, and ball collection. The main challenges were designing reliable corner-case navigation, handling collection near walls and corners, and dealing with the Sharp IR sensor curl-back region at very short distances. The most valuable lessons were architectural. We learned the importance of separating timing-critical control from heavier perception, adding recovery behavior for each motion primitive, and giving every open-loop action a sensor- or time-based exit condition. Future work should improve close-range sensing and make collection decisions more robust in difficult corner cases.
For future offerings of this competition lab, it would be helpful to give students early guidance on efficient design and rapid iteration. The mechanical design process takes much longer than expected, especially when students are also debugging sensors, wiring, control logic, and software integration. Encouraging students to keep designs simple, test subsystems early, and avoid overcomplicated mechanisms would help teams make better progress. It would also be useful to recommend that students complete Learning Factory certification early in the semester. Access to the Learning Factory helped significantly with rapid prototyping because parts could be made quickly without the longer wait times associated with departmental printing services.
Appendix A – Finite State Machine Diagrams
This appendix provides additional finite state machine diagrams for the main nested behaviors used by GravBot. These diagrams expand on the top-level FSM discussed in Section 5 and show the internal logic for wall recovery, ball collection, stall recovery, and maze exit detection. Lowlevel implementation details such as entry delays, encoder bookkeeping, and sensor filtering are omitted so that the diagrams emphasize the main behavioral transitions.
Figure A1. Sub-FSM for recovering the wall when lost.
Figure A2. Sub-FSM for ball collection.
Figure A3. Sub-FSM for recovering from stalled wheels.
Figure A4. Sub-FSM for exiting the maze.
Appendix B – Image Detection Script
import numpy as np
import cv2
import serial
# ================= CONFIG =================
MIN_RADIUS = 10
# Serial — 3-byte packet: [ 0xFF | x_byte | y_byte ]
# x/y bytes are clamped to 0-254 so they can never equal the 0xFF header.
ser = serial.Serial('/dev/ttyACM0', 115200, timeout=0.01)
# HSV range for tennis / green ball
lower_ball = np.array([25, 50, 50])
upper_ball = np.array([60, 255, 255])
# ================= CAMERA =================
cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
# Grab one frame to determine actual frame dimensions
grabbed, frame = cap.read()
if not grabbed:
raise RuntimeError("Camera not available")
FRAME_H, FRAME_W = frame.shape[:2]
CENTERLINE_Y = FRAME_H // 2
CENTERLINE_X = FRAME_W // 2
print("=== Ball Tracker Started ===")
print(f"Frame: {FRAME_W}x{FRAME_H} | Centerline X: {CENTERLINE_X} | Centerline Y: {CENTERLINE_Y}")
print("Protocol: 3-byte packets [ 0xFF | x_byte | y_byte ]")
while True:
grabbed, frame = cap.read()
if not grabbed:
break
display = frame.copy()
# Draw crosshairs
cv2.line(display, (CENTERLINE_X, 0), (CENTERLINE_X, FRAME_H), (255, 255, 0), 1)
cv2.line(display, (0, CENTERLINE_Y), (FRAME_W, CENTERLINE_Y), (255, 255, 0), 1)
# HSV processing
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lower_ball, upper_ball)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
ball_detected = False
bx, by = 0, 0
if len(contours) > 0:
largest = max(contours, key=cv2.contourArea)
(cx, cy), radius = cv2.minEnclosingCircle(largest)
cx, cy, radius = int(cx), int(cy), int(radius)
# Valid ball: radius large enough AND centre at or below horizontal centreline
if radius > MIN_RADIUS and cy >= CENTERLINE_Y:
ball_detected = True
bx, by = cx, cy
cv2.circle(display, (cx, cy), radius, (0, 255, 0), 2)
cv2.circle(display, (cx, cy), 3, (0, 0, 255), -1)
cv2.putText(
display,
f"({cx},{cy})",
(cx + 10, cy - 10),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 255, 0),
1
)
# Send 3-byte packet when ball detected
# Byte 0: 0xFF header — lets Arduino re-sync on misalignment
# Byte 1: x mapped to 0-254
# Byte 2: y mapped to 0-254
# No packet sent = no ball; Arduino detects loss via timeout.
if ball_detected:
x_byte = int(np.interp(bx, [0, FRAME_W - 1], [0, 254]))
y_byte = int(np.interp(by, [0, FRAME_H - 1], [0, 254]))
ser.write(bytes([0xFF, x_byte, y_byte]))
# Status overlay
label = f"BALL x={bx} y={by}" if ball_detected else "No ball (or above centreline)"
color = (0, 255, 0) if ball_detected else (0, 0, 255)
cv2.putText(display, label, (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
cv2.imshow("Mask", mask)
cv2.imshow("Ball Tracker", display)
if cv2.waitKey(1) & 0xFF == 27:
break
cap.release()
cv2.destroyAllWindows()
ser.close()Appendix C – Arduino Firmware
The full Arduino sketch implements the wall-following navigation, ball-collection override, stall recovery, reverse recovery, and serial communication protocol between the Raspberry Pi and Arduino.
// =============================================================
// Wall-Following + Ball Collector -- Arduino
//
// Default behaviour : wall following (left wall, IR sensors).
// Override : when RPi sends 3-byte ball packets, the
// robot suspends wall-following and runs the
// full TRACK -> COLLECT -> COLLECTED cycle,
// then resumes wall-following automatically.
// =============================================================
// ================= PIN DEFINITIONS =================
#define M1_DIR 7
#define M1_PWM 9
#define M2_DIR 8
#define M2_PWM 10
#define EN_PIN 4
#define SF_PIN 12
#define ENC_L 2
#define ENC_R 3
#define ROLLER_PIN 5
#define BREAK_BEAM A4
#define FRONT_IR_PIN A2
#define LEFT_IR_PIN A3
// ================= MOTOR STRUCT =================
struct Motor {
int dirPin, pwmPin, encPin;
volatile long count;
long lastCount;
float cpr;
float targetRPM;
float currentRPM;
float pwm;
float Kp, Ki, integral;
bool invertDir;
};
// ================= FILTER STRUCTS =================
struct BiquadCoeff { float b0, b1, b2, a1, a2; };
struct BiquadState { float z1 = 0, z2 = 0; };
struct ButterworthState { BiquadState s1, s2; };
#define MA_WINDOW 6
struct MovingAverage {
float buffer[MA_WINDOW] = {0};
float sum = 0;
int index = 0;
};
// ================= GLOBALS =================
Motor leftMotor, rightMotor;
ButterworthState frontFilter, leftFilter;
MovingAverage frontMA, leftMA;
unsigned long lastControlTime = 0;
unsigned long prevIRTime = 0;
// ================= STATE ENTRY DELAY =================
unsigned long stateEntryMs = 0;
#define STATE_ENTRY_DELAY_MS 400
#define BALL_ENTRY_DELAY_MS 300
// ================= FILTER COEFFICIENTS =================
const BiquadCoeff stage1Coeff = {
2.91464944656977e-05f, 2.91464944656977e-05f, 0.0f,
-0.939062505817492f, 0.0f
};
const BiquadCoeff stage2Coeff = {
1.0f, 2.0f, 1.0f,
-1.93529438685999f, 0.939120798806424f
};
// ================= IR CALIBRATION =================
const float A_front = 1.23072369e+04f;
const float B_front = 1.12642133e+01f;
const float C_front = 1.74338869e+00f;
const float A_left = 7.68904930e+03f;
const float B_left = 1.00000065e-03f;
const float C_left = -2.64920279e+00f;
// ================= TIMING =================
const int intervalMs = 100;
const unsigned long IR_SAMPLE_TIME = 10000;
// ================= WALL-FOLLOW THRESHOLDS =================
#define TARGET_LEFT_DIST 20.0f
#define FRONT_STOP_DIST 30.0f
#define FRONT_SLOW_DIST 30.0f
#define WALL_LOST_DIST 50.0f
#define WALL_RECOVERY_DIST 30.0f
#define EXIT_FRONT_DIST 200.0f
#define EXIT_LEFT_DIST 100.0f
// ================= WALL-FOLLOW SPEEDS =================
#define BASE_SPEED 20
#define SLOW_SPEED_WF 7
#define WALL_LOST_OL_SPEED 3
#define TURN_SPEED 7
#define ARC_OUTER_SPEED 6
#define ARC_INNER_SPEED 2
// ================= PIVOT / RECOVERY GEOMETRY =================
#define TURN_COUNTS_L 171
#define TURN_COUNTS_R 173
#define REVERSE_MAX_COUNTS_L 262
#define REVERSE_MAX_COUNTS_R 265
#define CREEP_COUNTS_L_WF 203
#define CREEP_COUNTS_R_WF 206
#define REALIGN_COUNTS 82
#define ARC_TIMEOUT_MS 10000
#define ARC_MAX_COUNTS 327
// ================= BALL-TRACK SPEEDS =================
#define TRACK_SPEED 7
#define CREEP_SPEED_BALL 4
#define MAX_CORRECTION 3
// ================= BALL HEADING CORRECTION =================
#define X_CENTER 127
#define DEAD_BAND 8
float Kp_heading = 0.030f;
// ================= BALL TIMEOUT =================
#define BALL_TIMEOUT_MS 300
unsigned long lastPacketMs = 0;
uint8_t pkt[2] = {0, 0};
bool ballVisible() {
return (millis() - lastPacketMs < BALL_TIMEOUT_MS);
}
// ================= BALL GIVE UP - CLOSE TO WALL =================
#define ROLLERS_TIMEOUT_MS 5000
#define BALL_TOO_CLOSE_TO_WALL 14.0f
#define BALL_GIVUP_COOLDOWN_MS 3000
unsigned long ballGiveUpMs = 0;
bool ballCooldown = false;
// ================= EXIT LOGIC =================
#define EXIT_CONFIRM_MS 1500
unsigned long exitDetectedMs = 0;
bool exitCandidate = false;
// ================= ARC ROLLER ASSIST THRESHOLD =================
#define BALL_ARC_Y_THRESHOLD 127
// ================= POST-COLLECTION REVERSE THRESHOLDS =================
#define FRONT_REV_BAND_LO 20.0f
#define FRONT_REV_BAND_HI 33.0f
#define FRONT_REV_RISE_CLEAR 5.0f
// ================= REVERSE TIMEOUT =================
#define REVERSE_TIMEOUT_MS 5000
// ================= STALL DETECTION / RECOVERY =================
#define STALL_DETECT_MS 500
#define STALL_REVERSE_MS 1000
#define STALL_REVERSE_PWM 60
#define SLIP_STALL_COUNTS 10
// ================= IR READINGS =================
float frontDistanceCM = 0;
float leftDistanceCM = 0;
// ================= TOP-LEVEL STATE MACHINE =================
enum RobotState {
STATE_WALL_FOLLOW,
STATE_TURNING,
STATE_WALL_LOST,
STATE_BALL_OVERRIDE,
STATE_STALL_RECOVERY,
STATE_EXIT,
STATE_STOPPED
};
RobotState robotState = STATE_WALL_FOLLOW;
long turnStartCountL = 0;
long turnStartCountR = 0;
enum WallLostPhase {
WL_REVERSE,
WL_CREEP,
WL_ARC,
WL_REALIGN
};
WallLostPhase wallLostPhase = WL_REVERSE;
long wallLostStartL = 0;
long wallLostStartR = 0;
unsigned long arcStartMs = 0;
unsigned long wlReverseStartMs = 0;
enum BallPhase {
BP_TRACK,
BP_COLLECT,
BP_COLLECTED
};
BallPhase ballPhase = BP_TRACK;
unsigned long ballPhaseMs = 0;
enum ExitPhase {
EX_CREEP,
EX_STOP
};
ExitPhase exitPhase = EX_CREEP;
long exitStartL = 0;
long exitStartR = 0;
// ================= STALL RECOVERY STATE =================
unsigned long stallRecoveryStartMs = 0;
RobotState stallResumeState = STATE_WALL_FOLLOW;
bool stallBiasLeft = false;
bool stallBiasBoth = false;
unsigned long leftStallSinceMs = 0;
unsigned long rightStallSinceMs = 0;
long lastStallCheckL = 0;
long lastStallCheckR = 0;
// ================= ENCODER ISR =================
void leftISR() {
leftMotor.count++;
}
void rightISR() {
rightMotor.count++;
}
// ================= MOTOR HELPERS =================
void initMotor(Motor &m, int dirPin, int pwmPin, int encPin,
float cpr, bool invertDir) {
m.dirPin = dirPin;
m.pwmPin = pwmPin;
m.encPin = encPin;
m.count = 0;
m.lastCount = 0;
m.cpr = cpr;
m.targetRPM = 0;
m.currentRPM = 0;
m.pwm = 0;
m.Kp = 1.5f;
m.Ki = 0.5f;
m.integral = 0;
m.invertDir = invertDir;
pinMode(dirPin, OUTPUT);
pinMode(pwmPin, OUTPUT);
pinMode(encPin, INPUT_PULLUP);
}
void setMotorPWM(Motor &m, float pwm) {
pwm = constrain(pwm, -255, 255);
bool fwd = (pwm >= 0);
if (m.invertDir) {
fwd = !fwd;
}
digitalWrite(m.dirPin, fwd ? HIGH : LOW);
analogWrite(m.pwmPin, (int)abs(pwm));
m.pwm = pwm;
}
void hardStop() {
leftMotor.targetRPM = 0;
rightMotor.targetRPM = 0;
leftMotor.integral = 0;
rightMotor.integral = 0;
leftMotor.pwm = 0;
rightMotor.pwm = 0;
setMotorPWM(leftMotor, 0);
setMotorPWM(rightMotor, 0);
}
void enableDriver() {
digitalWrite(EN_PIN, LOW);
delay(5);
digitalWrite(EN_PIN, HIGH);
delay(5);
}
void rollersOn() {
digitalWrite(ROLLER_PIN, HIGH);
}
void rollersOff() {
digitalWrite(ROLLER_PIN, LOW);
}
// ================= BIQUAD / BUTTERWORTH =================
float processBiquad(const BiquadCoeff &c, BiquadState &s, float x) {
float y = c.b0 * x + s.z1;
s.z1 = c.b1 * x - c.a1 * y + s.z2;
s.z2 = c.b2 * x - c.a2 * y;
return y;
}
float processButterworth(ButterworthState &f, float x) {
return processBiquad(stage2Coeff, f.s2,
processBiquad(stage1Coeff, f.s1, x));
}
// ================= MOVING AVERAGE =================
float applyMA(MovingAverage &ma, float x) {
ma.sum -= ma.buffer[ma.index];
ma.buffer[ma.index] = x;
ma.sum += x;
ma.index = (ma.index + 1) % MA_WINDOW;
return ma.sum / MA_WINDOW;
}
// ================= IR SENSOR =================
float computeFront(float adc) {
return max(A_front / (adc + B_front) + C_front, 0.0f);
}
float computeLeft(float adc) {
return constrain(A_left / (adc + B_left) + C_left, 0.0f, 500.0f);
}
void updateIR() {
unsigned long now = micros();
if (now - prevIRTime >= IR_SAMPLE_TIME) {
float fF = processButterworth(frontFilter, analogRead(FRONT_IR_PIN));
float fL = processButterworth(leftFilter, analogRead(LEFT_IR_PIN));
frontDistanceCM = applyMA(frontMA, computeFront(fF));
leftDistanceCM = applyMA(leftMA, computeLeft(fL));
prevIRTime += IR_SAMPLE_TIME;
}
}
// ================= RPM + PID =================
void updateRPM(Motor &m, float dt) {
long delta = m.count - m.lastCount;
m.lastCount = m.count;
m.currentRPM = ((float)delta / m.cpr / dt) * 60.0f;
}
void updatePID(Motor &m, float dt) {
if (m.targetRPM == 0) {
m.integral = 0;
m.pwm = 0;
setMotorPWM(m, 0);
return;
}
float error = m.targetRPM - m.currentRPM;
bool satHigh = (m.pwm >= 255 && error > 0);
bool satLow = (m.pwm <= 0 && error < 0);
if (!satHigh && !satLow) {
m.integral = constrain(m.integral + error * dt, -100, 100);
}
m.pwm = constrain(m.pwm + m.Kp * error + m.Ki * m.integral, 0, 255);
setMotorPWM(m, m.pwm);
}
void updateControl() {
unsigned long now = millis();
if (now - lastControlTime < (unsigned long)intervalMs) {
return;
}
float dt = (now - lastControlTime) / 1000.0f;
lastControlTime = now;
updateRPM(leftMotor, dt);
updateRPM(rightMotor, dt);
if (robotState == STATE_WALL_FOLLOW ||
robotState == STATE_BALL_OVERRIDE) {
updatePID(leftMotor, dt);
updatePID(rightMotor, dt);
}
}
// ================= SERIAL READ =================
void readSerial() {
while (Serial.available() >= 3) {
if (Serial.peek() != 0xFF) {
Serial.read();
continue;
}
Serial.read();
pkt[0] = Serial.read();
pkt[1] = Serial.read();
lastPacketMs = millis();
}
}
// ================= BREAK BEAM =================
bool breakBeamTriggered() {
return digitalRead(BREAK_BEAM) == LOW;
}
// ================= STALL DETECTOR =================
void checkStall() {
if (robotState != STATE_WALL_FOLLOW &&
robotState != STATE_BALL_OVERRIDE) {
leftStallSinceMs = millis();
rightStallSinceMs = millis();
lastStallCheckL = leftMotor.count;
lastStallCheckR = rightMotor.count;
return;
}
unsigned long now = millis();
bool leftStalled = false;
bool rightStalled = false;
if (leftMotor.targetRPM > 0) {
long delta = leftMotor.count - lastStallCheckL;
if (delta > SLIP_STALL_COUNTS) {
lastStallCheckL = leftMotor.count;
leftStallSinceMs = now;
} else if (now - leftStallSinceMs > STALL_DETECT_MS) {
leftStalled = true;
}
} else {
leftStallSinceMs = now;
lastStallCheckL = leftMotor.count;
}
if (rightMotor.targetRPM > 0) {
long delta = rightMotor.count - lastStallCheckR;
if (delta > SLIP_STALL_COUNTS) {
lastStallCheckR = rightMotor.count;
rightStallSinceMs = now;
} else if (now - rightStallSinceMs > STALL_DETECT_MS) {
rightStalled = true;
}
} else {
rightStallSinceMs = now;
lastStallCheckR = rightMotor.count;
}
if (leftStalled || rightStalled) {
hardStop();
rollersOff();
stallBiasBoth = (leftStalled && rightStalled);
stallBiasLeft = leftStalled;
stallResumeState = robotState;
stallRecoveryStartMs = now;
robotState = STATE_STALL_RECOVERY;
}
}
// ====================================================================
// STATE HANDLERS
// ====================================================================
void doWallFollow() {
if (millis() - stateEntryMs < STATE_ENTRY_DELAY_MS) {
return;
}
if (leftDistanceCM > WALL_LOST_DIST) {
hardStop();
wallLostPhase = WL_REVERSE;
wallLostStartL = leftMotor.count;
wallLostStartR = rightMotor.count;
wlReverseStartMs = millis();
stateEntryMs = millis();
robotState = STATE_WALL_LOST;
return;
}
if (frontDistanceCM > 0 && frontDistanceCM < FRONT_STOP_DIST) {
hardStop();
turnStartCountL = leftMotor.count;
turnStartCountR = rightMotor.count;
stateEntryMs = millis();
robotState = STATE_TURNING;
return;
}
float baseRPM = (frontDistanceCM > 0 && frontDistanceCM < FRONT_SLOW_DIST)
? SLOW_SPEED_WF
: BASE_SPEED;
float error = leftDistanceCM - TARGET_LEFT_DIST;
float correction = constrain(1.0f * error, -5.0f, 5.0f);
leftMotor.targetRPM = constrain(baseRPM - correction, 0, 20);
rightMotor.targetRPM = constrain(baseRPM + correction, 0, 20);
}
void doTurning() {
if (millis() - stateEntryMs < STATE_ENTRY_DELAY_MS) {
return;
}
long travelL = abs(leftMotor.count - turnStartCountL);
long travelR = abs(rightMotor.count - turnStartCountR);
if (travelL >= TURN_COUNTS_L && travelR >= TURN_COUNTS_R) {
hardStop();
stateEntryMs = millis();
robotState = STATE_WALL_FOLLOW;
return;
}
int pwmVal = map(TURN_SPEED, 0, 20, 0, 255);
setMotorPWM(leftMotor, (travelL < TURN_COUNTS_L) ? pwmVal : 0);
setMotorPWM(rightMotor, (travelR < TURN_COUNTS_R) ? -pwmVal : 0);
}
void doWallLost() {
if (millis() - stateEntryMs < STATE_ENTRY_DELAY_MS) {
return;
}
switch (wallLostPhase) {
case WL_REVERSE: {
long travelL = abs(leftMotor.count - wallLostStartL);
bool wallBack = (leftDistanceCM <= WALL_RECOVERY_DIST);
bool maxReached = (travelL >= REVERSE_MAX_COUNTS_L);
bool timedOut = (millis() - wlReverseStartMs >= REVERSE_TIMEOUT_MS);
if (wallBack || maxReached || timedOut) {
hardStop();
wallLostStartL = leftMotor.count;
wallLostStartR = rightMotor.count;
wallLostPhase = WL_CREEP;
stateEntryMs = millis();
return;
}
int pwmVal = map(WALL_LOST_OL_SPEED, 0, 20, 0, 255);
setMotorPWM(leftMotor, -pwmVal);
setMotorPWM(rightMotor, -pwmVal);
break;
}
case WL_CREEP: {
long creepL = abs(leftMotor.count - wallLostStartL);
long creepR = abs(rightMotor.count - wallLostStartR);
if (creepL >= CREEP_COUNTS_L_WF &&
creepR >= CREEP_COUNTS_R_WF) {
hardStop();
arcStartMs = millis();
wallLostPhase = WL_ARC;
stateEntryMs = millis();
return;
}
int pwmVal = map(WALL_LOST_OL_SPEED, 0, 20, 0, 255);
setMotorPWM(leftMotor, pwmVal);
setMotorPWM(rightMotor, pwmVal);
break;
}
case WL_ARC: {
if (breakBeamTriggered()) {
rollersOff();
} else if (ballVisible() && pkt[1] >= BALL_ARC_Y_THRESHOLD) {
rollersOn();
}
if (leftDistanceCM <= WALL_RECOVERY_DIST) {
hardStop();
rollersOff();
stateEntryMs = millis();
robotState = STATE_WALL_FOLLOW;
return;
}
if (millis() - arcStartMs > ARC_TIMEOUT_MS) {
hardStop();
rollersOff();
robotState = STATE_STOPPED;
return;
}
setMotorPWM(rightMotor, map(ARC_OUTER_SPEED, 0, 20, 0, 255));
setMotorPWM(leftMotor, map(ARC_INNER_SPEED, 0, 20, 0, 255));
break;
}
case WL_REALIGN: {
long sL = abs(leftMotor.count - wallLostStartL);
long sR = abs(rightMotor.count - wallLostStartR);
if (sL >= REALIGN_COUNTS && sR >= REALIGN_COUNTS) {
hardStop();
stateEntryMs = millis();
robotState = STATE_WALL_FOLLOW;
return;
}
int pwmVal = map(SLOW_SPEED_WF, 0, 20, 0, 255);
setMotorPWM(leftMotor, pwmVal);
setMotorPWM(rightMotor, pwmVal);
break;
}
}
}
void doBallOverride() {
switch (ballPhase) {
case BP_TRACK: {
if (millis() - ballPhaseMs < BALL_ENTRY_DELAY_MS) {
return;
}
if (!ballVisible() || leftDistanceCM < BALL_TOO_CLOSE_TO_WALL) {
rollersOn();
hardStop();
ballPhase = BP_COLLECT;
ballPhaseMs = millis();
return;
}
if (robotState == STATE_WALL_LOST &&
pkt[1] >= BALL_ARC_Y_THRESHOLD) {
rollersOn();
}
float error = (float)pkt[0] - (float)X_CENTER;
float correction = 0;
if (abs(error) > DEAD_BAND) {
correction = constrain(Kp_heading * error,
-(float)MAX_CORRECTION,
(float)MAX_CORRECTION);
}
leftMotor.targetRPM = constrain((float)TRACK_SPEED + correction,
0,
(float)(TRACK_SPEED + MAX_CORRECTION));
rightMotor.targetRPM = constrain((float)TRACK_SPEED - correction,
0,
(float)(TRACK_SPEED + MAX_CORRECTION));
break;
}
case BP_COLLECT: {
rollersOn();
if (breakBeamTriggered() ||
millis() - ballPhaseMs > ROLLERS_TIMEOUT_MS) {
hardStop();
rollersOff();
ballPhase = BP_COLLECTED;
ballPhaseMs = millis();
return;
}
leftMotor.targetRPM = CREEP_SPEED_BALL;
rightMotor.targetRPM = CREEP_SPEED_BALL;
break;
}
case BP_COLLECTED: {
rollersOff();
if (millis() - ballPhaseMs <= 2000) {
hardStop();
break;
}
static bool revInitDone = false;
static float revStartReading = 0.0f;
static float revMinReading = 0.0f;
static bool revNeedReverse = false;
static bool revCurlBack = false;
static unsigned long revStartMs = 0;
if (!revInitDone) {
while (Serial.available()) {
Serial.read();
}
lastPacketMs = 0;
float f = frontDistanceCM;
revNeedReverse = (f > FRONT_REV_BAND_LO &&
f < FRONT_REV_BAND_HI);
revStartReading = f;
revMinReading = f;
revCurlBack = false;
revStartMs = millis();
revInitDone = true;
}
if (!revNeedReverse) {
hardStop();
revInitDone = false;
stateEntryMs = millis();
robotState = STATE_WALL_FOLLOW;
break;
}
int pwmVal = map(WALL_LOST_OL_SPEED, 0, 20, 0, 255);
setMotorPWM(leftMotor, -pwmVal);
setMotorPWM(rightMotor, -pwmVal);
float f = frontDistanceCM;
if (f > 0.0f && f < revMinReading) {
revMinReading = f;
}
if (f > 0.0f && f < revStartReading - 1.0f) {
revCurlBack = true;
}
bool done = false;
if (!revCurlBack) {
done = (f > revStartReading + FRONT_REV_RISE_CLEAR);
} else {
done = (f >= FRONT_STOP_DIST);
}
if (millis() - revStartMs >= REVERSE_TIMEOUT_MS) {
done = true;
}
if (done) {
hardStop();
revInitDone = false;
stateEntryMs = millis();
robotState = STATE_WALL_FOLLOW;
}
break;
}
}
}
void doStallRecovery() {
if (millis() - stallRecoveryStartMs >= STALL_REVERSE_MS) {
hardStop();
leftStallSinceMs = millis();
rightStallSinceMs = millis();
lastStallCheckL = leftMotor.count;
lastStallCheckR = rightMotor.count;
stateEntryMs = millis();
robotState = stallResumeState;
return;
}
int fullPWM = STALL_REVERSE_PWM;
int halfPWM = STALL_REVERSE_PWM / 2;
if (stallBiasBoth || stallBiasLeft) {
setMotorPWM(leftMotor, -fullPWM);
setMotorPWM(rightMotor, -halfPWM);
} else {
setMotorPWM(rightMotor, -fullPWM);
setMotorPWM(leftMotor, -halfPWM);
}
}
void doExit() {
if (exitPhase == EX_CREEP) {
long tL = abs(leftMotor.count - exitStartL);
long tR = abs(rightMotor.count - exitStartR);
if (tL >= CREEP_COUNTS_L_WF &&
tR >= CREEP_COUNTS_R_WF) {
hardStop();
exitPhase = EX_STOP;
} else {
int pwmVal = map(WALL_LOST_OL_SPEED, 0, 20, 0, 255);
setMotorPWM(leftMotor, pwmVal);
setMotorPWM(rightMotor, pwmVal);
}
}
}
// ====================================================================
// SETUP
// ====================================================================
void setup() {
Serial.begin(115200);
pinMode(EN_PIN, OUTPUT);
pinMode(SF_PIN, INPUT);
pinMode(FRONT_IR_PIN, INPUT);
pinMode(LEFT_IR_PIN, INPUT);
pinMode(ROLLER_PIN, OUTPUT);
pinMode(BREAK_BEAM, INPUT_PULLUP);
rollersOff();
initMotor(leftMotor, M1_DIR, M1_PWM, ENC_L, 342.0f, false);
initMotor(rightMotor, M2_DIR, M2_PWM, ENC_R, 347.0f, false);
attachInterrupt(digitalPinToInterrupt(ENC_L), leftISR, RISING);
attachInterrupt(digitalPinToInterrupt(ENC_R), rightISR, RISING);
enableDriver();
delay(2000);
lastControlTime = millis();
prevIRTime = micros();
stateEntryMs = millis();
leftStallSinceMs = millis();
rightStallSinceMs = millis();
lastStallCheckL = 0;
lastStallCheckR = 0;
Serial.println("=== Wall-Follow + Ball Collector Started ===");
Serial.print("Target left dist : ");
Serial.print(TARGET_LEFT_DIST);
Serial.println(" cm");
Serial.print("Wall lost dist : ");
Serial.print(WALL_LOST_DIST);
Serial.println(" cm");
Serial.print("Base speed : ");
Serial.print(BASE_SPEED);
Serial.println(" RPM");
Serial.print("Track speed : ");
Serial.print(TRACK_SPEED);
Serial.println(" RPM");
Serial.print("Creep speed : ");
Serial.print(CREEP_SPEED_BALL);
Serial.println(" RPM");
Serial.print("Ball timeout : ");
Serial.print(BALL_TIMEOUT_MS);
Serial.println(" ms");
Serial.print("Entry delay WF : ");
Serial.print(STATE_ENTRY_DELAY_MS);
Serial.println(" ms");
Serial.print("Reverse timeout : ");
Serial.print(REVERSE_TIMEOUT_MS);
Serial.println(" ms");
Serial.print("Stall detect : ");
Serial.print(STALL_DETECT_MS);
Serial.println(" ms");
Serial.print("Stall reverse : ");
Serial.print(STALL_REVERSE_MS);
Serial.println(" ms");
Serial.println("============================================");
}
// ====================================================================
// LOOP
// ====================================================================
void loop() {
readSerial();
updateIR();
updateControl();
checkStall();
// --- Exit detection ---
if (robotState != STATE_EXIT &&
robotState != STATE_STOPPED) {
if (millis() > 3000 &&
frontDistanceCM > EXIT_FRONT_DIST &&
leftDistanceCM > EXIT_LEFT_DIST) {
if (!exitCandidate) {
exitCandidate = true;
exitDetectedMs = millis();
} else if (millis() - exitDetectedMs >= EXIT_CONFIRM_MS) {
hardStop();
rollersOff();
exitPhase = EX_CREEP;
exitStartL = leftMotor.count;
exitStartR = rightMotor.count;
robotState = STATE_EXIT;
exitCandidate = false;
}
} else {
exitCandidate = false;
}
}
if (robotState == STATE_WALL_FOLLOW &&
ballVisible() &&
!ballCooldown) {
hardStop();
ballPhase = BP_TRACK;
ballPhaseMs = millis();
robotState = STATE_BALL_OVERRIDE;
}
if (ballCooldown &&
millis() - ballGiveUpMs >= BALL_GIVUP_COOLDOWN_MS) {
ballCooldown = false;
rollersOff();
}
switch (robotState) {
case STATE_WALL_FOLLOW:
doWallFollow();
break;
case STATE_TURNING:
doTurning();
break;
case STATE_WALL_LOST:
doWallLost();
break;
case STATE_BALL_OVERRIDE:
doBallOverride();
break;
case STATE_STALL_RECOVERY:
doStallRecovery();
break;
case STATE_EXIT:
doExit();
break;
case STATE_STOPPED:
hardStop();
break;
}
}