Source Code

API Reference

This section documents the core modules from simple motor control up to complex navigation, automatically generated from source docstrings. Each section includes classes, methods, and usage parameters.

Motor Driver

The motor driver provides PWM control of DC motors through an H-bridge interface. This is the fundamental building block for velocity control across the platform.

Motor Driver Class

class motor.motor_driver(PWM_pin, DIR_pin, nSLP_pin, tim, chan)

H-bridge motor driver with PWM speed control and directional control.

Parameters:

PWM_pin (pyb.Pin) – Pin for PWM signal
DIR_pin (pyb.Pin) – Pin for direction control
nSLP_pin (pyb.Pin) – Pin for enable/sleep control (active high)
tim (pyb.Timer) – Timer object for PWM generation
chan (int) – Timer channel number for PWM output

motor.enable(): Enable the motor driver by setting nSLP pin high.

motor.disable(): Disable the motor driver by setting nSLP pin low.

motor.set_effort(effort)

Set motor effort with direction control.

Parameters:: effort (float) – Motor effort (-100 to +100, positive=forward, negative=reverse)

Motor Control Notes

Positive effort → DIR low (forward direction)
Negative effort → DIR high (reverse direction)
Effort range: -100 to +100 (percent duty cycle)
nSLP pin must be high to enable driver IC

Encoder

Position and velocity feedback is critical for both motor control and navigation. The encoder module provides quadrature decoding with hardware timer support and averaged velocity calculation to smooth sensor noise.

Encoder Class

class encoder.Encoder(tim, chA_pin, chB_pin)

Quadrature encoder driver with position tracking and averaged velocity.

Parameters:

tim (pyb.Timer) – Hardware timer object configured in encoder mode
chA_pin (pyb.Pin) – Pin for encoder channel A
chB_pin (pyb.Pin) – Pin for encoder channel B

encoder.update()

Update position and velocity from timer counter with rollover handling.

Calculates delta position handling 16-bit wraparound, updates velocity buffer with 5-point moving average.

encoder.get_position()

Get current encoder position in ticks.

Returns:: Accumulated position in ticks
Return type:: int

encoder.get_velocity()

Get 5-point averaged velocity.

Returns:: Velocity in ticks per microsecond
Return type:: float

encoder.zero(): Reset encoder position to zero.

Encoder Hardware Notes

Timer: Must be configured in ENC_AB encoder mode
Channels: A and B quadrature signals on timer channels 1 and 2
Counter Resolution: 16-bit (0-65535) with automatic rollover handling
Position: Accumulates to avoid counter reset issues
Velocity: 5-sample moving average reduces high-frequency noise

Closed-Loop Velocity Control

With motors and encoders in place, we implement PI control to maintain desired velocities. The closed-loop controller handles both straight-line tracking with IR-based line following and heading-based turning for navigation.

CL_control Class

class CL_control_task_V5.CL_control(multiple_ir_readings_object, motor)

Closed-loop PI controller for a single motor with line-following capability.

Parameters:

multiple_ir_readings_object (multiple_ir_readings) – IR sensor array object for line sensing
motor (str) – Motor identifier ("LEFT" or "RIGHT")

CL_control_task_V5.run(shares)

Cooperative task function implementing PI velocity control with line following.

Executes state machine: initialize → run (with line following) → optional rest and turning states.

Parameters:: shares (tuple) – Tuple containing all shared variables for inter-task communication

Velocity Control Parameters

Kp = 0.07              # Proportional gain
Ki = 0.035             # Integral gain
U_MAX = 100            # Maximum control output (% effort)
TICKS_PER_REV = 1437.1 # Encoder resolution
WHEEL_CIRC_MM = 220    # Wheel circumference in mm

Battery Compensation: Gains are scaled by \(K = K_{nominal} \times \frac{V_{nominal}}{V_{battery}}\) to maintain consistent performance as battery voltage drops.

Line Following Parameters

ln_const = 1.7         # Line error proportional gain
ln_integ_const = 1.0   # Line error integral gain

When line_follow_flg = 1, steering error from the IR sensor array modulates the differential velocity between left and right motors.

Heading Control Parameters

Kp_turn = 0.45         # Heading proportional gain
Ki_turn = 0.01         # Heading integral gain
TURN_EFFORT_MAX = 20   # Maximum turning effort

When nav_turn_flg = 1, heading error from the encoder odometry drives motor differential.

Anti-Windup Protection

Prevents integral accumulation during saturation:

satur_hi = (u >= U_MAX - 1e-9)
satur_lo = (u <= -U_MAX + 1e-9)
if (not satur_hi or e < 0) and (not satur_lo or e > 0):
    integ += e * dt

