A MicroPython motion-control library for a differential-drive robot built on the LEGO SPIKE Prime hub, using Pybricks. Hyperlib provides odometry, PID-based control, and a full suite of movement functions for autonomous navigation.
Odometry is not magic; it cannot pinpoint exactly where your robot tracks every single minute movement of both wheels. It is just an approximation, and the error keeps increasing as the robot travels. Therefore, odometry in the World Robot Olympiad program is just a method to cut corners, to travel to approximate positions. If you want exact positions, you must plan waypoints (walls, black line to follow, colors on the map,...) which the robot can track using other kinds of sensors
These functions can NEVER make the robot arrive at the desired position/heading with 100% accuracy. The idea behind these functions is that the robot keeps steering to its target to MINIMIZE the error between the robot's and the desired position/heading, not completely EFFACING it. Thus, if you want the robot to be 100% on the target, it will run forever and can never complete this task. Instead, as the robot approaches an acceptable margin of error, it will stop. But don't worry! Since odometry tracking is always on, the robot will ALWAYS be able to approximate where it is and where it isn't.
WE ENCOURAGE YOU TO ADD FUNCTIONS OF YOUR OWN TO THE PROGRAM! Many functions may be essential to you that we have not built yet! So, don't hesitate to study the method and programming style used to build these functions to construct your own! Also, you must firmly understand the MATHEMATICAL PROOF and LOGIC under the algorithms and functions to build your own programmes. Contact the author (Lam) if you have any question to ask!
├── Versions/
│ ├── Hyperlib_V1_4/
│ ├── Hyperlib_V1_5/
│ ├── Hyperlib_V1_6/
│ └── Hyperlib_V1_7/ ← Current version
│ ├── Hyperlib_v1_7.py ← Main library
│ ├── main_program.py ← Your program goes here
│ └── LineCalibration.py ← Run once to find your LineBlack / LineWhite values
├── Hyperlib V1.3.py
└── README.md
- Place
Hyperlib_v1_7.pyandmain_program.pyin the same directory on your SPIKE Prime hub. - If you plan to use line following, run
LineCalibration.pywith the sensor on black and on white to get yourLineBlack/LineWhitevalues, then set them at the top ofHyperlib_v1_7.py. - Write your autonomous routine inside
main_program.py:
from Hyperlib_v1_7 import *
import Hyperlib_v1_7 as lib
async def my_mission():
hub.imu.ready()
await OdomIni(0, 0, 0) # Start at origin, facing 0°
await TurnToPoint(-300, 300) # Face target coordinate
await MoveToPoint(-300, 300) # Drive to it
lib.PArray = [[0,0],[200,300],[400,0]]
await PurePursuit() # Follow the path
run_task(my_mission())Odometry no longer needs its own multitask coroutine — every movement function updates (odomX, odomY) inline on each control-loop tick whenever OdometrySwitch is True (the default).
| Parameter | Value | Description |
|---|---|---|
| Left Motor | Port A | Reversed (CounterClockwise) |
| Right Motor | Port E | Standard |
WheelD |
based on your robot | Wheel diameter |
Axle |
based on your robot | Distance between wheels |
LoopTime |
10 ms | Control loop update frequency |
MinSpeed |
input your motors' min dps (°/s) | Minimum motor speed |
MaxSpeed |
input your motors' max dps (°/s) | Maximum motor speed |
MaxAcc |
input your motors' max acceleration (°/s²) | Maximum acceleration |
| Constant | Value | Purpose |
|---|---|---|
GKp |
50 | Proportional gain for gyro heading correction |
GKd |
780 | Derivative gain for gyro heading correction |
ENCKp |
1 | Proportional gain for encoder straightening |
ENCKd |
10 | Derivative gain for encoder straightening |
EncTurnKp |
1 | Proportional gain for in-place turn drift correction |
EncTurnKd |
0 | Derivative gain for in-place turn drift correction |
DistanceKp |
3 | Proportional gain for distance-based speed |
GyroSingleTurnKp |
20 | Gain for gyro-based swing turns |
ENCSingleTurnKp |
12 | Gain for encoder-based swing turns |
DoubleTurnKp |
10 | Gain for two-motor point turns |
Set OdometrySwitch = False to disable the automatic OdomUpdate() call inside every movement function.
| Constant | Value | Purpose |
|---|---|---|
LineKpLow |
180 | Proportional gain while crawling (accel/decel ramps) |
LineKpHigh |
200 | Proportional gain at cruise speed |
LineKdLow |
180 | Derivative gain while crawling |
LineKdHigh |
200 | Derivative gain at cruise speed |
LineBlack |
210 | Raw sensor reading on black (calibrate per field) |
LineWhite |
1020 | Raw sensor reading on white (calibrate per field) |
LineMid |
0.5 | Default brightness target (the black/white edge) |
FollowLineMM blends LineKpLow/LineKdLow toward LineKpHigh/LineKdHigh as speed ramps up. Calibrate LineBlack and LineWhite for your field/lighting with LineCalibration.py.
Initializes the robot's position and heading. Resets the IMU to theta, sets coordinates to (x, y), and zeroes motor encoders.
Updates (odomX, odomY) using arc odometry — accurate for both straight paths and curves. Uses wheel encoder deltas combined with IMU heading. Returns (odomX, odomY, current_theta).
A plain (non-async) function, called inline from every movement function on each control-loop tick whenever OdometrySwitch is True — see Odometry Updates below.
Moves straight for a set distance (mm) using only wheel encoders for straightness correction (PD-controller on left/right encoder difference). No gyro required.
Moves straight for a set distance (mm) while maintaining a target heading using the IMU gyro (PD-controller on heading error).
Point turn using both motors to reach a target heading via the IMU.
Theta— target heading in degreesEarlyExit— acceptable heading error to stop (default5°)Forward— face forward (True) or backward (False)Direction—0= shortest path,1= force clockwise,2= force counterclockwise
Point turn using both motors, controlled purely by wheel encoders. Each wheel travels turn_mm in opposite directions. Positive = clockwise.
Turns in place to face a target coordinate (TargetX, TargetY) using odometry and the IMU.
Pivot turn using one motor while the other is held, controlled by the IMU gyro.
Side = 1→ left motor moves, right heldSide≠ 1 → right motor moves, left heldTheta— target heading in degrees
Pivot turn using one motor while the other is held, controlled by the moving wheel's encoder.
turn_mm— distance the moving wheel travels (mm). Positive = clockwise.
Swing turn to face a specific field coordinate using one motor and the IMU gyro.
Reads the color sensor's white channel and rescales it to a 0.0 (black) – 1.0 (white) brightness using LineBlack / LineWhite.
Follows the black/white edge of a line for a set distance (mm), steering with a speed-scheduled PD controller (LineKpLow/LineKdLow while crawling, blending to LineKpHigh/LineKdHigh at cruise speed). Flip EdgeSign if the robot steers away from the line instead of onto it.
Drives straight at a constant speed, holding heading Theta with the gyro, until the sensor's brightness crosses Threshold. Under=True stops on a dark line, Under=False stops on a light area.
Drives to a target coordinate (targetX, targetY) using a P-controller for distance-based speed and a PD-controller for heading correction. Stops within ArrivalThreshold mm of the target.
MoveToPoRo(position + orientation docking) from v1.6 is not part of v1.7 — see v1.6 → v1.7 Changes.
Follows a sequence of waypoints stored in PArray using the Pure Pursuit algorithm. The robot continuously steers toward a "lookahead point" — a point on the path a fixed distance ahead of the robot — instead of aiming directly at each waypoint. This produces smooth, curved trajectories through all the waypoints.
Before calling PurePursuit, set PArray to a list of [x, y] waypoints (in order):
import Hyperlib_v1_7 as lib
lib.PArray = [[0, 0], [200, 300], [400, 100], [600, 0]]
await PurePursuit(MaxSpeed=900)| Parameter | Default | Description |
|---|---|---|
MaxSpeed |
900 | Maximum motor speed (dps) |
MinSpeed |
250 | Minimum motor speed for final approach (dps) |
ArrivalThreshold |
25 mm | Distance to waypoint considered "reached" |
Lookahead |
50 mm | Lookahead radius — larger = smoother but less precise |
Forward |
True |
True = drive forward, False = drive backward |
Stopping |
True |
Whether to brake after reaching the final waypoint |
How it works:
The robot treats PArray as a path made of connected line segments. Each control loop iteration:
- For the current segment, it finds where a circle of radius
Lookaheadcentered on the robot intersects the segment. - It steers toward that intersection point ("the lookahead point"), not the waypoint itself.
- When the robot gets within
ArrivalThresholdof the next waypoint, it advances to the next segment. - Once all segments are traversed, it calls
MoveToPointto precisely arrive at the final waypoint.
This gives the robot smooth, continuous motion through a path — sharp corners are rounded naturally by the lookahead radius.
In v1.6, odometry ran as a separate coroutine composed with multitask. That relied on await OdomUpdate() — but OdomUpdate had been declared async def while every movement function called it as plain OdomUpdate() (no await), so the coroutine object was created and immediately discarded and position tracking never actually advanced.
v1.7 fixes this by removing the background task entirely. OdomUpdate() is now a plain function, and every movement function calls it directly at the top of its control loop, guarded by the OdometrySwitch flag:
def OdomUpdate():
... # plain function, not async
async def MoveMM(...):
while True:
if OdometrySwitch:
OdomUpdate()
...There is no odometry_task() or multitask() call needed anymore — just call run_task(my_mission()) directly. Set OdometrySwitch = False if you want to skip the per-loop odometry update (e.g. to shave a little time off the control loop when you don't need position tracking for a given move).
Brake both motors for 40 ms, then hold them in place.
Converts motor encoder degrees to millimeters of travel using the configured wheel diameter.
Returns the signed heading error in degrees (range −180° to 180°) between the current IMU heading and a target angle.
⚠️ First run: Before running any program, update your robot's physical specs at the top ofHyperlib_v1_7.pyto match your actual hardware:Incorrect values will cause all distance and odometry calculations to be wrong.WheelD = 62 # ← your wheel diameter in mm Axle = 170 # ← your axle width in mm (wheel center to wheel center)
- All movement functions are
asyncand must be called withawait. There is nomultitask()/background odometry task to set up — callrun_task(my_mission())directly (see Quick Start). PurePursuit,MoveToPoint,TurnToPoint, andSwingToPointall rely on odometry, so make sureOdometrySwitchisTrue(the default) whenever you use them.- Line following (
FollowLineMM,FollowGyroToLine) needsLineBlack/LineWhitecalibrated for your field — runLineCalibration.pyfirst. - The Bluetooth button is configured as the stop button to prevent accidental program termination during a run.
- Hyperlib uses arc odometry in
OdomUpdatefor accurate position tracking on both straight and curved paths.
| Version | File | Notes |
|---|---|---|
| v1.7 | Hyperlib_v1_7.py |
Line following (ReadLine, FollowLineMM, FollowGyroToLine), fixed OdomUpdate (no longer relies on multitask), fixed TurnGyro/TurnToPoint/SwingGyro bugs |
| v1.6 | Hyperlib_v1_6.py |
Added Pure Pursuit path following, concurrent odometry via multitask |
| v1.5 | Hyperlib_v1_5.py |
Added arc odometry, encoder-based turns (TurnMM, SwingMM), improved PID gains |
| v1.4 | Hyperlib_V1_4/ |
Previous stable version |
| v1.3 | Hyperlib V1.3.py |
Initial release |
Critical fix: odometry actually runs now
- In v1.6,
OdomUpdatewas declaredasync def, but every movement function called it asOdomUpdate()withoutawait. That silently created a coroutine object and threw it away — the function body never executed, so(odomX, odomY)never updated no matter how the mission called it. - v1.7 makes
OdomUpdatea plain (non-async) function and calls it directly from inside every movement function's loop, gated byOdometrySwitch. Theodometry_task()/multitask()pattern from v1.6 is no longer needed — justrun_task(my_mission()). SwingGyro's right-motor-moves branch was also missing itsOdomUpdate()call entirely; it now updates odometry on both branches.
Line following (ReadLine, FollowLineMM, FollowGyroToLine)
- New color-sensor-based steering, using a
PUPDeviceon Port C read in RGBW mode (channel 3 = white/brightness). ReadLine()rescales the raw reading to0.0–1.0using theLineBlack/LineWhitecalibration constants — calibrate these per field/lighting with the newLineCalibration.pyscript.FollowLineMMdrives a set distance while PD-steering to hold a target brightness (LineMidby default), with gains scheduled betweenLineKpLow/LineKdLow(crawling) andLineKpHigh/LineKdHigh(cruise speed).FollowGyroToLinedrives at a constant speed, holding heading with the gyro, until the sensor crosses a brightness threshold — useful for approaching and squaring off against a line.
Turn heading-wrap bug fix (TurnGyro, TurnToPoint)
- Both functions previously computed
abs(ThetaErr(Theta) + OneEighty)— adding the 180° "face backward" offset after wrapping into [-180°, 180°]. WithForward=Falsethis could produce an error that never dropped belowEarlyExit, so the turn never finished. - Fixed by moving the offset inside
ThetaErr(ThetaErr(Theta + OneEighty)), so wrapping happens after the offset is applied.
Removed MoveToPoRo
- The v1.6 position + orientation docking function is not present in v1.7.
Pure Pursuit (PurePursuit)
- New function for following multi-waypoint paths smoothly. Replaces chaining multiple
MoveToPointcalls when you need the robot to travel through a sequence of coordinates without stopping at each one. - Uses a lookahead circle to find the next steering target on the path, producing naturally curved trajectories.
- Path is defined by setting
PArray(a list of[x, y]waypoints) before callingPurePursuit.
Concurrent Odometry via multitask
- Odometry is now separated into its own
odometry_task()coroutine that runs concurrently with the mission code using Pybricks'multitask. - In v1.5, odometry was called inside each movement function. In v1.6, it runs continuously in the background, so position is always current regardless of which movement is executing.
- The new program structure splits
main()intomy_mission()+odometry_task()composed withmultitask.
Odometry is a method to estimate the robot's position. There are two main odometry methods: linear odometry and arc odometry. During straight movements, linear odometry is best suited for use, whereas during curved moment, arc odometry is the optimal method
Here is the mathematical proof of it, you can try to look for symmetries in these graph and the code to better understand odometry:
Pure Pursuit is a path-tracking algorithm. Instead of driving directly toward each waypoint and stopping, the robot steers toward a lookahead point — a point on the path a fixed lookahead distance L ahead of the robot. This produces smooth, continuous curves through the entire path.
Here is the mathematical proof of it, you can try to look for symmetries in this graph and the code to better understand Pure Pursuit:
Mr. Phong and his colleagues have been working with us since day one of robotics, and I want to dedicate a section of this library to honor their contributions to building our knowledge and curiosity, which have allowed us to build this library.
👤author: @Lamcao2109 👥contributors: @DuCpHancm, @haing2811