Pathplanning and autonomous (#27)

* use pathplannerlib with markers

* in lab paths

* add requirement

* documentation, indexed commands in marker map

* flip based on alliance

* log auto, proper fliparound

* ok actually proper fliparound with respect to world coords

* more logging in autos

* rotate controls based off of which alliance side

* log auto error

* more logging

* new auton paths with waypoints

* wrap commands to prevent errors

* deal with conflicting commands

.pathplanner/settings.json

@@ -0,0 +1,7 @@
+{
+  "robotWidth": 0.83,
+  "robotLength": 0.78,
+  "holonomicMode": true,
+  "generateJSON": false,
+  "generateCSV": false
+}

commands/auto/autoeventnames.md

@@ -0,0 +1,15 @@
+# Autonomous Event Naming
+
+### because markers need names
+
+#### namings that become actual positions
+
+| Name    | Action                                                                                  |
+| ------- | --------------------------------------------------------------------------------------- |
+| store   | stores the arm inwards                                                                  |
+| top     | puts the arm into the TOP score position                                                |
+| mid     | puts the arm into the MID score position                                                |
+| hybrid  | puts the arm into the HYBRID score position                                             |
+| engage  | auto balance and engage with the charging station (continuous, command does not finish) |
+| intake  | runs the collection mechanism to intake a gamepiece                                     |
+| outtake | runs the collection mechanism to outtake a gamepiece                                    |

commands/auto/autohelper.py

@@ -0,0 +1,14 @@
+from typing import List
+from pathplannerlib import PathPlanner, PathPlannerTrajectory
+from wpilib import DataLogManager
+
+import constants
+
+
+def trajectoryFromFile(name: str) -> List[PathPlannerTrajectory]:
+    DataLogManager.log(f"Loading path {name}")
+    return PathPlanner.loadPathGroup(
+        name,
+        constants.kMaxForwardLinearVelocity,
+        constants.kMaxForwardLinearAcceleration,
+    )

commands/auto/autonomousaction.py

@@ -0,0 +1,117 @@
+from typing import Dict, List
+from commands2 import (
+    Command,
+    CommandBase,
+    FunctionalCommand,
+    ParallelCommandGroup,
+    ParallelDeadlineGroup,
+    SequentialCommandGroup,
+    WaitCommand,
+)
+from pathplannerlib import PathPlannerTrajectory
+from wpilib import DataLogManager
+
+from commands.auto.autohelper import trajectoryFromFile
+from commands.auto.followtrajectory import FollowTrajectory
+from commands.resetdrive import ResetDrive
+from subsystems.drivesubsystem import DriveSubsystem
+
+
+class AutonomousRoutine(SequentialCommandGroup):
+    markerMap: Dict[str, Command]
+
+    def __init__(
+        self,
+        drive: DriveSubsystem,
+        name: str,
+        simultaneousCommands: List[Command],
+    ):
+        self.name = name
+        self.markerMap = {  # later todo: actual implementation
+            "store": WaitCommand(2),
+            "top": WaitCommand(2),
+            "mid": WaitCommand(2),
+            "hybrid": WaitCommand(2),
+            "engage": WaitCommand(5),
+            "intake": WaitCommand(0.25),
+            "outtake": WaitCommand(0.25),
+        }
+        self.paths = trajectoryFromFile(name)
+        followCommands = [
+            SequentialCommandGroup(
+                self.stopEventGroup(path.getStartStopEvent()),
+                FollowTrajectory(drive, path, path.getMarkers(), self.markerMap),
+                self.stopEventGroup(path.getEndStopEvent()),
+            )
+            for path in self.paths
+        ]
+
+        super().__init__(
+            ResetDrive(drive),
+            ParallelCommandGroup(
+                SequentialCommandGroup(*followCommands), *simultaneousCommands
+            ),
+        )
+        self.setName(name)
+
+    def execute(self) -> None:
+        DataLogManager.log(f"Starting auto: {self.name}")
+        return super().execute()
+
+    def wrappedEventCommand(self, eventCommand: Command) -> CommandBase:
+        return FunctionalCommand(
+            eventCommand.initialize,
+            eventCommand.execute,
+            eventCommand.end,
+            eventCommand.isFinished,
+            list(eventCommand.getRequirements()),
+        )
+
+    def getStopEventCommands(
+        self, stopEvent: PathPlannerTrajectory.StopEvent
+    ) -> Command:
+        commands = [
+            self.wrappedEventCommand(self.markerMap.get(name))
+            for name in stopEvent.names
+            if name in self.markerMap
+        ]
+        if (
+            stopEvent.executionBehavior
+            == PathPlannerTrajectory.StopEvent.ExecutionBehavior.PARALLEL
+        ):
+            return ParallelCommandGroup(commands)
+        elif (
+            stopEvent.executionBehavior
+            == PathPlannerTrajectory.StopEvent.ExecutionBehavior.SEQUENTIAL
+        ):
+            return SequentialCommandGroup(commands)
+        else:
+            raise NotImplementedError("That execution behaviour is not implemented")
+
+    def stopEventGroup(self, stopEvent: PathPlannerTrajectory.StopEvent) -> Command:
+        eventCommands = self.getStopEventCommands(stopEvent)
+        if (
+            stopEvent.waitBehavior
+            == PathPlannerTrajectory.StopEvent.WaitBehavior.BEFORE
+        ):
+            return SequentialCommandGroup(
+                WaitCommand(stopEvent.waitTime), eventCommands
+            )
+        elif (
+            stopEvent.waitBehavior == PathPlannerTrajectory.StopEvent.WaitBehavior.AFTER
+        ):
+            return SequentialCommandGroup(
+                eventCommands, WaitCommand(stopEvent.waitTime)
+            )
+        elif (
+            stopEvent.waitBehavior
+            == PathPlannerTrajectory.StopEvent.WaitBehavior.DEADLINE
+        ):
+            return ParallelDeadlineGroup(WaitCommand(stopEvent.waitTime), eventCommands)
+        elif (
+            stopEvent.waitBehavior
+            == PathPlannerTrajectory.StopEvent.WaitBehavior.MINIMUM
+        ):
+            return ParallelCommandGroup(WaitCommand(stopEvent.waitTime), eventCommands)
+        else:
+            return eventCommands

commands/auto/followtrajectory.py

@@ -0,0 +1,213 @@
+from math import pi
+import functools
+import operator
+from typing import Dict, List, Tuple
+from commands2 import Command, CommandBase
+from pathplannerlib import PathPlannerTrajectory
+from wpilib import DataLogManager, DriverStation, SmartDashboard, Timer
+from wpimath.controller import (
+    PIDController,
+    ProfiledPIDControllerRadians,
+    HolonomicDriveController,
+)
+from wpimath.geometry import Pose2d, Rotation2d
+from wpimath.trajectory import TrapezoidProfileRadians
+
+from subsystems.drivesubsystem import DriveSubsystem
+import constants
+
+
+class FollowTrajectory(CommandBase):
+    unpassedMarkers: List[PathPlannerTrajectory.EventMarker]
+
+    def __init__(
+        self,
+        drive: DriveSubsystem,
+        trajectory: PathPlannerTrajectory,
+        markers: List[PathPlannerTrajectory.EventMarker],
+        markerMap: Dict[str, Command],
+    ) -> None:
+        CommandBase.__init__(self)
+
+        self.drive = drive
+        self.markers = markers
+        self.markerMap = markerMap
+
+        self.currentCommands: List[Tuple[Command, bool]] = []
+
+        for marker in self.markers:
+            for markerName in marker.names:
+                if markerName in self.markerMap.keys():
+                    reqs = self.markerMap[markerName].getRequirements()
+                    self.addRequirements(
+                        reqs
+                    )  # make sure we require everything our sub commands require
+
+        self.setControllers()
+
+        self.originTrajectory = trajectory
+        self.trajectory = trajectory
+
+        self.timer = Timer()
+
+        self.addRequirements([self.drive])
+        self.setName(__class__.__name__)
+
+    def setControllers(self) -> None:
+        self.xController = PIDController(
+            constants.kTrajectoryPositionPGain,
+            constants.kTrajectoryPositionIGain,
+            constants.kTrajectoryPositionDGain,
+        )
+        self.yController = PIDController(
+            constants.kTrajectoryPositionPGain,
+            constants.kTrajectoryPositionIGain,
+            constants.kTrajectoryPositionDGain,
+        )
+        self.thetaController = ProfiledPIDControllerRadians(
+            constants.kTrajectoryAnglePGain,
+            constants.kTrajectoryAngleIGain,
+            constants.kTrajectoryAngleDGain,
+            TrapezoidProfileRadians.Constraints(
+                constants.kMaxRotationAngularVelocity,
+                constants.kMaxRotationAngularAcceleration,
+            ),
+        )
+        self.thetaController.enableContinuousInput(-pi, pi)
+
+        self.controller = HolonomicDriveController(
+            self.xController, self.yController, self.thetaController
+        )
+
+    def initialize(self):
+        self.timer.reset()
+        self.timer.start()
+
+        self.unpassedMarkers = []
+        self.unpassedMarkers.extend(self.markers)
+
+        initialState = self.trajectory.getInitialState()
+        allianceRespectiveStartingPoint = self.getAllianceRespectivePoint(
+            initialState.pose, initialState.holonomicRotation
+        )
+        self.drive.resetOdometryAtPosition(
+            Pose2d(
+                allianceRespectiveStartingPoint[0].translation(),
+                allianceRespectiveStartingPoint[1],
+            ),
+        )
+
+        self.setControllers()
+
+        transformedStates = [
+            self.getAllianceRespectivePoint(state.pose, state.pose.rotation())
+            for state in self.originTrajectory.getStates()
+        ]
+
+        SmartDashboard.putNumberArray(
+            constants.kAutonomousPathKey,
+            functools.reduce(
+                operator.add,
+                [
+                    [state[0].X(), state[0].Y(), state[1].radians()]
+                    for state in transformedStates
+                ],
+                [],
+            ),
+        )
+        DataLogManager.log("begin trajectory")
+
+    def getAllianceRespectivePoint(
+        self, pose: Pose2d, holonomicRotation: Rotation2d
+    ) -> Tuple[Pose2d, Rotation2d]:
+        if DriverStation.getAlliance() == DriverStation.Alliance.kRed:
+            return (
+                Pose2d(
+                    constants.kFieldLength - pose.X(),
+                    pose.Y(),
+                    -pose.rotation() + Rotation2d(pi),
+                ),
+                -holonomicRotation + Rotation2d(pi),
+            )
+        else:
+            return (pose, holonomicRotation)
+
+    def execute(self) -> None:
+        for command, running in self.currentCommands:
+            if not running:
+                continue
+            command.execute()
+
+            if command.isFinished():
+                command.end(False)
+                running = False
+
+        curTime = self.timer.get()
+        currentState = self.drive.getPose()
+        desiredState = self.trajectory.sample(curTime)
+
+        allianceRespectiveDesiredState = self.getAllianceRespectivePoint(
+            desiredState.pose, desiredState.holonomicRotation
+        )
+
+        SmartDashboard.putNumberArray(
+            constants.kAutonomousPathError,
+            [
+                currentState.X() - allianceRespectiveDesiredState[0].X(),
+                currentState.Y() - allianceRespectiveDesiredState[0].Y(),
+                (currentState.rotation() - allianceRespectiveDesiredState[1]).radians(),
+            ],
+        )
+        SmartDashboard.putNumberArray(
+            constants.kAutonomousPathSample,
+            [
+                allianceRespectiveDesiredState[0].X(),
+                allianceRespectiveDesiredState[0].Y(),
+                allianceRespectiveDesiredState[1].radians(),
+            ],
+        )
+
+        targetChassisSpeeds = self.controller.calculate(
+            currentState,
+            allianceRespectiveDesiredState[0],
+            desiredState.velocity,
+            allianceRespectiveDesiredState[1],
+        )
+
+        SmartDashboard.putNumberArray(
+            constants.kAutonomousChassisSpeeds,
+            [targetChassisSpeeds.vx, targetChassisSpeeds.vy, targetChassisSpeeds.omega],
+        )
+
+        self.drive.arcadeDriveWithSpeeds(
+            targetChassisSpeeds, DriveSubsystem.CoordinateMode.RobotRelative
+        )
+
+        # marker relatev stuff
+        if len(self.unpassedMarkers) > 0 and curTime >= self.unpassedMarkers[0].time:
+            marker = self.unpassedMarkers.pop(0)
+            for name in marker.names:
+                if name in self.markerMap:
+                    eventCommand = self.markerMap[name]
+                    eventCommand.initialize()
+                    self.currentCommands.append((eventCommand, True))
+
+    def isFinished(self) -> bool:
+        return self.timer.hasElapsed(self.trajectory.getTotalTime())
+
+    def getFinalPosition(self) -> Pose2d:
+        endState = self.trajectory.getEndState()
+        finalAllianceRespectivePose = self.getAllianceRespectivePoint(
+            endState.pose, endState.holonomicRotation
+        )
+        return Pose2d(
+            finalAllianceRespectivePose[0].translation(),
+            finalAllianceRespectivePose[1],
+        )
+
+    def end(self, _interrupted: bool) -> None:
+        self.drive.resetOdometryAtPosition(self.getFinalPosition())
+        self.drive.arcadeDriveWithFactors(
+            0, 0, 0, DriveSubsystem.CoordinateMode.RobotRelative
+        )
+        DataLogManager.log("end trajectory")