Force-Straight Mode

When force_straight_flg = 1, line following is disabled (ln_error = 0) and only heading-based control is active, providing precise directional control for turns.

Line Following

For the vehicle to track a line, we extract steering error from the IR sensor array using centroid-based weighting. This error then feeds into the closed-loop controller’s differential velocity command.

Line following controller using centroid-based error calculation. Computes steering error from IR sensor array with configurable bias for directional preference.

Hardware:

IR Sensor Array: 7 sensors (indexed -3 to +3 from left to right)
ADC Resolution: 12-bit (0-4095)

Notes:

Positive error = line to the right, turn right
Negative error = line to the left, turn left
Bias allows preferential steering for forks or curves

class line_follow_V1.LineFollower(multi_sensor_read_object, black, white, bias=0.0)[source]

Line following controller with weighted centroid error calculation.

calculate_error()[source]

Calculate steering error using weighted centroid of normalized sensor readings.

Returns:: Steering error (-3 to +3, including bias)
Return type:: float

calibrate()[source]

Calculate average sensor reading for calibration.

Parameters:: multi_sensor_read_object (multiple_ir_readings) – IR sensor array object
Returns:: Average ADC value across all 7 sensors
Return type:: float

get_raw_readings()[source]

Get raw sensor readings for debugging or calibration.

Returns:: List of raw ADC values from all sensors
Return type:: list

set_bias(bias)[source]

Update steering bias dynamically.

Parameters:: bias (float) – New steering bias value

LineFollower Class

class line_follow_V1.LineFollower(multi_sensor_read_object, black, white, bias=0.0)[source]

Line following controller with weighted centroid error calculation.

Parameters:

multi_sensor_read_object (multiple_ir_readings) – IR sensor array object
black (int) – ADC value for black surface (line)
white (int) – ADC value for white surface (background)
bias (float) – Steering bias (positive=right, negative=left, range: -3 to +3)

line_follow_V1.calculate_error()

Calculate steering error using weighted centroid of normalized sensor readings.

Algorithm:

Normalize: \(norm = \frac{reading - white}{black - white}\), clamped to [0, 1]
Compute centroid: \(error = \frac{\sum w_i \cdot norm_i}{\sum norm_i}\)
Apply bias: \(final\_error = error + bias\)

Returns:: Steering error (-3 to +3, including bias)
Return type:: float

line_follow_V1.set_bias(bias)

Update steering bias dynamically for curves or fork detection.

Parameters:: bias (float) – New steering bias value

line_follow_V1.get_raw_readings()

Get raw sensor readings for debugging or calibration.

Returns:: List of raw ADC values from all sensors
Return type:: list

line_follow_V1.calibrate(multi_sensor_read_object)

Calculate average sensor reading for calibration reference.

Parameters:: multi_sensor_read_object (multiple_ir_readings) – IR sensor array object
Returns:: Average ADC value across all 7 sensors
Return type:: float

Error Convention

Positive error: Line to the right → turn right
Negative error: Line to the left → turn left
Bias: Allows preferential steering for complex course sections (forks, curves)

Autonomous Navigation

The navigation task orchestrates the entire system through a 24-state finite state machine. It sequences line following, heading control, and collision recovery to autonomously navigate the obstacle course.

Obstacle course navigation state machine using encoder-based odometry. 24-state FSM coordinating line following, heading control, and obstacle avoidance. Uses encoder distance and heading instead of IMU for improved scheduler performance.

Hardware:

Motor Encoders: 1437.1 ticks/rev differential encoders
IR Sensor Array: 7 reflectance sensors for line detection
Bump Sensor: Wall collision detection

Notes:

Distance thresholds measured in mm from encoder odometry
Heading control uses encoder differential (not IMU)
State 15 triggers bump sensor wall interaction sequence (States 16-23)
Line following bias adjustable per state for curve navigation
Force-straight mode overrides line following for precise heading control

navigation_v2_compact_encoder.navigation_task_fun(shares)[source]

Cooperative task function implementing obstacle course navigation FSM.

Parameters:: shares (tuple) – Tuple of shared variables for inter-task communication

Navigation Task Function

navigation_v2_compact_encoder.navigation_task_fun(shares)[source]

Cooperative task function implementing 24-state obstacle course FSM.

Handles course progression, state transitions based on distance/heading thresholds, and wall collision recovery.

Parameters:: shares (tuple) – Tuple containing all navigation-related shared variables

Course Navigation States (0–15)

The robot progresses through distinct course sections, each with distance targets and heading setpoints:

