drive_utils.hpp File Reference

Header-only drive helpers built on top of light::Drive: More...

#include <algorithm>
#include <cmath>
#include "LightLib/drive/drive.hpp"
#include "LightLib/path/field_map.hpp"
#include "LightLib/drive/odometry.hpp"
#include "LightLib/util/util.hpp"
#include "okapi/api/units/QLength.hpp"
#include "pros/distance.hpp"
#include "subsystems.hpp"

Go to the source code of this file.

Functions
void	drive_until_distance (okapi::QLength target, pros::Distance &sensor, int speed=127, int timeout_ms=10000)
	Drive forward (or reverse) until a distance sensor reads at or below target.
void	distance_reset (light::Drive &chassis, pros::Distance &sensor, double offset_fwd, double offset_side, bool fix_x, bool faces_positive=false, double field_size_in=light::field::FIELD_SIZE_IN)
	Re-zero one odom axis (x or y) using a wall-facing distance sensor.
void	drive_distance_reset (light::Drive &chassis, double known_coord, bool fix_x)
	Re-zero one odom axis from a known field coordinate (e.g.
void	angle_reset (light::Drive &chassis, pros::Distance &front_sensor, pros::Distance &rear_sensor, double separation)
	Correct heading from front- and rear-mounted distance sensors looking at the same parallel wall.

Detailed Description

Header-only drive helpers built on top of light::Drive:

• drive_until_distance(): drive until a distance sensor reads under a threshold.
• distance_reset(): re-zero one odom axis using a wall-facing sensor.
• drive_distance_reset(): re-zero one odom axis from a known coordinate.
• angle_reset(): correct heading from front+rear distance sensors on a parallel wall.

Definition in file drive_utils.hpp.

Function Documentation

angle_reset()

void angle_reset ( light::Drive &  chassis, pros::Distance &  front_sensor, pros::Distance &  rear_sensor, double  separation  )

inline

Correct heading from front- and rear-mounted distance sensors looking at the same parallel wall.

The angular offset is atan2(rear − front, separation) and is applied to the IMU's stored offset.

Parameters

chassis	the drivetrain (IMU is reset on it)
front_sensor	forward-mounted distance sensor
rear_sensor	rear-mounted distance sensor on the same side
separation	distance between the two sensors along the robot, inches

Definition at line 154 of file drive_utils.hpp.

distance_reset()

void distance_reset ( light::Drive &  chassis, pros::Distance &  sensor, double  offset_fwd, double  offset_side, bool  fix_x, bool  faces_positive = false, double  field_size_in = light::field::FIELD_SIZE_IN  )

inline

Re-zero one odom axis (x or y) using a wall-facing distance sensor.

Reads the sensor, projects its measurement through the current heading to the axis of interest, and updates the odom pose with light::setPose. Bails silently if the sensor returns PROS_ERR.

Parameters

chassis	the drivetrain (used for IMU heading)
sensor	wall-facing distance sensor
offset_fwd	sensor offset in inches along the robot's forward axis
offset_side	sensor offset in inches along the robot's lateral axis
fix_x	true = correct X, false = correct Y
faces_positive	true if the sensor faces the positive end of the field axis
field_size_in	field size used to flip the coordinate when faces_positive

Definition at line 86 of file drive_utils.hpp.

drive_distance_reset()

void drive_distance_reset ( light::Drive &  chassis, double  known_coord, bool  fix_x  )

inline

Re-zero one odom axis from a known field coordinate (e.g.

when robot is pinned against a wall whose coordinate is known a priori).

Parameters

chassis	the drivetrain (used for IMU heading)
known_coord	the known field-frame coordinate, inches
fix_x	true = set X, false = set Y

Definition at line 126 of file drive_utils.hpp.

drive_until_distance()

void drive_until_distance ( okapi::QLength  target, pros::Distance &  sensor, int  speed = 127, int  timeout_ms = 10000  )

inline

Drive forward (or reverse) until a distance sensor reads at or below target.

The PID is given an arbitrarily large 5000 in target — the sensor triggers the real stop. Falls back to pid_wait_quick_chain() on timeout.

Parameters

target	distance threshold (with units)
sensor	distance sensor used for the stop trigger
speed	signed drive speed, ±127. Negative = reverse.
timeout_ms	bail after this many ms if the threshold is never crossed

Definition at line 41 of file drive_utils.hpp.