diff --git a/autonomous/autonav.py b/autonomous/autonav.py new file mode 100644 index 00000000..093d21ba --- /dev/null +++ b/autonomous/autonav.py @@ -0,0 +1,148 @@ +import math +from typing import List + +from wpimath.geometry import Pose2d, Translation2d +from magicbot import AutonomousStateMachine, state + +from components.chassis import Chassis +from utilities.path_follow import Path, PathFollow + + +class AutoNavBase(AutonomousStateMachine): + + chassis: Chassis + + def __init__(self) -> None: + super().__init__() + self.path_num = 0 + self.paths: List[Path] + + def setup(self) -> None: + self.path_follow: PathFollow = PathFollow(self.chassis) + + def on_enable(self) -> None: + self.chassis.reset_odometry(self.paths[0].start) + self.path_num = 0 + super().on_enable() + + def done(self) -> None: + super().done() + self.chassis.disable_brake_mode() + + + @state(first=True) + def move(self, initial_call) -> None: + """ + Follow the trajectory defined by our waypoints + """ + if initial_call: + path = self.paths[self.path_num] + self.path_follow.new_path(path) + self.path_follow.run() + if self.path_follow.path_done(): + self.next_state("done_move") + + @state + def done_move(self): + self.path_num += 1 + if self.path_num >= len(self.paths): + self.done() + else: + self.next_state("move") + + +class test(AutoNavBase): + MODE_NAME = "Test" + DEFAULT = True + + def setup(self): + self.paths = [ + Path( + [ + Pose2d(0, 0, 0), + Translation2d(1, 1), + Translation2d(0, 1), + Translation2d(-1, 0), + Pose2d(0, 0, 0), + ], + reversed=False, + ) + ] + super().setup() + + +class Slalom(AutoNavBase): + MODE_NAME = "Slalom" + + def setup(self): + self.paths = [ + Path( + [ + Pose2d(1.056, 0.618, 0), + Translation2d(2.188, 0.908), + Translation2d(2.678, 2.127), + Translation2d(3.560, 2.579), + Translation2d(5.600, 2.557), + Translation2d(6.700, 2.120), + Translation2d(7.150, 0.872), + Translation2d(8.056, 0.834), + Translation2d(8.464, 1.543), + Translation2d(8.012, 2.304), + Translation2d(7.194, 2.323), + Translation2d(6.404, 0.834), + Translation2d(4.537, 0.711), + Translation2d(2.755, 0.747), + Translation2d(2.087, 1.769), + Pose2d(1.263, 2.479, math.radians(139.25)), + ], + reversed=False, + ), + ] + super().setup() + + +class Bounce(AutoNavBase): + MODE_NAME = "Bounce" + + def setup(self): + path_offset = 0.5 + self.paths = [ + Path( + [ + Pose2d(1.019, 2.139, 0), + Translation2d(1.830, 2.359), + Pose2d(2.250, 3.406, math.pi / 2), + ], + reversed=False, + ), + Path( + [ + Pose2d(2.250, 3.406, math.pi / 2), + Translation2d(2.686, 2.169), + Translation2d(3.164, 1.137), + Translation2d(3.795, 0.786), + Translation2d(4.443, 1.186), + Pose2d(4.557, 3.529 + path_offset/2, -math.pi / 2), + ], + reversed=True, + ), + Path( + [ + Pose2d(4.557, 3.529 + path_offset/3, -math.pi / 2), + Translation2d(4.668, 1.020 + path_offset), + Translation2d(5.673, 0.666 + path_offset), + Translation2d(6.800, 0.768 + path_offset), + Pose2d(6.856, 3.614 + path_offset, math.pi / 2), + ], + reversed=False, + ), + Path( + [ + Pose2d(6.856, 3.614 + path_offset, math.pi / 2), + Translation2d(7.157, 2.328 + 1.5 * path_offset), + Pose2d(8.352, 2.168 + 1.5 * path_offset, math.pi), + ], + reversed=True, + ), + ] + super().setup() diff --git a/autonomous/shoot_move_shoot.py b/autonomous/shoot_move_shoot.py index 6253bd2b..b0ca35d2 100644 --- a/autonomous/shoot_move_shoot.py +++ b/autonomous/shoot_move_shoot.py @@ -135,23 +135,6 @@ def has_fired_balls(self) -> bool: return self.shooter_controller.fired_count >= balls_to_fire -class test(ShootMoveShootBase): - MODE_NAME = "Test" - DEFAULT = True - - def setup(self): - self.start_poses = [to_pose(0, 0, math.pi)] - self.end_poses = [to_pose(-2, 0, math.pi)] - self.waypoints = [[geometry.Translation2d(-1, math.pi)]] - self.trajectory_config = trajectory.TrajectoryConfig( - maxVelocity=1, maxAcceleration=1 - ) - self.expected_balls = [0] - self.reversed = [False] - self.trajectory_max = 1 - super().setup() - - class _3Right3(ShootMoveShootBase): MODE_NAME = "3RIGHT3" diff --git a/robot.py b/robot.py index e201ccd7..21ccc87c 100755 --- a/robot.py +++ b/robot.py @@ -97,6 +97,10 @@ def autonomousInit(self) -> None: self.indexer.shimmying = False self.indexer.auto_retract = False + def disabledInit(self) -> None: + self.chassis.drive(0, 0) + super().disabledInit() + def disabledPeriodic(self) -> None: self.vision.execute() diff --git a/utilities/path_follow.py b/utilities/path_follow.py new file mode 100644 index 00000000..1e6da675 --- /dev/null +++ b/utilities/path_follow.py @@ -0,0 +1,90 @@ +from typing import List + +from wpilib import controller +from wpimath import trajectory +from wpilib import Timer +from wpimath.trajectory import constraint +from wpimath.geometry import Pose2d, Translation2d + +from components.chassis import Chassis + + +class Path: + start: Pose2d + waypoints: List[Translation2d] + end: Pose2d + reversed: bool + + def __init__(self, points, reversed) -> None: + self.start, *self.waypoints, self.end = points + self.reversed = reversed + + +class PathFollow: + """ + A class to follow a given class, run must be called in each loop where + an output is wanted + """ + + def __init__(self, chassis: Chassis) -> None: + self.chassis = chassis + self.controller = controller.RamseteController() + self.trajectory_config = trajectory.TrajectoryConfig( + maxVelocity=2.5, maxAcceleration=1.5 + ) + self.gen = trajectory.TrajectoryGenerator() + self.trajectory_config.setKinematics(self.chassis.kinematics) + self.trajectory_config.addConstraint( + constraint.DifferentialDriveVoltageConstraint( + self.chassis.ff_calculator, self.chassis.kinematics, maxVoltage=10 + ) + ) + self.trajectory_config.addConstraint(constraint.CentripetalAccelerationConstraint(maxCentripetalAcceleration=2.5)) + self.trajectory: trajectory.Trajectory + self.start_time: float + + def new_path(self, path: Path) -> None: + """ + Give the path follower a new path, it will abandon the current one and + follow it instead + """ + self.trajectory_config.setReversed(path.reversed) + self.trajectory = self.gen.generateTrajectory( + path.start, + path.waypoints, + path.end, + self.trajectory_config, + ) + self.start_time = Timer.getFPGATimestamp() + + def new_quintic_path(self, waypoints: List[Pose2d], reversed: bool): + """ + Give the path follower a new path, it will abandon the current one and + follow it instead. This method specifies the heading at each waypoint. + """ + self.trajectory_config.setReversed(reversed) + self.trajectory = self.gen.generateTrajectory( + waypoints, + self.trajectory_config, + ) + self.start_time = Timer.getFPGATimestamp() + + def run(self) -> None: + """ + Send the chassis control inputs for this loop + """ + if self.path_done(): + self.chassis.drive(0, 0) + else: + pos = self.chassis.get_pose() + goal = self.trajectory.sample(self.current_path_time) + speeds = self.controller.calculate(pos, goal) + self.chassis.drive(speeds.vx, speeds.omega) + + def path_done(self) -> bool: + """ + Check to see if enough time has passed to complete the path + """ + # TODO investigate closing the loop here + self.current_path_time = Timer.getFPGATimestamp() - self.start_time + return self.current_path_time > self.trajectory.totalTime()