State 0:  Line follow (bias=0)          to 650 mm
State 1:  Fork quarter-circle (bias=1.55) to 914 mm
State 2:  Turn to heading -90 deg
State 3:  Straight at -90 deg          to 1114 mm (diamond section)
State 4:  U-turn semicircle            to 1742 mm
State 5:  Turn to heading 90 deg
State 6:  Straight at 90 deg           to 1992 mm (parking garage)
State 7:  Fork quarter-circle (bias=1.8) to 2227 mm
State 8:  Complex 2x quarter-circles   to 3549 mm
State 9:  Turn to heading 180 deg
State 10: Straight at 180 deg          to 3849 mm
State 11: Line follow (bias=0.2)       to 4099 mm
State 12: Turn to heading 180 deg
State 13: Straight through parking     to 4799 mm
State 14: Turn to heading 90 deg
State 15: Straight until bump or       to 5149 mm

Wall Recovery Sequence (States 16–23)

When bump sensor detects collision during State 15:

State 16: Back up 50 mm
State 17: Turn to heading 0 deg
State 18: Go straight 175 mm at 0 deg
State 19: Turn to heading 90 deg
State 20: Go straight 200 mm at 90 deg
State 21: Turn to heading 180 deg
State 22: Line follow 200 mm (bias=0)
State 23: STOP - course complete

Configuration Parameters

Distance Thresholds (millimeters)

STATE_0_DIST   = 650.0         # Start to fork entrance
STATE_1_DIST   = 914.0         # Fork quarter-circle
STATE_3_DIST   = 1114.0        # Through diamond
STATE_4_DIST   = 1742.0        # After U-turn
STATE_6_DIST   = 1992.0        # Before second fork
STATE_7_DIST   = 2227.0        # Second fork complete
STATE_8_DIST   = 3549.0        # Complex section complete
STATE_10_DIST  = 3849.0        # Approach finish
STATE_11_DIST  = 4099.0        # Enter final parking
STATE_13_DIST  = 4799.0        # Exit final parking
STATE_15_DIST  = 5149.0        # Maximum distance

Heading Targets (degrees)

HEADING_NEG_90   = -90.0       # Diamond section heading
HEADING_90       = 90.0        # Parking garage heading
HEADING_180      = 180.0       # Return path heading
HEADING_TOLERANCE = 4.0        # +/- 4 deg acceptance band

Control Modes

Line Following Mode (line_follow_flg = 1): Uses IR sensor centroid with configurable bias for directional tracking.
Force Straight Mode (force_straight_flg = 1): Disables line following; uses encoder-based heading control for precision turns.
Turning Mode (nav_turn_flg = 1): Heading-based PI control with zero velocity until target heading reached.

Helper Functions

navigation_v2_compact_encoder.calc_heading_error(current_heading, target_heading)

Calculate normalized heading error in range [-180, 180].

Handles angle wrapping to find shortest path to target heading.

Parameters:

current_heading (float) – Current robot heading in degrees
target_heading (float) – Desired heading in degrees

Returns:

Heading error in degrees

Return type:

float

Encoder Odometry

To navigate the course, we need to track position and heading. The encoder odometry task reads dual encoders and computes distance traveled and heading angle using kinematic relationships.

Odometry task for calculating robot heading and distance from encoder data. Computes heading from encoder differential and distance from average encoder ticks. Uses unwrapping to handle ±180° discontinuities smoothly.

Hardware:

Motor Encoders: 1437.1 ticks/rev
Wheel Radius: 35mm
Track Width: 141mm

Notes:

Positive heading = counter-clockwise rotation (left turn)
Heading normalized to [-180, 180] degrees
Distance calculated as average of left and right encoder ticks

encoder_heading_task.encoder_heading_task_fun(shares)[source]

Cooperative task function that computes heading and distance from encoders.

Parameters:: shares (tuple) – Tuple of (left_enc_pos, right_enc_pos, enc_heading_share, enc_distance_share)

Encoder Odometry Function

encoder_heading_task.encoder_heading_task_fun(shares)[source]

Cooperative task function that computes heading and distance from encoders.

Updates shared state from left and right encoder positions at each iteration.

Parameters:: shares (tuple) – Tuple of (left_enc_pos, right_enc_pos, enc_heading_share, enc_distance_share)

Hardware Constants

WHEEL_RADIUS_MM = 35.0         # Wheel radius in millimeters
TICKS_PER_REV = 1437.1         # Encoder ticks per complete revolution
TRACK_WIDTH_MM = 141.0         # Distance between left and right wheels
WHEEL_CIRC_MM = 220.0          # Wheel circumference: 2π × radius
MM_PER_TICK = 0.153            # Millimeters per encoder tick

Odometry Calculations

Distance Traveled

\[\text{distance} = \frac{\text{left_ticks} + \text{right_ticks}}{2} \cdot \text{mm_per_tick}\]

