Organising Code for Multi-Sensor Robot Projects

Start with three jobs, not one big loop

When you first get a robot moving, it is tempting to write one long loop: read a sensor, make a decision, set the motors, repeat. That works for a simple line follower on a kitchen floor. Add a second sensor or a bump switch, and the loop becomes a tangle of if statements. You change one wire and three things break. The fix is simple: decide that your program has three separate jobs. Read the sensors. Decide what to do. Drive the motors. Write one function for each job, and call them in order.

Reading sensors without mixing in decisions

A sensor function should only answer a question. It should not decide anything, and it should never touch a motor. For example, read_distance_cm() returns a number. read_line_left() and read_line_right() return a value for each sensor. Keep these functions free of motor commands. Where you can, return values rather than writing to global variables. If your board has very little memory, globals are acceptable, but name them clearly, such as g_distance_cm.

• read_ultrasonic_cm() — sends a pulse, waits for the echo, returns a float in centimetres.
• read_line_sensors() — returns left and right values as a small list or two separate numbers.
• read_bump_switch() — returns true when pressed, with a short delay to ignore bounce.

Print the values to your serial monitor and watch them change as you wave your hand or slide a white card under the sensor. If a reading looks wrong, you know exactly where to look.

Keeping decisions separate and testable

The decision function takes sensor readings and returns a command. For example, decide_action(distance, left_line, right_line) might return "forward", "turn_left", "turn_right", or "stop". There should be no motor writes inside this function. That makes it easy to test without any hardware. Call decide_action(20, false, true) and check that it says "turn_left". Write a small table of inputs and expected outputs before you power the motors.

This is also the right place for timing. Instead of using delay(), which freezes your robot and stops it reading sensors, use a simple state machine based on millis(). Keep a variable such as robot_state that can be FORWARD, TURNING, or STOPPED. The decision function checks how long the robot has been in that state and returns the next command.

Motor control as a thin layer

Motor functions take a command and make it happen. set_motors(left_speed, right_speed) is a good starting point, with speeds from -100 to 100. You might also add drive_forward(speed), turn_left(speed), and stop(). Inside these functions, map your speed values to the PWM pins and set the direction pins. Keep all your calibration constants here: the minimum speed that actually moves the robot, the left and right trim values, and any inversion flags. If your robot pulls to one side, you adjust the trim in one place.

No sensor reads belong in the motor layer. The motor layer does not care why it is driving forward. It just drives. Also write an emergency_stop() function that cuts the motors immediately. That one function can save your robot from a collision with the skirting board.

A practical folder and function layout

For a small project, one file with clear comment banners is perfectly fine. For a larger build, split the code into sensors.py, decisions.py, motors.py, and main.py. Either way, use the same order inside each file:

• Constants and pin numbers at the very top, so you change wiring in one place.
• setup() initialises pins, serial, and any sensor objects.
• loop() calls read_sensors(), then decide_action(), then drive_motors().
• Sensor functions grouped together.
• Decision functions grouped together.
• Motor functions grouped together.

Use names that say what they do. read_distance_cm() is good. read1() or doStuff() will confuse you next month. If you are working with a friend or a school club, clear names mean they can read your code without a guided tour.

Testing each part on the bench before you drive

Before the robot moves under its own power, test the sensors alone. Print the distance values and check they match a tape measure. Test the motors alone with a simple sweep from slow to fast. Then test the decision function with fake inputs: set distance = 15, left_line = false, and right_line = true, and see what command comes back. Only when each part works on its own should you combine them.

When something goes wrong later, this separation is your best debugging tool. You can comment out read_sensors() and replace it with fixed values to see whether the problem is in the sensors, the decisions, or the motors. You can drive the motors directly from a test function without running the whole robot. This is how you fix a fault without chasing a moving robot around the kitchen table.

Start with three clear jobs, keep them in separate functions, and your robot will stay easy to test and modify. Next week you can add a new sensor by writing one new reading function and one new decision rule. The motor code does not need to change at all. That is the real reward of organising your code from the beginning.