taolib

#include "taolib/taolib.h"

// Create a drivetrain instance with some motors and an inertial sensor.
// You can also use external encoders if you wish!
tao::DifferentialDrivetrain chassis(left_drive, right_drive, IMU, {
	.drive_gains = { 4.24, 0, 0.06 },
	.turn_gains = { 0.82, 0.003, 0.0875 },
	.drive_tolerance = 0.7,
	.turn_tolerance = 1.4,
	.lookahead_distance = 8.5,
	.track_width = 13.75,
	.wheel_diameter = 2.02,
	.gearing = (84.0 / 60.0),
});

void autonomous() {
	// Start the tracking period at (0, 0) facing 90 degrees
	chassis.start_tracking();

	// Move the robot to (25, 25) using odometry for targetting
	chassis.move_to(tao::Vector2(25, 25));
}