Average of both encoders smooths odometry error and provides forward displacement in millimeters.

Heading Angle

\[\text{ticks_diff} = \text{left_ticks} - \text{right_ticks}\]

\[\theta_{\text{rad}} = \frac{\text{ticks_diff}}{\text{TICKS_PER_REV}} \left( \frac{\text{WHEEL_CIRC_MM}}{\text{TRACK_WIDTH_MM}} \right)\]

\[\theta_{\text{deg}} = \theta_{\text{rad}} \cdot \frac{180}{\pi}\]

Positive heading indicates counter-clockwise rotation (left turn).

Heading Unwrapping

To handle the ±180° discontinuity smoothly, delta heading is checked and adjusted:

delta = heading_deg - prev_heading
if delta > 180:
    delta -= 360
elif delta < -180:
    delta += 360
continuous_heading += delta

Odometry Notes

Positive heading = counter-clockwise rotation (left turn)
Heading normalized to [-180, 180] deg range
Heading unwrapping prevents jumps at +/- 180 deg boundary
Distance accumulation provides cumulative traveled distance

Supporting Sensors

Bump Sensor

encoder_heading_task.bump_sensor_task_fun(shares)

Cooperative task function that monitors bump sensor and updates shared state.

Detects wall collisions during navigation and signals recovery sequence.

Parameters:: shares (tuple) – Tuple containing bump_detected_share (Share object)

Bump Sensor Hardware

Pin: PC11 (active LOW, pull-up resistor enabled)
Debounce Time: 30 ms
Trigger: Pressed (logic 0) → sets bump_detected_share = 1

Algorithm

Read bump sensor pin state
If pressed (LOW) and not already detected:
1. Wait 30 ms (debounce time)
2. Confirm still pressed
3. Set bump_detected_share = 1
4. Set bump_already_detected = True (prevents re-trigger)
5. Print detection message
Yield to scheduler

Bump Sensor Notes

Sensor is active LOW: pressed = 0, released = 1
Only triggers once per run (one-shot behavior)
30 ms debounce prevents false triggers from noise
Used in navigation State 15 to trigger wall recovery

Battery Monitor

Battery voltage monitoring module using ADC (Analog-to-Digital Converter). Reads voltage directly from pin PC0 with configurable averaging and sampling. Provides both class-based and convenience function interfaces for battery voltage measurements.

Hardware:

ADC Pin: PC0 (default, configurable)
Reference Voltage: 3.3V
Resolution: 12-bit ADC

Notes:

No voltage scaling applied; reads direct ADC voltage
For battery monitoring, ensure voltage divider if battery > 3.3V

class battery_adc.BatteryMonitor(pin_name='PC0', vref=3.3, adc_bits=12, samples=8, sample_delay_ms=0)[source]

ADC-based battery voltage monitor with configurable averaging.

read_voltage()[source]

Read and return averaged ADC voltage in volts.

Returns:: Averaged voltage reading in volts
Return type:: float

battery_adc.battery_voltage()[source]

Quick-access function to read battery voltage using default settings.

Automatically initializes with default settings on first call.

Returns:: Current ADC voltage in volts
Return type:: float

battery_adc.configure(pin_name='PC0', vref=3.3, adc_bits=12, samples=8, sample_delay_ms=0)[source]

Configure the default battery monitor with custom settings.

Parameters:

pin_name (str) – ADC pin name (default ‘PC0’)
vref (float) – Reference voltage for ADC in volts (default 3.3)
adc_bits (int) – ADC resolution in bits (default 12)
samples (int) – Number of readings to average (default 8)
sample_delay_ms (int) – Optional delay between samples in ms (default 0)

Battery voltage monitoring is critical for compensating motor control gains as battery discharges during operation.

class battery_adc.BatteryMonitor(adc_pin=None, samples=10)[source]

ADC-based battery voltage monitor with configurable averaging.

Parameters:

adc_pin (pyb.Pin or str) – ADC pin for voltage measurement (default: PC0)
samples (int) – Number of samples for averaging

Battery Hardware

ADC Pin: PC0 (default, configurable)
Reference Voltage: 3.3 V
ADC Resolution: 12-bit (0–4095)
Voltage Scaling: Depends on voltage divider on battery circuit

battery_adc.battery_voltage()[source]

Get current battery voltage (convenience function).

Returns:: Battery voltage in volts
Return type:: float

Battery Monitor Notes

Voltage readings are averaged to reduce noise
Used in CL_control to scale PI gains
Maintains consistent control performance across battery discharge
Nominal battery voltage: 3.07 V (example reference)

IR Sensor Array

The multi-sensor IR array provides simultaneous synchronized readings from all 7 sensors using timer-triggered ADC conversions. This ensures all sensors are sampled at the same instant for accurate centroid calculations.

multiple_ir_readings Class

class multi_sensor_read.multiple_ir_readings(pin_1, pin_2, pin_3, pin_4, pin_5, pin_6, pin_7)

Seven-sensor IR array interface with timer-synchronized ADC sampling.

Parameters:

pin_1 (pyb.Pin) – Pin object for sensor 1 (leftmost)
pin_2 (pyb.Pin) – Pin object for sensor 2
pin_3 (pyb.Pin) – Pin object for sensor 3
pin_4 (pyb.Pin) – Pin object for sensor 4 (center)
pin_5 (pyb.Pin) – Pin object for sensor 5
pin_6 (pyb.Pin) – Pin object for sensor 6
pin_7 (pyb.Pin) – Pin object for sensor 7 (rightmost)

multi_sensor_read.read()

Read all 7 IR sensors simultaneously using timed ADC sampling.

Uses ADC.read_timed_multi() to ensure all sensors are sampled at the exact same instant, eliminating timing skew that could affect centroid calculations.

Returns:: List of 7 averaged ADC values [sensor1, sensor2, …, sensor7]
Return type:: list

IR Array Hardware

Sensors: 7 analog IR reflectance sensors
ADC Resolution: 12-bit (0-4095)
Sampling Timer: Timer 8 at 100Hz
Buffer Size: 1 (instantaneous readings without averaging)
Sensor Spacing: ~8mm between sensors
Sensor Indexing: 1 (left) to 7 (right)

Timed Multi-ADC Algorithm

The read_timed_multi() function synchronizes all 7 ADC conversions to a single timer event:

Timer 8 triggers at 100Hz
All 7 ADCs sample simultaneously on timer event
Results stored in pre-allocated arrays
Average calculated (trivial with buffer_size=1)

This ensures temporal coherence across the sensor array, critical for accurate line position estimation.

Motor Control Task

The motor control task manages individual motors through a two-state FSM, handling initialization, encoder data collection, and motor effort updates. This task provides the foundation for velocity control.

motor_control_task Class

class motor_ctrl_task_V3.motor_control_task(motor, encoder)

Motor control task for single motor with encoder feedback and data logging.

Parameters:

motor (motor_driver) – Motor driver object
encoder (Encoder) – Encoder object for position/velocity feedback

motor_ctrl_task_V3.run(shares)

Cooperative task function implementing motor control state machine.

Executes two-state FSM: initialize motor → continuous data collection and effort updates.

Parameters:: shares (tuple) – Tuple of shared variables for inter-task communication

Shared Variables:

start_flg (Share): Start signal (0 or 1)
set_point (Share): Motor control effort (-100 to +100)
end_flg (Share): End signal (0 or 1)
enc_pos (Share): Encoder position output (ticks)
enc_speed (Share): Encoder velocity output (ticks/us)
exp_time (Share): Elapsed time output (microseconds)
desired_time_ms (Share): Maximum run time (milliseconds)
proc_data_flg (Share): Data processing flag
desired_vel (Share): Desired velocity setpoint

State Machine

State 0: Motor Start/Initialization

if start_flg == 1:
    - Zero encoder position
    - Enable motor driver
    - Record start time
    - Transition to State 1

State 1: Continuous Data Collection

While end_flg == 0:
    - Update encoder position and velocity
    - Apply motor effort from set_point
    - Track elapsed time
    - Check time limit
When end_flg == 1:
    - Disable motor
    - Return to State 0

Motor Control Notes

Runs at 12ms period (highest frequency task)
Handles divide-by-zero in velocity calculations
Calls garbage collector to prevent memory fragmentation
Time limit enforcement prevents runaway operation
Zero-crossing initialization ensures clean startup

Serial Communication

The serial task provides Bluetooth command interface for calibration, parameter configuration, and trial control. It bridges the gap between user commands and robot operation.

Serial Task Function

serial_task_V6.serial_task_fun(shares)

Cooperative task function for Bluetooth serial communication and control.

Implements three-state FSM: wait for commands → handle trials → send data (unused).

Parameters:: shares (tuple) – Tuple of shared variables for inter-task communication

Shared Variables:

left_start_flg (Share): Left motor start flag
right_start_flg (Share): Right motor start flag
left_desired_vel (Share): Left motor velocity setpoint
right_desired_vel (Share): Right motor velocity setpoint
left_end_flg (Share): Left motor end flag
right_end_flg (Share): Right motor end flag
left_proc_data_flg (Share): Left data processing flag
right_proc_data_flg (Share): Right data processing flag
proc_pos_right (Queue): Right position data queue
proc_pos_left (Queue): Left position data queue
proc_speed_right (Queue): Right speed data queue
proc_speed_left (Queue): Left speed data queue
left_proc_time (Queue): Left timing data queue
right_proc_time (Queue): Right timing data queue
calibration_flg (Share): Line sensor calibration status (0-3)
observer_calibration_flg (Share): Observer calibration trigger

Command Protocol

PARAMS Command

Format: PARAMS,trial_time,desired_velocity
Example: PARAMS,0,50

Sets trial duration and desired velocity for both motors.

CALIBRATE_BLACK Command

Format: CALIBRATE_BLACK

Triggers black surface calibration (sets calibration_flg = 1).

CALIBRATE_WHITE Command

Format: CALIBRATE_WHITE

Triggers white surface calibration (sets calibration_flg = 2).

CALIBRATION_COMPLETE Command

Format: CALIBRATION_COMPLETE

Signals calibration finished (sets calibration_flg = 3).
Automatically starts motors after this command.

END_COMM Command

Format: END_COMM

Stops current trial and resets system to wait state.

Serial Hardware

UART: UART 1 (PA9/PA10)
Baud Rate: 460800
Protocol: ASCII text with newline termination
Module: HC-05 Bluetooth (or similar)
Data Bits: 8
Stop Bits: 1
Parity: None

State Machine Workflow

State 0: Wait for Commands

Listen for PARAMS, CALIBRATE_*, CALIBRATION_COMPLETE
Parse parameters and update shared variables
Transition to State 1 when calibration complete

State 1: Handle Trials

Monitor for END_COMM command
Check for trial completion (both end flags set)
Clear data queues on trial completion
Return to State 0 when trial ends

State 2: Send Data (Currently Unused)

Reserved for future telemetry transmission

Serial Task Notes

Runs at 150ms period (lowest priority)
Automatic motor start after calibration
Robust command parsing with exception handling
Clears UART buffer after parameter reception
Synchronizes left and right motor operations

PC User Interface

The PC-side user interface provides a simple command-line tool for controlling the robot via Bluetooth. This program runs on the user’s laptop, sending calibration commands and monitoring robot output.

Main Function

serial_task_V6.main()

Main program loop for robot control interface.

Establishes serial connection to robot and runs trials based on user input. Handles connection setup, trial execution, and graceful shutdown.

Trial Execution Function

serial_task_V6.run_trial(bt)

Execute single trial with automatic calibration and monitoring.

Sends calibration sequence, then continuously monitors and displays robot debug output until user interrupts with Ctrl+C.

Parameters:: bt (serial.Serial) – Serial connection object to robot

User Interface Workflow

1. Connection Establishment

port = 'COM6'  # Windows
# port = '/dev/tty.HC-05'  # macOS/Linux
bt = serial.Serial(port, baudrate=460800, timeout=1)

2. Trial Sequence

User presses Enter
↓
Send "PARAMS,0,50\n" (50 mm/s velocity)
↓
Send "CALIBRATE_BLACK\n" (preset calibration)
↓
Send "CALIBRATE_WHITE\n" (preset calibration)
↓
Send "CALIBRATION_COMPLETE\n" (start robot)
↓
Monitor MCU debug output (continuous)
↓
User presses Ctrl+C
↓
Send "END_COMM\n" (stop robot)

3. Output Monitoring

The interface continuously polls the serial port and displays all MCU output in real-time, providing visibility into robot state, navigation progress, and debug messages.

UI Configuration

Serial Port

Windows: COM6 (check Device Manager)
macOS/Linux: /dev/tty.HC-05 or /dev/ttyUSB0

Preset Parameters

trial_time = 0        # Unlimited duration (until END_COMM)
desired_velocity = 50  # mm/s forward velocity

Timing Delays

command_delay = 0.5s  # Between calibration commands
poll_interval = 0.1s  # MCU output polling rate

UI Notes

Platform: Runs on user’s PC (Windows/macOS/Linux)
Dependencies: pyserial library (pip install pyserial)
Calibration: Uses preset values (no user input required)
Monitoring: Real-time display of all MCU debug output
Termination: Ctrl+C sends END_COMM and returns to prompt
Restart: Press Enter to start another trial

Example Session

$ python UI_Line_Follow.py
Connected to COM6
Press Enter to start bot...

Sending: PARAMS,0,50
Sent: CALIBRATE_BLACK (preset)
Sent: CALIBRATE_WHITE (preset)
Sent: CALIBRATION_COMPLETE
MCU response: [OK] Starting navigation...

Bot running (press Ctrl+C to stop)...
MCU: [NAV] State 0 - Distance: 123.4 mm
MCU: [NAV] State 1 - Distance: 678.9 mm
^C

Stopping trial...
Press Enter to start bot...

Main Program

The main program orchestrates all subsystems through cooperative multitasking. It initializes hardware, creates shared variables, and launches the task scheduler for autonomous obstacle course navigation.

System Architecture

The main program coordinates eight cooperative tasks using the cotask scheduler:

Serial Task (Priority 3, Period 150ms) - Bluetooth command interface for calibration and control - Lowest period for non-critical communication
Left Motor Control Task (Priority 3, Period 12ms) - Updates left motor PWM and collects encoder data - Fastest task for responsive motor control
Right Motor Control Task (Priority 3, Period 12ms) - Updates right motor PWM and collects encoder data - Matches left motor for synchronized operation
Left Closed-Loop Control Task (Priority 3, Period 20ms) - PI velocity control for left motor - Line following and heading control integration
Right Closed-Loop Control Task (Priority 3, Period 20ms) - PI velocity control for right motor - Line following and heading control integration
Encoder Heading Task (Priority 3, Period 50ms) - Computes robot heading and distance from dual encoders - Provides odometry for navigation
Navigation Task (Priority 4, Period 50ms) - 24-state obstacle course FSM - Highest priority for critical decision-making
Bump Sensor Task (Priority 3, Period 20ms) - Collision detection and wall recovery trigger

Hardware Configuration

Motors and Encoders:

# Right motor on pins C9 (dir), H1 (PWM), H0 (nSLP), timer 3 channel 4
mot_right = motor_driver(Pin.cpu.C9, Pin.cpu.H1, Pin.cpu.H0, Timer(3, freq=30_000), 4)

# Left motor on pins B0 (dir), C12 (PWM), C10 (nSLP), timer 3 channel 3
mot_left = motor_driver(Pin.cpu.B0, Pin.cpu.C12, Pin.cpu.C10, Timer(3, freq=30_000), 3)

# Right encoder on pins A8, A9 (timer 1)
enc_right = Encoder(Timer(1, prescaler=0, period=0xFFFF), Pin.cpu.A8, Pin.cpu.A9)

# Left encoder on pins A0, B3 (timer 2)
enc_left = Encoder(Timer(2, prescaler=0, period=0xFFFF), Pin.cpu.A0, Pin.cpu.B3)

IR Sensor Array:

# 7 IR reflectance sensors for line detection
ir_pins = [Pin.cpu.C4, Pin.cpu.B1, Pin.cpu.A7, Pin.cpu.C1,
           Pin.cpu.A4, Pin.cpu.A1, Pin.cpu.C3]
ir_array = multiple_ir_readings(*ir_pins)

Bump Sensor:

# Collision detection sensor on pin PC11
# Active LOW with pull-up resistor

Robot Physical Parameters:

Wheel Diameter: 70mm (radius = 35mm)
Track Width: 141mm
Encoder Resolution: 1437.1 ticks/revolution
Wheel Circumference: 220mm

Shared Variables

The system uses 30+ shared variables for inter-task communication:

Motor Control:

left_start_flg, right_start_flg: Motor start signals
left_set_point, right_set_point: Control effort outputs
left_desired_vel, right_desired_vel: Velocity setpoints
left_end_flg, right_end_flg: Motor stop signals

Encoder Feedback:

left_enc_pos, right_enc_pos: Position (ticks)
left_enc_speed, right_enc_speed: Velocity (ticks/us)
enc_heading_share: Robot heading (degrees)
enc_distance_share: Distance traveled (mm)

Navigation Control:

line_follow_flg: Enable line following (0/1)
force_straight_flg: Force straight mode (0/1)
nav_turn_flg: Enable heading-based turning (0/1)
desired_angle_share: Target heading (degrees)
bias_share: Line following bias (-3 to +3)
bump_detected_share: Collision detection flag

Calibration:

calibration_flg: Line sensor calibration status (0-3)
observer_calibration_flg: Reserved for future use

Controller Objects:

left_controller_share: Left CL_control instance
right_controller_share: Right CL_control instance

Task Timing and Priorities

The task scheduler executes tasks based on priority (higher number = higher priority) and period:

Task Name	Priority	Period (ms)	Function
Navigation	4	50	Obstacle course FSM
Serial	3	150	Bluetooth command interface
Left Motor Control	3	12	Motor PWM and encoder
Right Motor Control	3	12	Motor PWM and encoder
Left CL Control	3	20	PI velocity control
Right CL Control	3	20	PI velocity control
Encoder Heading	3	50	Odometry calculation
Bump Sensor	3	20	Collision detection

Startup Sequence

The main program follows this initialization sequence:

Hardware Initialization: - Create motor driver objects with PWM timers - Initialize encoders with quadrature decoding - Set up IR sensor array with timer-triggered ADC - Configure bump sensor with pull-up resistor
Object Creation: - Create motor_control_task instances for both motors - Create CL_control instances for PI velocity control - Store controller objects in shared variables
Shared Variable Creation: - Create all 30+ shares for inter-task communication - Create queues for data logging (unused in final version) - Initialize calibration flags and control parameters
Task Creation: - Create 8 cooperative tasks with cotask.Task() - Assign priorities and periods for each task - Pass appropriate shares tuple to each task
Scheduler Execution: - Run garbage collector to free memory - Execute cotask.task_list.pri_sched() in infinite loop - Handle KeyboardInterrupt for graceful shutdown

Control Flow Example

A typical control cycle showing task coordination:

Time 0ms:
├─ Left Motor Control (12ms task)
│  ├─ Update encoder position and velocity
│  ├─ Apply motor effort from set_point
│  └─ Check time limits
│
├─ Right Motor Control (12ms task)
│  ├─ Update encoder position and velocity
│  ├─ Apply motor effort from set_point
│  └─ Check time limits

Time 20ms:
├─ Left CL Control (20ms task)
│  ├─ Read IR sensors for line position
│  ├─ Calculate line following error
│  ├─ Run PI velocity controller
│  └─ Update left_set_point
│
├─ Right CL Control (20ms task)
│  ├─ Read IR sensors for line position
│  ├─ Calculate line following error
│  ├─ Run PI velocity controller
│  └─ Update right_set_point
│
├─ Bump Sensor (20ms task)
│  ├─ Read bump sensor pin
│  ├─ Debounce for 30ms if pressed
│  └─ Set bump_detected_share if confirmed

Time 50ms:
├─ Encoder Heading (50ms task)
│  ├─ Read left and right encoder positions
│  ├─ Calculate heading from differential
│  ├─ Calculate distance from average
│  └─ Update enc_heading_share, enc_distance_share
│
├─ Navigation (50ms task - Priority 4)
│  ├─ Read current distance and heading
│  ├─ Check state transition conditions
│  ├─ Update line_follow_flg, bias_share
│  ├─ Set desired_angle_share for turns
│  └─ Check bump_detected_share for recovery

Time 150ms:
├─ Serial (150ms task)
│  ├─ Check for Bluetooth commands
│  ├─ Parse PARAMS, CALIBRATE_*, END_COMM
│  └─ Update calibration_flg, velocity setpoints

Memory Management

The program includes memory management for MicroPython:

import gc
gc.collect()  # Run before starting scheduler

This helps prevent memory fragmentation during long-running autonomous operation.

Main Program Notes

No IMU: Uses pure encoder-based odometry (simplified from original approach)
Bump Recovery: Automatic wall collision recovery in navigation FSM
Calibration: Bluetooth-controlled line sensor calibration before each run
Modularity: Each subsystem isolated in separate task with defined interfaces
Robustness: Cooperative multitasking prevents blocking and missed deadlines
Debugging: Serial output provides real-time visibility into robot state

Source Code

API Reference

Motor Driver

Motor Driver Class

Motor Control Notes

Encoder

Encoder Class

Encoder Hardware Notes

Closed-Loop Velocity Control

CL_control Class

Velocity Control Parameters

Line Following Parameters

Heading Control Parameters

Anti-Windup Protection

Force-Straight Mode

Line Following

LineFollower Class

Error Convention

Autonomous Navigation

Navigation Task Function

Course Navigation States (0–15)

Wall Recovery Sequence (States 16–23)

Configuration Parameters

Distance Thresholds (millimeters)

Heading Targets (degrees)

Control Modes

Helper Functions

Navigation Notes

Encoder Odometry

Encoder Odometry Function

Hardware Constants

Odometry Calculations

Distance Traveled

Heading Angle

Odometry Notes

Supporting Sensors

Bump Sensor

Bump Sensor Hardware

Algorithm

Bump Sensor Notes

Battery Monitor

Battery Hardware

Battery Monitor Notes

IR Sensor Array

multiple_ir_readings Class

IR Array Hardware

Timed Multi-ADC Algorithm

Motor Control Task

motor_control_task Class

State Machine

Motor Control Notes

Serial Communication

Serial Task Function

Command Protocol

Serial Hardware

State Machine Workflow

Serial Task Notes

PC User Interface

Main Function

Trial Execution Function

User Interface Workflow

UI Configuration

UI Notes

Example Session

Main Program

System Architecture

Hardware Configuration

Shared Variables

Task Timing and Priorities

Startup Sequence

Control Flow Example

Memory Management

Main Program Notes