diff --git a/Earthfile b/Earthfile new file mode 100644 index 00000000..d83446f9 --- /dev/null +++ b/Earthfile @@ -0,0 +1,37 @@ +VERSION 0.7 +FROM debian:bookworm-20240110 +RUN apt-get update -y +WORKDIR /RobotCode2024 + +javabase: + RUN apt-get install -y openjdk-17-jdk-headless git + +ccbase: + RUN apt-get install -y build-essential git cmake clang lld + +ccbase-mingw-w64: + FROM +ccbase + RUN apt-get install -y mingw-w64 + +osxcross: + FROM crazymax/osxcross:13.1-r0-debian + SAVE ARTIFACT /osxcross + +robotcode2024-dependencies: + CACHE /root/.gradle + FROM +javabase + COPY gradle/ gradle/ + COPY settings.gradle settings.gradle + COPY build.gradle build.gradle + COPY .wpilib .wpilib + COPY vendordeps vendordeps + COPY gradlew gradlew + RUN ./gradlew build --no-daemon + SAVE IMAGE --cache-hint + +robotcode2024-build: + CACHE /root/.gradle + FROM +robotcode2024-dependencies + COPY src src + RUN ./gradlew build --no-daemon + SAVE IMAGE --cache-hint \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44161065..fcf0d9a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; import frc.robot.subsystems.apriltagvision.AprilTagVision; @@ -56,7 +55,7 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state as field + // Load robot state private final RobotState robotState = RobotState.getInstance(); // Subsystems @@ -85,12 +84,14 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]), + false); + // aprilTagVision = + // new AprilTagVision( + // new + // AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + // shooter = new Shooter(new ShooterIOSparkMax()); + // intake = new Intake(new IntakeIOSparkMax()); arm = new Arm(new ArmIOKrakenFOC()); } } @@ -103,7 +104,8 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.moduleConfigs[0]), new ModuleIOSim(DriveConstants.moduleConfigs[1]), new ModuleIOSim(DriveConstants.moduleConfigs[2]), - new ModuleIOSim(DriveConstants.moduleConfigs[3])); + new ModuleIOSim(DriveConstants.moduleConfigs[3]), + false); shooter = new Shooter(new ShooterIOSim()); intake = new Intake(new IntakeIOSim()); arm = new Arm(new ArmIOSim()); @@ -116,7 +118,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, - new ModuleIO() {}); + new ModuleIO() {}, + false); shooter = new Shooter(new ShooterIO() {}); intake = new Intake(new IntakeIO() {}); arm = new Arm(new ArmIO() {}); @@ -130,7 +133,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, - new ModuleIO() {}); + new ModuleIO() {}, + false); } if (aprilTagVision == null) { @@ -167,10 +171,7 @@ public RobotContainer() { new FeedForwardCharacterization( shooter, shooter::runRightCharacterizationVolts, - shooter::getRightCharacterizationVelocity)); - autoChooser.addOption( - "Arm Quasistatic Forward", arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption("Arm Dynamic Forward", arm.sysIdDynamic(SysIdRoutine.Direction.kForward)); + shooter::getRightCharacterizationVelocity));\ autoChooser.addOption("Arm get static current", arm.getStaticCurrent()); // Testing autos paths @@ -181,7 +182,7 @@ public RobotContainer() { traj -> Commands.runOnce( () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(drive.setTrajectoryCommand(traj))); + .andThen(drive.followTrajectory(traj))); }; final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { @@ -226,6 +227,16 @@ private void configureButtonBindings() { robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) .ignoringDisable(true)); controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); + controller + .a() + .whileTrue( + drive.autoAlignToPose( + () -> + AllianceFlipUtil.apply( + new Pose2d( + FieldConstants.ampCenter.getX(), + FieldConstants.ampCenter.getY() - 0.6, + new Rotation2d(Math.PI / 2.0))))); } /** diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index de357ee7..8375dc6a 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -58,7 +58,6 @@ private RobotState() { /** Add odometry observation */ public void addOdometryObservation(OdometryObservation observation) { - Pose2d lastOdometryPose = odometryPose; Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()); lastWheelPositions = observation.wheelPositions(); // Check gyro connected @@ -74,7 +73,7 @@ public void addOdometryObservation(OdometryObservation observation) { // Add pose to buffer at timestamp poseBuffer.addSample(observation.timestamp(), odometryPose); // Calculate diff from last odometry pose and add onto pose estimate - estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose)); + estimatedPose = estimatedPose.exp(twist); } public void addVisionObservation(VisionObservation observation) { @@ -141,6 +140,7 @@ public void resetPose(Pose2d initialPose) { poseBuffer.clear(); } + @AutoLogOutput(key = "Odometry/FieldVelocity") public Twist2d fieldVelocity() { Translation2d linearFieldVelocity = new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation()); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 85dc9a35..9910e455 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,8 +22,8 @@ import frc.robot.RobotState; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.shooting.ShotCalculator; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -64,9 +64,10 @@ public static Command joystickDrive( linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(), linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(), omega * DriveConstants.driveConfig.maxAngularVelocity()); - drive.setDriveVelocity( + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, RobotState.getInstance().getEstimatedPose().getRotation())); + speeds, RobotState.getInstance().getEstimatedPose().getRotation()); + drive.setVelocity(speeds); }); } diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index 20e0667d..3d01170d 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldConstants; import frc.robot.RobotState; -import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.shooting.ShotCalculator; import java.util.function.Supplier; public class AutoCommands { diff --git a/src/main/java/frc/robot/commands/auto/TestAutos.java b/src/main/java/frc/robot/commands/auto/TestAutos.java index 2bf27691..45c22126 100644 --- a/src/main/java/frc/robot/commands/auto/TestAutos.java +++ b/src/main/java/frc/robot/commands/auto/TestAutos.java @@ -65,8 +65,7 @@ public static Command fourNoteDavis(Drive drive, Intake intake) { return Commands.sequence( resetPoseCommand(trajectory.getStartPose()), - drive.setTrajectoryCommand(trajectory), - Commands.parallel(sequenceIntake)); + Commands.parallel(drive.followTrajectory(trajectory), sequenceIntake)); } private static Command resetPoseCommand(Pose2d pose) { diff --git a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java index ac37c823..5163fd5e 100644 --- a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java +++ b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java @@ -22,7 +22,7 @@ public class AprilTagVisionConstants { Units.inchesToMeters(9.974), Units.inchesToMeters(6.839), new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0)) - .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0))) // offset? + .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0))) }; default -> new Pose3d[] {}; }; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4aab1a00..6d1e3f3b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,16 +13,19 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; +import frc.robot.subsystems.drive.planners.AutoAlignMotionPlanner; +import frc.robot.subsystems.drive.planners.TrajectoryMotionPlanner; import frc.robot.util.GeomUtil; +import frc.robot.util.LoggedTunableNumber; import frc.robot.util.swerve.ModuleLimits; import frc.robot.util.swerve.SwerveSetpoint; import frc.robot.util.swerve.SwerveSetpointGenerator; @@ -31,31 +34,42 @@ import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Supplier; import java.util.stream.IntStream; import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - - private enum ControlMode { - TRAJECTORY_FOLLOWING, - DRIVER_INPUT + private static final LoggedTunableNumber coastSpeedLimit = + new LoggedTunableNumber( + "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); + private static final LoggedTunableNumber coastDisableTime = + new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5); + + @AutoLog + public static class OdometryTimestampInputs { + public double[] timestamps = new double[] {}; } public static final Lock odometryLock = new ReentrantLock(); // TODO: DO THIS BETTER! public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); + private final OdometryTimestampInputsAutoLogged odometryTimestampInputs = + new OdometryTimestampInputsAutoLogged(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; - private ControlMode currentControlMode = ControlMode.DRIVER_INPUT; + private boolean modulesOrienting = false; + private boolean characterizing = false; + private final Timer coastTimer = new Timer(); + private boolean brakeModeEnabled = true; + private double characterizationVolts = 0.0; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); - private final AutoMotionPlanner autoMotionPlanner; - private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; private SwerveSetpoint currentSetpoint = new SwerveSetpoint( @@ -68,47 +82,54 @@ private enum ControlMode { }); private SwerveSetpointGenerator setpointGenerator; - private boolean characterizing = false; - private double characterizationVolts = 0.0; + private final TrajectoryMotionPlanner trajectoryMotionPlanner; + private final AutoAlignMotionPlanner autoAlignMotionPlanner; - public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { + public Drive( + GyroIO gyroIO, + ModuleIO fl, + ModuleIO fr, + ModuleIO bl, + ModuleIO br, + boolean useMotorConroller) { System.out.println("[Init] Creating Drive"); this.gyroIO = gyroIO; - modules[0] = new Module(fl, 0); - modules[1] = new Module(fr, 1); - modules[2] = new Module(bl, 2); - modules[3] = new Module(br, 3); + modules[0] = new Module(fl, 0, useMotorConroller); + modules[1] = new Module(fr, 1, useMotorConroller); + modules[2] = new Module(bl, 2, useMotorConroller); + modules[3] = new Module(br, 3, useMotorConroller); setpointGenerator = SwerveSetpointGenerator.builder() .kinematics(DriveConstants.kinematics) .moduleLocations(DriveConstants.moduleTranslations) .build(); - autoMotionPlanner = new AutoMotionPlanner(); + trajectoryMotionPlanner = new TrajectoryMotionPlanner(); + autoAlignMotionPlanner = new AutoAlignMotionPlanner(); } public void periodic() { // Update & process inputs odometryLock.lock(); - // Read timestamps from odometry thread and fake sim timestamps - double[] timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); - if (timestamps.length == 0) { - timestamps = new double[] {Timer.getFPGATimestamp()}; + odometryTimestampInputs.timestamps = + timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); + if (odometryTimestampInputs.timestamps.length == 0) { + odometryTimestampInputs.timestamps = new double[] {Timer.getFPGATimestamp()}; } timestampQueue.clear(); - - // Read inputs from gyro and modules + Logger.processInputs("Drive/OdometryTimestamps", odometryTimestampInputs); + // Read inputs from gyro gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); - + // Read inputs from modules Arrays.stream(modules).forEach(Module::updateInputs); odometryLock.unlock(); // Calculate the min odometry position updates across all modules int minOdometryUpdates = IntStream.of( - timestamps.length, + odometryTimestampInputs.timestamps.length, Arrays.stream(modules) .mapToInt(module -> module.getModulePositions().length) .min() @@ -129,63 +150,66 @@ public void periodic() { Arrays.stream(modules) .map(module -> module.getModulePositions()[odometryIndex]) .toArray(SwerveModulePosition[]::new)); + // Add observation to robot state RobotState.getInstance() .addOdometryObservation( - new RobotState.OdometryObservation(wheelPositions, yaw, timestamps[i])); + new RobotState.OdometryObservation( + wheelPositions, yaw, odometryTimestampInputs.timestamps[i])); } // update current velocities use gyro when possible - ChassisSpeeds robotRelativeVelocity = - DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + ChassisSpeeds robotRelativeVelocity = getSpeeds(); robotRelativeVelocity.omegaRadiansPerSecond = gyroInputs.connected ? gyroInputs.yawVelocityRadPerSec : robotRelativeVelocity.omegaRadiansPerSecond; RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); + // Disabled, stop modules and coast + if (DriverStation.isDisabled()) { + Arrays.stream(modules).forEach(Module::stop); + if (Math.hypot( + robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) + <= coastSpeedLimit.get() + && brakeModeEnabled) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else { + coastTimer.start(); + } + return; + } + + // Run characterization if (characterizing) { - // Run characterization for (Module module : modules) { module.runCharacterization(characterizationVolts); } - - Logger.recordOutput("Drive/SwerveStates/Desired(b4 Poofs)", new double[] {}); - Logger.recordOutput("Drive/SwerveStates/Setpoints", new double[] {}); - Logger.recordOutput("Drive/DesiredSpeeds", new double[] {}); - Logger.recordOutput("Drive/SetpointSpeeds", new double[] {}); return; } - if (currentControlMode != null) { - // Update setpoint - switch (currentControlMode) { - case TRAJECTORY_FOLLOWING -> - desiredSpeeds = - autoMotionPlanner.update( - Timer.getFPGATimestamp(), - RobotState.getInstance().getEstimatedPose(), - RobotState.getInstance().fieldVelocity()); - case DRIVER_INPUT -> { - // set in runVelocity method - } - } + // Skip if orienting modules + if (modulesOrienting) { + return; } - // Run robot at speeds - // account for skew - desiredSpeeds = ChassisSpeeds.discretize(desiredSpeeds, 0.02); - // generate feasible next setpoint - // currentSetpoint = - // setpointGenerator.generateSetpoint( - // currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); + // Run robot at desiredSpeeds + // Generate feasible next setpoint + currentSetpoint = + setpointGenerator.generateSetpoint( + currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); // run modules - SwerveModuleState[] optimizedSetpointStates = - DriveConstants.kinematics.toSwerveModuleStates(desiredSpeeds); + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) { // Optimize setpoints optimizedSetpointStates[i] = - SwerveModuleState.optimize(optimizedSetpointStates[i], modules[i].getAngle()); + SwerveModuleState.optimize(currentSetpoint.moduleStates()[i], modules[i].getAngle()); modules[i].runSetpoint(optimizedSetpointStates[i]); } @@ -198,12 +222,10 @@ public void periodic() { Logger.recordOutput("Drive/SetpointSpeeds", currentSetpoint.chassisSpeeds()); } - /** Runs drive at velocity from drive input */ - public void setDriveVelocity(ChassisSpeeds velocity) { - if (DriverStation.isTeleopEnabled()) { - currentControlMode = ControlMode.DRIVER_INPUT; - desiredSpeeds = velocity; - } + /** Set drive velocity (robot relative) */ + public void setVelocity(ChassisSpeeds velocity) { + desiredSpeeds = ChassisSpeeds.discretize(velocity, 0.02); + setBrakeMode(true); } /** Runs forwards at the commanded voltage. */ @@ -212,6 +234,7 @@ public void runCharacterizationVolts(double volts) { characterizationVolts = volts; } + /** Disables the characterization mode. */ public void endCharacterization() { characterizing = false; } @@ -225,30 +248,50 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } + /** Set brake mode enabled */ public void setBrakeMode(boolean enabled) { + brakeModeEnabled = enabled; Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } - public void setTrajectory(Trajectory trajectory) { - if (currentControlMode != ControlMode.TRAJECTORY_FOLLOWING) - currentControlMode = ControlMode.TRAJECTORY_FOLLOWING; - autoMotionPlanner.setTrajectory(trajectory); + public Command orientModules(Rotation2d orientation) { + return orientModules(new Rotation2d[] {orientation, orientation, orientation, orientation}); } - public Command setTrajectoryCommand(Trajectory trajectory) { - return Commands.runOnce(() -> setTrajectory(trajectory)); + public Command orientModules(Rotation2d[] orientations) { + return run(() -> { + for (int i = 0; i < orientations.length; i++) { + modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); + } + }) + .until( + () -> + Arrays.stream(modules) + .allMatch( + module -> + Math.abs( + module.getAngle().getDegrees() + - module.getSetpointState().angle.getDegrees()) + <= 2.0)) + .beforeStarting(() -> modulesOrienting = true) + .finallyDo(() -> modulesOrienting = false); } - public boolean finishedTrajectory() { - return autoMotionPlanner.isFinished(); + /** Follows a trajectory using the trajectory motion planner. */ + public Command followTrajectory(Trajectory trajectory) { + return run(() -> setVelocity(trajectoryMotionPlanner.update())) + .beforeStarting(() -> trajectoryMotionPlanner.setTrajectory(trajectory)) + .until(trajectoryMotionPlanner::isFinished); } - private SwerveDriveWheelPositions getWheelPositions() { - return new SwerveDriveWheelPositions( - Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new)); + /** Auto aligns to a pose using the auto align motion planner. */ + public Command autoAlignToPose(Supplier goalPose) { + return run(() -> setVelocity(autoAlignMotionPlanner.update())) + .beforeStarting(() -> autoAlignMotionPlanner.setGoalPose(goalPose.get())) + .until(autoAlignMotionPlanner::atGoal); } - @AutoLogOutput(key = "Odometry/GyroYaw") + @AutoLogOutput(key = "Drive/GyroYaw") public Rotation2d getGyroYaw() { return gyroInputs.connected ? gyroInputs.yawPosition @@ -256,11 +299,17 @@ public Rotation2d getGyroYaw() { } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") + @AutoLogOutput(key = "Drive/SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); } + /** Returns the measured speeds of the robot in the robot's frame of reference. */ + @AutoLogOutput(key = "Drive/MeasuredSpeeds") + private ChassisSpeeds getSpeeds() { + return DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + } + public static Rotation2d[] getStraightOrientations() { return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index d7f7b208..68e19b41 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -13,6 +13,18 @@ /** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { + // TODO: get effective wheel radius + public static final double wheelRadius = Units.inchesToMeters(2.0); + + // For Kraken + public static class KrakenDriveConstants { + public static final boolean useTorqueCurrentFOC = false; + public static final boolean useMotionMagic = false; + public static final double motionMagicCruiseVelocity = 0.0; + public static final double motionMagicAcceleration = 0.0; + } + + // Drive Constants public static DriveConfig driveConfig = switch (Constants.getRobot()) { case SIMBOT, COMPBOT -> @@ -32,7 +44,6 @@ public final class DriveConstants { 7.93, 29.89); }; - public static final double wheelRadius = Units.inchesToMeters(2.0); public static final Translation2d[] moduleTranslations = new Translation2d[] { new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), @@ -40,10 +51,10 @@ public final class DriveConstants { new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0) }; - public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); + // Odometry Constants public static final double odometryFrequency = switch (Constants.getRobot()) { case SIMBOT -> 50.0; @@ -56,6 +67,7 @@ public final class DriveConstants { default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); }; + // Module Constants public static ModuleConfig[] moduleConfigs = switch (Constants.getRobot()) { case COMPBOT, RAINBOWT -> @@ -75,7 +87,17 @@ public final class DriveConstants { public static ModuleConstants moduleConstants = switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> + case COMPBOT -> + new ModuleConstants( + 2.0, + 0.0, + 200.0, + 0.0, + 200.0, + 20.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + case RAINBOWT -> new ModuleConstants( 0.1, 0.13, @@ -93,7 +115,7 @@ public final class DriveConstants { 0.0, 10.0, 0.0, - Mk4iReductions.L2.reduction, + Mk4iReductions.L3.reduction, Mk4iReductions.TURN.reduction); }; @@ -103,13 +125,14 @@ public final class DriveConstants { driveConfig.maxLinearVelocity() * 5, Units.degreesToRadians(1080.0)); + // Trajectory Following public static TrajectoryConstants trajectoryConstants = switch (Constants.getRobot()) { case COMPBOT, RAINBOWT -> new TrajectoryConstants( 6.0, 0.0, - 10.0, + 5.0, 0.0, Units.inchesToMeters(4.0), Units.degreesToRadians(5.0), @@ -131,6 +154,21 @@ public final class DriveConstants { driveConfig.maxAngularVelocity() / 2.0); }; + // Auto Align + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + + // Swerve Heading Control public static HeadingControllerConstants headingControllerConstants = switch (Constants.getRobot()) { default -> new HeadingControllerConstants(3.0, 0.0); @@ -177,6 +215,18 @@ public record TrajectoryConstants( double linearVelocityTolerance, double angularVelocityTolerance) {} + public record AutoAlignConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) {} + public record HeadingControllerConstants(double Kp, double Kd) {} private enum Mk4iReductions { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index e37282a4..fbfde029 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -6,11 +6,10 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; import org.littletonrobotics.junction.Logger; public class Module { - private static final LoggedTunableNumber wheelRadius = - new LoggedTunableNumber("Drive/Module/WheelRadius", DriveConstants.wheelRadius); private static final LoggedTunableNumber driveKp = new LoggedTunableNumber("Drive/Module/DriveKp", DriveConstants.moduleConstants.driveKp()); private static final LoggedTunableNumber driveKd = @@ -24,9 +23,10 @@ public class Module { private static final LoggedTunableNumber turnKd = new LoggedTunableNumber("Drive/Module/TurnKd", DriveConstants.moduleConstants.turnKd()); + private final int index; private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; + private final boolean useMotorController; private final PIDController driveController = new PIDController( @@ -34,13 +34,16 @@ public class Module { private final PIDController turnController = new PIDController( DriveConstants.moduleConstants.turnKp(), 0.0, DriveConstants.moduleConstants.turnKd()); - private SimpleMotorFeedforward driveFf = + private SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward( DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0); - public Module(ModuleIO io, int index) { + @Getter private SwerveModuleState setpointState = new SwerveModuleState(); + + public Module(ModuleIO io, int index, boolean useMotorController) { this.io = io; this.index = index; + this.useMotorController = useMotorController; turnController.enableContinuousInput(-Math.PI, Math.PI); } @@ -50,24 +53,43 @@ public void updateInputs() { io.updateInputs(inputs); Logger.processInputs("Drive/Module" + index, inputs); - driveController.setPID(driveKp.get(), 0, driveKd.get()); + // Update FF and controllers LoggedTunableNumber.ifChanged( hashCode(), - () -> driveFf = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0), + () -> { + driveFF = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0); + io.setDriveFF(driveKs.get(), driveKv.get(), 0); + }, driveKs, driveKv); - turnController.setPID(turnKp.get(), 0, driveKd.get()); + if (useMotorController) { + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setDrivePID(driveKp.get(), 0, driveKd.get()), driveKp, driveKd); + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setTurnPID(turnKp.get(), 0, turnKd.get()), turnKp, turnKd); + } else { + driveController.setPID(driveKp.get(), 0, driveKd.get()); + turnController.setPID(turnKp.get(), 0, turnKd.get()); + } } public void runSetpoint(SwerveModuleState setpoint) { - io.setDriveVoltage( - driveController.calculate( - inputs.driveVelocityRadPerSec, - setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) - + driveFf.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); - io.setTurnVoltage( - turnController.calculate( - inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); + setpointState = setpoint; + if (useMotorController) { + io.setDriveVelocitySetpoint( + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius, + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setTurnPositionSetpoint(setpoint.angle.getRadians()); + } else { + io.setDriveVoltage( + driveController.calculate( + inputs.driveVelocityRadPerSec, + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) + + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setTurnVoltage( + turnController.calculate( + inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); + } } public void runCharacterization(double volts) { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 407cd2c9..d60f0e8c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -22,13 +22,15 @@ class ModuleIOInputs { public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; - public double driveCurrentAmps = 0.0; + public double driveSupplyCurrentAmps = 0.0; + public double driveTorqueCurrentAmps = 0.0; public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; - public double turnCurrentAmps = 0.0; + public double turnSupplyCurrentAmps = 0.0; + public double turnTorqueCurrentAmps = 0.0; public double[] odometryDrivePositionsMeters = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; @@ -37,13 +39,26 @@ class ModuleIOInputs { /** Updates the set of loggable inputs. */ default void updateInputs(ModuleIOInputs inputs) {} + /** Run drive motor at volts */ default void setDriveVoltage(double volts) {} + /** Run turn motor at volts */ default void setTurnVoltage(double volts) {} - default void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) {} + /** Set drive velocity setpoint */ + default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {} - default void setTurnPositionSetpoint(double angleRadians, double ffVolts) {} + /** Set turn position setpoint */ + default void setTurnPositionSetpoint(double angleRads) {} + + /** Configure drive PID */ + default void setDrivePID(double kP, double kI, double kD) {} + + /** Configure turn PID */ + default void setTurnPID(double kP, double kI, double kD) {} + + /** Configure drive feedforward for TorqueCurrentFOC */ + default void setDriveFF(double kS, double kV, double kA) {} /** Enable or disable brake mode on the drive motor. */ default void setDriveBrakeMode(boolean enable) {} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java index 0368f494..ee06881d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -1,279 +1,266 @@ -//// Copyright 2021-2024 FRC 6328 -//// http://github.com/Mechanical-Advantage -//// -//// This program is free software; you can redistribute it and/or -//// modify it under the terms of the GNU General Public License -//// version 3 as published by the Free Software Foundation or -//// available in the root directory of this project. -//// -//// This program is distributed in the hope that it will be useful, -//// but WITHOUT ANY WARRANTY; without even the implied warranty of -//// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -//// GNU General Public License for more details. -// -// package frc.robot.subsystems.drive; -// -// import static frc.robot.subsystems.drive.DriveConstants.*; -// -// import com.ctre.phoenix6.BaseStatusSignal; -// import com.ctre.phoenix6.StatusCode; -// import com.ctre.phoenix6.StatusSignal; -// import com.ctre.phoenix6.configs.MotorOutputConfigs; -// import com.ctre.phoenix6.configs.TalonFXConfiguration; -// import com.ctre.phoenix6.controls.NeutralOut; -// import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -// import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -// import com.ctre.phoenix6.controls.VoltageOut; -// import com.ctre.phoenix6.hardware.TalonFX; -// import com.ctre.phoenix6.signals.InvertedValue; -// import com.ctre.phoenix6.signals.NeutralModeValue; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.kinematics.SwerveModuleState; -// import edu.wpi.first.math.util.Units; -// import edu.wpi.first.wpilibj.AnalogInput; -// import edu.wpi.first.wpilibj.RobotController; -// import java.util.Queue; -// import java.util.function.Supplier; -// -// public class ModuleIOKrakenFOC implements ModuleIO { -// // Reseed relative encoder to absolute encoder value -// private static final int reseedRelativeEncoderCounts = 100; -// private int currentReseedCount = reseedRelativeEncoderCounts; -// -// private final StatusSignal drivePosition; -// private final StatusSignal driveVelocity; -// private final StatusSignal driveAppliedVolts; -// private final StatusSignal driveCurrent; -// -// private final StatusSignal turnPosition; -// private final Supplier turnAbsolutePosition; -// private final StatusSignal turnVelocity; -// private final StatusSignal turnAppliedVolts; -// private final StatusSignal turnCurrent; -// private final boolean isTurnMotorInverted = true; -// -// // queues -// private final Queue drivePositionQueue; -// private final Queue turnPositionQueue; -// -// private final TalonFX driveTalon; -// private final TalonFX turnTalon; -// private final AnalogInput turnAbsoluteEncoder; -// private final Rotation2d absoluteEncoderOffset; -// -// // Setpoint control -// private final VelocityTorqueCurrentFOC driveVelocityControl = -// new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false, false); -// private final PositionTorqueCurrentFOC turnPositionControl = -// new PositionTorqueCurrentFOC(0.0, 0.0, 0.0, 1, false, false, false); -// private final VoltageOut driveVoltageControl = new VoltageOut(0.0).withEnableFOC(true); -// private final NeutralOut driveBrake = new NeutralOut(); -// private final NeutralOut turnBrake = new NeutralOut(); -// private Mode currentMode = Mode.SETPOINT; -// private double driveSpeedSetpoint = 0.0; -// private Rotation2d turnAngleSetpoint = new Rotation2d(); -// private double driveVoltage = 0.0; -// -// public ModuleIOKrakenFOC(ModuleConfig config) { -// // init controllers and encoders from config constants -// driveTalon = new TalonFX(config.driveID()); -// turnTalon = new TalonFX(config.turnID()); -// turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); -// absoluteEncoderOffset = config.absoluteEncoderOffset(); -// -// var driveConfig = new TalonFXConfiguration(); -// driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; -// driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; -// driveConfig.Voltage.PeakForwardVoltage = 12.0; -// driveConfig.Voltage.PeakReverseVoltage = -12.0; -// // TUNE PID CONSTANTS -// driveConfig.Slot0.kP = moduleConstants.driveKp(); -// driveConfig.Slot0.kI = 0.0; -// driveConfig.Slot0.kD = moduleConstants.driveKd(); -// driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; -// driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; -// -// var turnConfig = new TalonFXConfiguration(); -// turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; -// turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; -// turnConfig.Voltage.PeakForwardVoltage = 12.0; -// turnConfig.Voltage.PeakReverseVoltage = -12.0; -// // TUNE PID CONSTANTS -// turnConfig.Slot0.kP = moduleConstants.turnKp(); -// turnConfig.Slot0.kI = 0.0; -// turnConfig.Slot0.kD = moduleConstants.turnKd(); -// turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; -// turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; -// turnConfig.ClosedLoopGeneral.ContinuousWrap = true; -// // Map rotation of motor shaft to 1 rotation of turn shaft -// turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); -// -// // Apply configs -// for (int i = 0; i < 4; i++) { -// boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; -// setDriveBrakeMode(true); -// error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); -// setTurnBrakeMode(true); -// if (!error) break; -// } -// -// // Get signals and set update rate -// // 50hz signals -// drivePosition = driveTalon.getPosition(); -// driveVelocity = driveTalon.getVelocity(); -// driveAppliedVolts = driveTalon.getMotorVoltage(); -// driveCurrent = driveTalon.getStatorCurrent(); -// -// turnPosition = turnTalon.getPosition(); -// turnAbsolutePosition = -// () -> -// Rotation2d.fromRadians( -// turnAbsoluteEncoder.getVoltage() -// / RobotController.getVoltage5V() -// * 2.0 -// * Math.PI) -// .minus(absoluteEncoderOffset); -// turnVelocity = turnTalon.getVelocity(); -// turnAppliedVolts = turnTalon.getMotorVoltage(); -// turnCurrent = turnTalon.getStatorCurrent(); -// -// // 250hz signals -// drivePositionQueue = -// PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); -// turnPositionQueue = -// PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); -// -// BaseStatusSignal.setUpdateFrequencyForAll( -// 50.0, -// driveVelocity, -// driveAppliedVolts, -// driveCurrent, -// turnVelocity, -// turnAppliedVolts, -// turnCurrent); -// BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); -// driveTalon.optimizeBusUtilization(); -// turnTalon.optimizeBusUtilization(); -// } -// -// @Override -// public void updateInputs(ModuleIOInputs inputs) { -// BaseStatusSignal.refreshAll( -// drivePosition, -// driveVelocity, -// driveAppliedVolts, -// driveCurrent, -// turnPosition, -// turnVelocity, -// turnAppliedVolts, -// turnCurrent); -// -// inputs.drivePositionRad = -// Units.rotationsToRadians(drivePosition.getValueAsDouble()) -// / moduleConstants.driveReduction(); -// inputs.driveVelocityRadPerSec = -// Units.rotationsToRadians(driveVelocity.getValueAsDouble()) -// / moduleConstants.driveReduction(); -// inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); -// inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; -// -// inputs.turnAbsolutePosition = turnAbsolutePosition.get(); -// inputs.turnPosition = -// Rotation2d.fromRotations(turnPosition.getValueAsDouble() / -// moduleConstants.turnReduction()); -// inputs.turnVelocityRadPerSec = -// Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / -// moduleConstants.turnReduction(); -// inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); -// inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; -// -// inputs.odometryDrivePositionsMeters = -// drivePositionQueue.stream() -// .mapToDouble( -// signal -> -// Units.rotationsToRadians(signal) -// * wheelRadius -// / moduleConstants.driveReduction()) -// .toArray(); -// inputs.odometryTurnPositions = -// turnPositionQueue.stream() -// .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction())) -// .toArray(Rotation2d[]::new); -// drivePositionQueue.clear(); -// turnPositionQueue.clear(); -// } -// -// @Override -// public void periodic() { -// // Only reseed if turn abs encoder is online -// if (++currentReseedCount >= reseedRelativeEncoderCounts -// && turnAbsoluteEncoder.getVoltage() != 0.0) { -// // heading of wheel --> turn motor shaft rotations -// // timeout of 20 ms -// boolean passed = -// turnTalon.setPosition( -// Units.radiansToRotations(turnAbsolutePosition.get().getRadians()) -// * moduleConstants.turnReduction(), -// 0.02) -// == StatusCode.OK; -// // redo next cycle if did not pass -// currentReseedCount = passed ? 0 : currentReseedCount; -// } -// -// // Control motors -// if (currentMode == Mode.NEUTRAL) { -// driveTalon.setControl(driveBrake); -// turnTalon.setControl(turnBrake); -// } else { -// if (currentMode == Mode.SETPOINT) { -// driveTalon.setControl(driveVelocityControl.withVelocity(driveSpeedSetpoint)); -// } else if (currentMode == Mode.CHARACTERIZATION) { -// driveTalon.setControl(driveVoltageControl.withOutput(driveVoltage)); -// } -// turnTalon.setControl(turnPositionControl.withPosition(turnAngleSetpoint.getRotations())); -// } -// } -// -// @Override -// public void runSetpoint(SwerveModuleState state) { -// currentMode = Mode.SETPOINT; -// driveSpeedSetpoint = state.speedMetersPerSecond / wheelRadius; -// turnAngleSetpoint = state.angle; -// } -// -// @Override -// public void runCharacterization(double volts) { -// currentMode = Mode.CHARACTERIZATION; -// driveVoltage = volts; -// } -// -// @Override -// public void setDriveBrakeMode(boolean enable) { -// var config = new MotorOutputConfigs(); -// config.Inverted = InvertedValue.CounterClockwise_Positive; -// config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; -// driveTalon.getConfigurator().apply(config); -// } -// -// @Override -// public void setTurnBrakeMode(boolean enable) { -// var config = new MotorOutputConfigs(); -// config.Inverted = -// isTurnMotorInverted -// ? InvertedValue.Clockwise_Positive -// : InvertedValue.CounterClockwise_Positive; -// config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; -// turnTalon.getConfigurator().apply(config); -// } -// -// @Override -// public void stop() { -// currentMode = Mode.NEUTRAL; -// } -// -// private enum Mode { -// SETPOINT, -// CHARACTERIZATION, -// NEUTRAL; -// } -// } +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; +import java.util.Queue; +import java.util.function.Supplier; + +public class ModuleIOKrakenFOC implements ModuleIO { + // Hardware + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; + + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveTorqueCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrent; + private final StatusSignal turnTorqueCurrent; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private static final int shouldResetCounts = 100; + private int resetCounter = shouldResetCounts; + + private Slot0Configs driveFeedbackConfig = new Slot0Configs(); + private Slot0Configs turnFeedbackConfig = new Slot0Configs(); + + public ModuleIOKrakenFOC(ModuleConfig config) { + // Init controllers and encoders from config constants + driveTalon = new TalonFX(config.driveID(), "canivore"); + turnTalon = new TalonFX(config.turnID(), "canivore"); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + + // Config Motors + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.Voltage.PeakForwardVoltage = 12.0; + driveConfig.Voltage.PeakReverseVoltage = -12.0; + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnConfig.Voltage.PeakForwardVoltage = 12.0; + turnConfig.Voltage.PeakReverseVoltage = -12.0; + turnConfig.MotorOutput.Inverted = + config.turnMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + + // If in motoControl mode, set reference points in rotations convert from radians + // Affects getPosition() and getVelocity() + driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); + turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + + // Config Motion Magic + if (KrakenDriveConstants.useMotionMagic) { + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + KrakenDriveConstants.motionMagicCruiseVelocity; + turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; + } + + // Apply configs + for (int i = 0; i < 4; i++) { + boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; + setDriveBrakeMode(true); + error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); + setTurnBrakeMode(true); + if (!error) break; + } + + // Get signals and set update rate + // 100hz signals + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveTorqueCurrent = driveTalon.getTorqueCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); + turnTorqueCurrent = turnTalon.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + // 250hz signals + drivePosition = driveTalon.getPosition(); + turnPosition = turnTalon.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + + // TODO: what does this do? + // driveTalon.optimizeBusUtilization(); + // turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCounts + && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { + turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); + resetCounter = 0; + } + + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + inputs.driveTorqueCurrentAmps = driveTorqueCurrent.getValueAsDouble(); + + inputs.turnAbsolutePosition = turnAbsolutePosition.get(); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turnTorqueCurrentAmps = turnTorqueCurrent.getValueAsDouble(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble(signalValue -> Units.rotationsToRadians(signalValue) * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); + } else { + driveTalon.setControl( + new VelocityVoltage(velocityRotationsPerSec) + .withFeedForward(ffVolts) + .withEnableFOC(true)); + } + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + double angleRotations = Units.radiansToRotations(angleRads); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); + } else { + turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); + } + } else { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicVoltage(angleRotations).withEnableFOC(true)); + } else { + turnTalon.setControl(new PositionVoltage(angleRotations).withEnableFOC(true)); + } + } + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedbackConfig.kP = kP; + driveFeedbackConfig.kI = kI; + driveFeedbackConfig.kD = kD; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedbackConfig.kP = kP; + turnFeedbackConfig.kI = kI; + turnFeedbackConfig.kD = kD; + turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + } + + @Override + public void setDriveFF(double kS, double kV, double kA) { + driveFeedbackConfig.kS = kS; + driveFeedbackConfig.kV = kV; + driveFeedbackConfig.kA = kA; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void stop() { + driveTalon.setControl(new NeutralOut()); + turnTalon.setControl(new NeutralOut()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 8fc2b247..e0ca78cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -16,52 +16,46 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -/** - * Physics sim implementation of module IO. - * - *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized - * to a random value. The flywheel sims are not physically accurate, but provide a decent - * approximation for the behavior of the module. - */ public class ModuleIOSim implements ModuleIO { - private static final double LOOP_PERIOD_SECS = 0.02; private final DCMotorSim driveSim = - new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.driveReduction(), 0.025); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025); private final DCMotorSim turnSim = - new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.turnReduction(), 0.004); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.turnReduction(), 0.004); + + private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); + private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; private final Rotation2d turnAbsoluteInitPosition; - private Rotation2d turnAbsolutePosition; public ModuleIOSim(ModuleConfig config) { turnAbsoluteInitPosition = config.absoluteEncoderOffset(); - turnAbsolutePosition = turnAbsoluteInitPosition; + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(LOOP_PERIOD_SECS); - turnSim.update(LOOP_PERIOD_SECS); + driveSim.update(0.02); + turnSim.update(0.02); inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - turnAbsolutePosition = inputs.turnAbsolutePosition; inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); inputs.odometryDrivePositionsMeters = new double[] {driveSim.getAngularPositionRad() * wheelRadius}; @@ -79,6 +73,28 @@ public void setTurnVoltage(double volts) { turnSim.setInputVoltage(turnAppliedVolts); } + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + setDriveVoltage( + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) + + ffVolts); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + setTurnVoltage(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedback.setPID(kP, kI, kD); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedback.setPID(kP, kI, kD); + } + @Override public void stop() { driveSim.setInputVoltage(0.0); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 38112b06..4f688368 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -15,11 +15,9 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import com.revrobotics.*; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; @@ -28,18 +26,25 @@ import java.util.function.Supplier; public class ModuleIOSparkMax implements ModuleIO { + // Hardware private final CANSparkMax driveMotor; private final CANSparkMax turnMotor; private final RelativeEncoder driveEncoder; private final RelativeEncoder turnRelativeEncoder; private final AnalogInput turnAbsoluteEncoder; + // Controllers + private final SparkPIDController driveController; + private final SparkPIDController turnController; + // Queues private final Queue drivePositionQueue; private final Queue turnPositionQueue; private final Rotation2d absoluteEncoderOffset; private final Supplier absoluteEncoderValue; + private static final int shouldResetCount = 100; + private int resetCounter = shouldResetCount; public ModuleIOSparkMax(ModuleConfig config) { // Init motor & encoder objects @@ -50,6 +55,13 @@ public ModuleIOSparkMax(ModuleConfig config) { driveEncoder = driveMotor.getEncoder(); turnRelativeEncoder = turnMotor.getEncoder(); + driveController = driveMotor.getPIDController(); + turnController = turnMotor.getPIDController(); + turnController.setPositionPIDWrappingEnabled(true); + final double maxInput = Math.PI * moduleConstants.driveReduction(); + turnController.setPositionPIDWrappingMinInput(-maxInput); + turnController.setPositionPIDWrappingMaxInput(maxInput); + driveMotor.restoreFactoryDefaults(); turnMotor.restoreFactoryDefaults(); driveMotor.setCANTimeout(250); @@ -81,11 +93,8 @@ public ModuleIOSparkMax(ModuleConfig config) { absoluteEncoderValue = () -> - Rotation2d.fromRadians( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V() - * 2.0 - * Math.PI) + Rotation2d.fromRotations( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) .minus(absoluteEncoderOffset); drivePositionQueue = @@ -97,13 +106,24 @@ public ModuleIOSparkMax(ModuleConfig config) { @Override public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCount + && Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) <= 0.1) { + for (int i = 0; i < 4; i++) { + turnRelativeEncoder.setPosition( + absoluteEncoderValue.get().getRotations() * moduleConstants.turnReduction()); + } + resetCounter = 0; + } + inputs.drivePositionRad = Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( driveEncoder.getVelocity() / moduleConstants.driveReduction()); inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); - inputs.driveCurrentAmps = driveMotor.getOutputCurrent(); + inputs.driveSupplyCurrentAmps = driveMotor.getOutputCurrent(); inputs.turnAbsolutePosition = absoluteEncoderValue.get(); inputs.turnPosition = @@ -114,7 +134,7 @@ public void updateInputs(ModuleIOInputs inputs) { Units.rotationsPerMinuteToRadiansPerSecond( turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); - inputs.turnCurrentAmps = turnMotor.getOutputCurrent(); + inputs.turnSupplyCurrentAmps = turnMotor.getOutputCurrent(); inputs.odometryDrivePositionsMeters = drivePositionQueue.stream() @@ -140,13 +160,38 @@ public void setTurnVoltage(double volts) { } @Override - public void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) { - // TODO + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + driveController.setReference( + Units.radiansToRotations(velocityRadsPerSec) * moduleConstants.driveReduction(), + CANSparkBase.ControlType.kVelocity, + 0, + ffVolts, + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + turnController.setReference( + Units.radiansToRotations(angleRads) * moduleConstants.turnReduction(), + CANSparkBase.ControlType.kPosition); } @Override - public void setTurnPositionSetpoint(double angleRadians, double ffVolts) { - // TODO + public void setDrivePID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + driveController.setP(kP); + driveController.setI(kI); + driveController.setD(kD); + } + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + turnController.setP(kP); + turnController.setI(kI); + turnController.setD(kD); + } } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 231829d0..325fddf0 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -17,10 +17,10 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import edu.wpi.first.wpilibj.Timer; import java.util.*; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -87,7 +87,7 @@ public void run() { } finally { signalsLock.unlock(); } - double fpgaTimestamp = Timer.getFPGATimestamp(); + double fpgaTimestamp = Logger.getRealTimestamp() / 1.0e6; // Save new data to queues Drive.odometryLock.lock(); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index eaaed519..010e7084 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,12 +14,12 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.Timer; import java.util.ArrayList; import java.util.List; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -60,7 +60,7 @@ public Queue registerSignal(DoubleSupplier signal) { private void periodic() { Drive.odometryLock.lock(); - Drive.timestampQueue.offer(Timer.getFPGATimestamp()); + Drive.timestampQueue.offer(Logger.getRealTimestamp() / 1.0e6); try { for (int i = 0; i < signals.size(); i++) { queues.get(i).offer(signals.get(i).getAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java new file mode 100644 index 00000000..2bb12533 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java @@ -0,0 +1,169 @@ +package frc.robot.subsystems.drive.planners; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.robot.RobotState; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class AutoAlignMotionPlanner { + private static LoggedTunableNumber linearKp = + new LoggedTunableNumber("AutoAlign/driveKp", DriveConstants.autoAlignConstants.linearKp()); + private static LoggedTunableNumber linearKd = + new LoggedTunableNumber("AutoAlign/driveKd", DriveConstants.autoAlignConstants.linearKd()); + private static LoggedTunableNumber thetaKp = + new LoggedTunableNumber("AutoAlign/thetaKp", DriveConstants.autoAlignConstants.thetaKp()); + private static LoggedTunableNumber thetaKd = + new LoggedTunableNumber("AutoAlign/thetaKd", DriveConstants.autoAlignConstants.thetaKd()); + private static LoggedTunableNumber linearTolerance = + new LoggedTunableNumber( + "AutoAlign/controllerLinearTolerance", + DriveConstants.autoAlignConstants.linearTolerance()); + private static LoggedTunableNumber thetaTolerance = + new LoggedTunableNumber( + "AutoAlign/controllerThetaTolerance", DriveConstants.autoAlignConstants.thetaTolerance()); + private static LoggedTunableNumber maxLinearVelocity = + new LoggedTunableNumber( + "AutoAlign/maxLinearVelocity", DriveConstants.autoAlignConstants.maxLinearVelocity()); + private static LoggedTunableNumber maxLinearAcceleration = + new LoggedTunableNumber( + "AutoAlign/maxLinearAcceleration", + DriveConstants.autoAlignConstants.maxLinearAcceleration()); + private static LoggedTunableNumber maxAngularVelocity = + new LoggedTunableNumber( + "AutoAlign/maxAngularVelocity", DriveConstants.autoAlignConstants.maxAngularVelocity()); + private static LoggedTunableNumber maxAngularAcceleration = + new LoggedTunableNumber( + "AutoAlign/maxAngularAcceleration", + DriveConstants.autoAlignConstants.maxAngularAcceleration()); + + @Getter(onMethod_ = @AutoLogOutput(key = "AutoAlign/goalPose")) + private Pose2d goalPose = null; + + // Controllers for translation and rotation + private final ProfiledPIDController linearController; + private final ProfiledPIDController thetaController; + + // Store previous velocities for acceleration limiting + private Translation2d prevLinearVelocity; + private double prevAngularVelocity; + + public AutoAlignMotionPlanner() { + linearController = + new ProfiledPIDController( + linearKp.get(), + 0, + linearKd.get(), + new TrapezoidProfile.Constraints(maxLinearVelocity.get(), maxLinearAcceleration.get())); + linearController.setTolerance(linearTolerance.get()); + + thetaController = + new ProfiledPIDController( + thetaKp.get(), + 0, + thetaKd.get(), + new TrapezoidProfile.Constraints( + maxAngularVelocity.get(), maxAngularAcceleration.get())); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(thetaTolerance.get()); + } + + public void setGoalPose(Pose2d goalPose) { + this.goalPose = goalPose; + + // Reset controllers + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); + // Linear controller will control to 0 so distance is the measurement + Rotation2d rotationToGoal = + goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + double velocity = + -new Translation2d(fieldVelocity.dx, fieldVelocity.dy) + .rotateBy(rotationToGoal.unaryMinus()) + .getX(); + linearController.reset( + currentPose.getTranslation().getDistance(goalPose.getTranslation()), velocity); + linearController.setGoal(0.0); + thetaController.reset(currentPose.getRotation().getRadians(), fieldVelocity.dtheta); + + prevLinearVelocity = new Translation2d(fieldVelocity.dx, fieldVelocity.dy); + prevAngularVelocity = fieldVelocity.dtheta; + } + + public ChassisSpeeds update() { + updateControllers(); + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); + + // Calculate feedback velocities (based on position error). + double linearVelocityScalar = + linearController.calculate( + currentPose.getTranslation().getDistance(goalPose.getTranslation())) + + linearController.getSetpoint().velocity; + Rotation2d rotationToGoal = + goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + double xVelocity = -linearVelocityScalar * rotationToGoal.getCos(); + double yVelocity = -linearVelocityScalar * rotationToGoal.getSin(); + + // If current angular velocity is fast enough in current direction continue in direction + double angularVelocity = + thetaController.calculate( + currentPose.getRotation().getRadians(), goalPose.getRotation().getRadians()) + + thetaController.getSetpoint().velocity; + + // Limit linear acceleration + // Forward limiting and brake limiting + Translation2d desiredLinearVelocity = new Translation2d(xVelocity, yVelocity); + Translation2d deltaVelocity = desiredLinearVelocity.minus(prevLinearVelocity); + double maxDeltaVelocity = maxLinearAcceleration.get() * 0.02; + if (deltaVelocity.getNorm() > maxDeltaVelocity) { + desiredLinearVelocity = + prevLinearVelocity.plus(deltaVelocity.times(maxDeltaVelocity / deltaVelocity.getNorm())); + } + prevLinearVelocity = new Translation2d(fieldVelocity.dx, fieldVelocity.dy); + + ChassisSpeeds fieldRelativeSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelocity); + Logger.recordOutput("AutoAlign/speeds", fieldRelativeSpeeds); + Logger.recordOutput("AutoAlign/linearError", linearController.getPositionError()); + Logger.recordOutput("AutoAlign/thetaError", thetaController.getPositionError()); + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, currentPose.getRotation()); + } + + @AutoLogOutput(key = "AutoAlign/atGoal") + public boolean atGoal() { + return linearController.atGoal() && thetaController.atSetpoint(); + } + + private void updateControllers() { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> linearController.setPID(linearKp.get(), 0, linearKd.get()), + linearKp, + linearKd); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> thetaController.setPID(thetaKp.get(), 0, thetaKd.get()), + thetaKp, + thetaKd); + LoggedTunableNumber.ifChanged( + hashCode(), () -> linearController.setTolerance(linearTolerance.get()), linearTolerance); + LoggedTunableNumber.ifChanged( + hashCode(), () -> thetaController.setTolerance(thetaTolerance.get()), thetaTolerance); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> + linearController.setConstraints( + new TrapezoidProfile.Constraints( + maxLinearVelocity.get(), maxLinearAcceleration.get())), + maxLinearVelocity, + maxLinearAcceleration); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java similarity index 66% rename from src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java rename to src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java index 5933f533..93c95313 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java +++ b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.drive; +package frc.robot.subsystems.drive.planners; import static frc.robot.subsystems.drive.DriveConstants.*; import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; @@ -9,15 +9,17 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.Constants; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.RobotState; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.trajectory.HolonomicDriveController; import frc.robot.util.trajectory.Trajectory; import java.util.Arrays; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class AutoMotionPlanner { +public class TrajectoryMotionPlanner { private static LoggedTunableNumber trajectoryLinearKp = new LoggedTunableNumber("Trajectory/linearKp", trajectoryConstants.linearKp()); private static LoggedTunableNumber trajectoryLinearKd = @@ -47,11 +49,10 @@ public class AutoMotionPlanner { trajectoryConstants.angularVelocityTolerance()); private Trajectory trajectory = null; - private Double startTime = null; - private Double currentTime = null; + private Timer trajectoryTimer = new Timer(); private final HolonomicDriveController controller; - public AutoMotionPlanner() { + public TrajectoryMotionPlanner() { controller = new HolonomicDriveController( trajectoryLinearKp.get(), @@ -61,94 +62,85 @@ public AutoMotionPlanner() { configTrajectoryTolerances(); } - protected void setTrajectory(Trajectory trajectory) { - // Only set if not following trajectory or done with current one - if (this.trajectory == null || isFinished()) { - this.trajectory = trajectory; - controller.setGoalState(trajectory.getEndState()); - controller.resetControllers(); - startTime = null; - currentTime = null; - // Log poses - Logger.recordOutput( - "Trajectory/trajectoryPoses", - Arrays.stream(trajectory.getTrajectoryPoses()) - .map(AllianceFlipUtil::apply) - .toArray(Pose2d[]::new)); - } + public void setTrajectory(Trajectory trajectory) { + this.trajectory = trajectory; + controller.setGoalState(AllianceFlipUtil.apply(trajectory.getEndState())); + controller.resetControllers(); + trajectoryTimer.restart(); + // Log poses + Logger.recordOutput( + "Trajectory/trajectoryPoses", + Arrays.stream(trajectory.getTrajectoryPoses()) + .map(AllianceFlipUtil::apply) + .toArray(Pose2d[]::new)); } /** Output setpoint chassis speeds in */ - protected ChassisSpeeds update(double timestamp, Pose2d robot, Twist2d fieldVelocity) { - System.out.println(timestamp + " " + robot.toString() + " " + fieldVelocity.toString()); + public ChassisSpeeds update() { + updateControllers(); + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); // If disabled reset everything and stop if (DriverStation.isDisabled()) { trajectory = null; - // Stop logs - Logger.recordOutput("Trajectory/trajectoryPoses", new Pose2d[] {}); - Logger.recordOutput("Trajectory/setpointPose", new Pose2d()); - Logger.recordOutput("Trajectory/speeds", new double[] {}); + trajectoryTimer.stop(); + trajectoryTimer.reset(); + controller.resetControllers(); return new ChassisSpeeds(); } - // Tunable numbers - if (Constants.tuningMode) { - // Update PID Controllers - LoggedTunableNumber.ifChanged( - hashCode(), - pid -> controller.setPID(pid[0], pid[1], pid[2], pid[3]), - trajectoryLinearKp, - trajectoryLinearKd, - trajectoryThetaKp, - trajectoryThetaKp); - // Tolerances - LoggedTunableNumber.ifChanged( - hashCode(), - this::configTrajectoryTolerances, - trajectoryLinearTolerance, - trajectoryThetaTolerance, - trajectoryGoalLinearTolerance, - trajectoryGoalThetaTolerance, - trajectoryLinearVelocityTolerance, - trajectoryAngularVelocityTolerance); - } - + HolonomicDriveState currentState = + new HolonomicDriveState( + currentPose, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); + // Sample and flip state + HolonomicDriveState setpointState = trajectory.sample(trajectoryTimer.get()); + setpointState = AllianceFlipUtil.apply(setpointState); + // calculate trajectory speeds + ChassisSpeeds speeds = controller.calculate(currentState, setpointState); // Reached end - if (isFinished() || trajectory == null) { + if (isFinished()) { + trajectoryTimer.stop(); // Stop logs Logger.recordOutput("Trajectory/trajectoryPoses", new Pose2d[] {}); Logger.recordOutput("Trajectory/setpointPose", new Pose2d()); Logger.recordOutput("Trajectory/speeds", new double[] {}); return new ChassisSpeeds(); } - - if (startTime == null) { - startTime = timestamp; - } - currentTime = timestamp; - - HolonomicDriveState currentState = - new HolonomicDriveState(robot, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); - // Sample and flip state - HolonomicDriveState setpointState = trajectory.sample(currentTime - startTime); - setpointState = AllianceFlipUtil.apply(setpointState); - // calculate trajectory speeds - ChassisSpeeds speeds = controller.calculate(currentState, setpointState); - // Log trajectory data Logger.recordOutput("Trajectory/setpointPose", setpointState.pose()); Logger.recordOutput( "Trajectory/translationError", controller.getPoseError().getTranslation().getNorm()); Logger.recordOutput( "Trajectory/rotationError", controller.getPoseError().getRotation().getRadians()); + Logger.recordOutput("Trajectory/speeds", speeds); return speeds; } - protected boolean isFinished() { + private void updateControllers() { + // Update PID Controllers + LoggedTunableNumber.ifChanged( + hashCode(), + pid -> controller.setPID(pid[0], pid[1], pid[2], pid[3]), + trajectoryLinearKp, + trajectoryLinearKd, + trajectoryThetaKp, + trajectoryThetaKp); + // Tolerances + LoggedTunableNumber.ifChanged( + hashCode(), + this::configTrajectoryTolerances, + trajectoryLinearTolerance, + trajectoryThetaTolerance, + trajectoryGoalLinearTolerance, + trajectoryGoalThetaTolerance, + trajectoryLinearVelocityTolerance, + trajectoryAngularVelocityTolerance); + } + + @AutoLogOutput(key = "Trajectory/finished") + public boolean isFinished() { return trajectory != null - && currentTime != null - && startTime != null - && currentTime - startTime >= trajectory.getDuration() + && trajectoryTimer.hasElapsed(trajectory.getDuration()) && controller.atGoal(); } diff --git a/src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java b/src/main/java/frc/robot/util/shooting/ShotCalculator.java similarity index 98% rename from src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java rename to src/main/java/frc/robot/util/shooting/ShotCalculator.java index bb0ae8ac..1f674413 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java +++ b/src/main/java/frc/robot/util/shooting/ShotCalculator.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.util.shooting; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java index 0c221f4e..9f7a0ad9 100644 --- a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java @@ -2,7 +2,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; import lombok.Setter; @@ -16,20 +16,19 @@ public class HolonomicDriveController { /** -- SETTER -- Set tolerance for goal state */ @Setter private HolonomicDriveState goalTolerance = null; + private Pose2d controllerTolerance = null; + /** -- SETTER -- Set goal state */ @Setter private HolonomicDriveState goalState = null; @Getter private Pose2d poseError; - /** -- SETTER -- Set tolerance of controller with Pose2d */ - @Setter private Pose2d controllerTolerance = null; - public HolonomicDriveController( double linearKp, double linearKd, double thetaKp, double thetaKd) { linearController = new PIDController(linearKp, 0, linearKd); thetaController = new PIDController(thetaKp, 0, thetaKd); - this.thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.enableContinuousInput(-Math.PI, Math.PI); } /** Reset all controllers */ @@ -42,6 +41,11 @@ public void resetThetaController() { thetaController.reset(); } + public void setControllerTolerance(Pose2d controllerTolerance) { + linearController.setTolerance(controllerTolerance.getTranslation().getNorm()); + thetaController.setTolerance(controllerTolerance.getRotation().getRadians()); + } + /** Set PID values */ public void setPID(double linearKp, double linearKd, double thetaKp, double thetaKd) { linearController.setPID(linearKp, 0, linearKd); @@ -52,20 +56,30 @@ public void setPID(double linearKp, double linearKd, double thetaKp, double thet public ChassisSpeeds calculate( HolonomicDriveState currentState, HolonomicDriveState setpointState) { this.currentState = currentState; - Pose2d poseRef = setpointState.pose(); - poseError = poseRef.relativeTo(currentState.pose()); + Pose2d setpointPose = setpointState.pose(); + poseError = setpointPose.relativeTo(currentState.pose()); // Calculate feedback velocities (based on position error). - Translation2d currToStateTranslation = - poseRef.getTranslation().minus(currentState.pose().getTranslation()); double linearFeedback = linearController.calculate( - 0, currentState.pose().getTranslation().getDistance(poseRef.getTranslation())); - double xFeedback = linearFeedback * (currToStateTranslation.getAngle().getCos()); - double yFeedback = linearFeedback * (currToStateTranslation.getAngle().getSin()); + 0, currentState.pose().getTranslation().getDistance(setpointPose.getTranslation())); + Rotation2d currentToStateAngle = + setpointPose.getTranslation().minus(currentState.pose().getTranslation()).getAngle(); + double xFeedback = linearFeedback * currentToStateAngle.getCos(); + double yFeedback = linearFeedback * currentToStateAngle.getSin(); double thetaFeedback = thetaController.calculate( - currentState.pose().getRotation().getRadians(), poseRef.getRotation().getRadians()); + currentState.pose().getRotation().getRadians(), + setpointPose.getRotation().getRadians()); + if (atGoal()) { + if (linearController.atSetpoint()) { + xFeedback = 0; + yFeedback = 0; + } + if (thetaController.atSetpoint()) { + thetaFeedback = 0; + } + } // Return next output. return ChassisSpeeds.fromFieldRelativeSpeeds( @@ -75,6 +89,10 @@ public ChassisSpeeds calculate( currentState.pose().getRotation()); } + public boolean atSetpoint() { + return linearController.atSetpoint() && thetaController.atSetpoint(); + } + /** Within tolerance of current goal */ public boolean atGoal() { Pose2d goalPoseError = goalState.pose().relativeTo(currentState.pose()); diff --git a/trajectory_native/.devcontainer/devcontainer.json b/trajectory_native/.devcontainer/devcontainer.json new file mode 100644 index 00000000..c48c1214 --- /dev/null +++ b/trajectory_native/.devcontainer/devcontainer.json @@ -0,0 +1,12 @@ +{ + "image": "littletonrobotics/vts-dev:latest", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "zxh404.vscode-proto3" + ] + } + } +} \ No newline at end of file diff --git a/trajectory_native/.gitignore b/trajectory_native/.gitignore new file mode 100644 index 00000000..0566c63a --- /dev/null +++ b/trajectory_native/.gitignore @@ -0,0 +1,3 @@ +libs/ +generated/ +build/ \ No newline at end of file diff --git a/trajectory_native/CMakeLists.txt b/trajectory_native/CMakeLists.txt new file mode 100644 index 00000000..9d4af1d7 --- /dev/null +++ b/trajectory_native/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.24) +project(trajectory_native) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--whole-archive --allow-multiple-definition") + +find_package(protobuf CONFIG REQUIRED) +find_package(gRPC CONFIG REQUIRED) +find_package(Threads) +find_package(fmt REQUIRED) +find_package(nlohmann_json REQUIRED) + +add_library(trajectory_native_proto proto/VehicleTrajectoryService.proto) +target_link_libraries(trajectory_native_proto PUBLIC protobuf::libprotobuf gRPC::grpc gRPC::grpc++ gRPC::grpc++_reflection) +target_include_directories(trajectory_native_proto PUBLIC ${CMAKE_CURRENT_BINARY_DIR}) + +get_target_property(grpc_cpp_plugin_location gRPC::grpc_cpp_plugin LOCATION) +protobuf_generate(TARGET trajectory_native_proto LANGUAGE cpp) +protobuf_generate(TARGET trajectory_native_proto LANGUAGE grpc GENERATE_EXTENSIONS .grpc.pb.h .grpc.pb.cc PLUGIN "protoc-gen-grpc=${grpc_cpp_plugin_location}") + +find_library(TRAJOPT_LIBRARY NAMES TrajoptLib) +message(${TRAJOPT_LIBRARY}) + + +add_executable(trajectory_native src/trajectory_service.cpp) +target_link_libraries(trajectory_native PRIVATE ${TRAJOPT_LIBRARY} trajectory_native_proto fmt::fmt nlohmann_json::nlohmann_json) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile new file mode 100644 index 00000000..d8aadddc --- /dev/null +++ b/trajectory_native/Earthfile @@ -0,0 +1,77 @@ +VERSION 0.7 +FROM debian:bookworm-20240110 +WORKDIR /RobotCode2024/trajectory_native + +apt-deps: + ENV DEBIAN_FRONTEND=noninteractive + RUN apt update && apt install -y wget build-essential cmake autoconf libtool pkg-config git libblas-dev liblapack-dev clang lld gfortran + SAVE IMAGE --cache-hint + +grpc: + FROM +apt-deps + RUN git clone --recurse-submodules -b v1.60.0 --depth 1 --shallow-submodules https://github.com/grpc/grpc + RUN mkdir grpc/build + WORKDIR grpc/build + ENV CC=clang + ENV CXX=clang++ + RUN cmake -DgRPC_INSTALL=ON -DgRPC_BUILD_TESTS=OFF .. + RUN make -j4 + RUN make DESTDIR=$(pwd)/installroot_top install + RUN mkdir installroot + RUN mv installroot_top/usr/local/* installroot/ + SAVE ARTIFACT installroot + +ipopt: + FROM +apt-deps + RUN wget https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.14.tar.gz + RUN tar -zvxf 3.14.14.tar.gz + RUN mkdir Ipopt-releases-3.14.14/build + WORKDIR Ipopt-releases-3.14.14/build + ENV CC=clang + ENV CXX=clang++ + RUN ../configure + RUN make -j4 + RUN make DESTDIR=$(pwd)/installroot_top install + RUN mkdir installroot + RUN mv installroot_top/usr/local/* installroot/ + SAVE ARTIFACT installroot + +trajoptlib: + FROM +apt-deps + BUILD +ipopt + COPY +ipopt/installroot /usr/local/ + GIT CLONE https://github.com/camearle20/TrajoptLib TrajoptLib + RUN mkdir TrajoptLib/build + WORKDIR TrajoptLib/build + ENV CC=clang + ENV CXX=clang++ + RUN cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF .. + RUN make -j4 + RUN make DESTDIR=$(pwd)/installroot_top install + RUN mkdir installroot + RUN mv installroot_top/usr/local/* installroot/ + SAVE ARTIFACT installroot + +dev-image: + FROM +apt-deps + BUILD +grpc + BUILD +trajoptlib + COPY +grpc/installroot /usr/local/ + COPY +trajoptlib/installroot /usr/local/ + ENV CC=clang + ENV CXX=clang++ + SAVE IMAGE littletonrobotics/vts-dev + +vts: + FROM +dev-image + COPY src src + COPY proto proto + COPY CMakeLists.txt CMakeLists.txt + RUN mkdir build + WORKDIR build + RUN cmake .. + RUN make -j4 + EXPOSE 56328 + ENV GRPC_VERBOSITY=info + ENTRYPOINT ["./trajectory_native"] + SAVE IMAGE littletonrobotics/vts \ No newline at end of file diff --git a/trajectory_native/proto/VehicleTrajectoryService.proto b/trajectory_native/proto/VehicleTrajectoryService.proto new file mode 100644 index 00000000..f876bf6f --- /dev/null +++ b/trajectory_native/proto/VehicleTrajectoryService.proto @@ -0,0 +1,80 @@ +syntax = "proto3"; + +package org.littletonrobotics.vehicletrajectoryservice; + +message VehicleModel { + double mass = 1; + double moi = 2; + double vehicle_length = 3; + double vehicle_width = 4; + double wheel_radius = 5; + double max_wheel_omega = 6; + double max_wheel_torque = 7; +} + +message VehicleState { + double x = 1; + double y = 2; + double theta = 3; + double vx = 4; + double vy = 5; + double omega = 6; +} + +message TimestampedVehicleState { + double time = 1; + VehicleState state = 2; +} + +message VehicleVelocityConstraint { + double vx = 1; + double vy = 2; + double omega = 3; +} + +message ZeroVelocityConstraint { +} + +message Waypoint { + reserved 5 to 9; + double x = 1; + double y = 2; + optional double heading_constraint = 3; + optional uint32 samples = 4; + oneof velocity_constraint { + ZeroVelocityConstraint zero_velocity = 10; + VehicleVelocityConstraint vehicle_velocity = 11; + } +} + +message PathSegment { + repeated Waypoint waypoints = 1; + optional double max_velocity = 2; + optional double max_omega = 3; + bool straight_line = 4; +} + +message Trajectory { + repeated TimestampedVehicleState states = 1; + string hash_code = 2; +} + +message TrajectoryGenerationError { + string reason = 1; +} + +message PathRequest { + repeated PathSegment segments = 1; + VehicleModel model = 2; +} + +message TrajectoryResponse { + oneof response { + Trajectory trajectory = 1; + TrajectoryGenerationError error = 2; + } +} + +service VehicleTrajectoryService { + rpc GenerateTrajectory(PathRequest) returns (TrajectoryResponse) {} +} \ No newline at end of file diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp new file mode 100644 index 00000000..065e6bcc --- /dev/null +++ b/trajectory_native/src/trajectory_service.cpp @@ -0,0 +1,214 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vts = org::littletonrobotics::vehicletrajectoryservice; + +trajopt::SwerveDrivetrain create_drivetrain(const vts::VehicleModel &model) { + trajopt::SwerveDrivetrain drivetrain{ + .mass = model.mass(), + .moi = model.moi(), + .modules = {{.x = model.vehicle_length(), + .y = model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = model.vehicle_length(), + .y = -model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = -model.vehicle_length(), + .y = model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = -model.vehicle_length(), + .y = -model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}}}; + + return drivetrain; +} + +int guess_control_interval_count(const vts::Waypoint &waypoint, const vts::Waypoint &prev_waypoint, + const vts::VehicleModel &model) { + double dx = waypoint.x() - prev_waypoint.x(); + double dy = waypoint.y() - prev_waypoint.y(); + double dtheta = waypoint.has_heading_constraint() && prev_waypoint.has_heading_constraint() ? + waypoint.heading_constraint() - prev_waypoint.heading_constraint() : 0; + static const double heading_weight = 0.5; + double distance = hypot(dx, dy); + double max_force = model.max_wheel_torque() / model.wheel_radius(); + double max_accel = (max_force * 4) / model.mass(); + double max_vel = model.max_wheel_omega() * model.wheel_radius(); + double distance_at_cruise = distance - (max_vel * max_vel) / max_accel; + double total_time = distance_at_cruise < 0 ? + 2 * (sqrt(distance * max_accel) / max_accel) : + distance / max_vel + max_vel / max_accel; + total_time += heading_weight * abs(dtheta); + return ceil(total_time / 0.1); +} + +void convert_sample(vts::TimestampedVehicleState *state_out, const trajopt::HolonomicTrajectorySample &sample_in) { + state_out->set_time(sample_in.timestamp); + vts::VehicleState *vehicle_state = state_out->mutable_state(); + vehicle_state->set_x(sample_in.x); + vehicle_state->set_y(sample_in.y); + vehicle_state->set_theta(sample_in.heading); + vehicle_state->set_vx(sample_in.velocityX); + vehicle_state->set_vy(sample_in.velocityY); + vehicle_state->set_omega(sample_in.angularVelocity); +} + +void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::HolonomicTrajectory &trajectory_in) { + for (const trajopt::HolonomicTrajectorySample &sample: trajectory_in.samples) { + convert_sample(trajectory_out->add_states(), sample); + } +} + +class VehicleTrajectoryService final + : public vts::VehicleTrajectoryService::Service { +public: + ::grpc::Status GenerateTrajectory(::grpc::ServerContext *context, + const vts::PathRequest *request, + vts::TrajectoryResponse *response) override { + trajopt::SwervePathBuilder builder; + builder.SetDrivetrain(create_drivetrain(request->model())); + std::vector control_intervals; + + int segment_start_offset = 0; + for (int segment_idx = 0; segment_idx < request->segments_size(); segment_idx++) { + fmt::print("Starting segment {}\n", segment_idx); + // Add segment waypoints + const vts::PathSegment &segment = request->segments(segment_idx); + const int last_waypoint_idx = segment.waypoints_size() - 1; + const vts::Waypoint *prev_waypoint; + + for (int waypoint_idx = 0; waypoint_idx < segment.waypoints_size(); waypoint_idx++) { + const vts::Waypoint &waypoint = segment.waypoints(segment_start_offset + waypoint_idx); + + if (waypoint.has_heading_constraint()) { + fmt::print("Adding pose waypoint {} ({}, {}, {})\n", segment_start_offset + waypoint_idx, + waypoint.x(), waypoint.y(), waypoint.heading_constraint()); + builder.PoseWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y(), + waypoint.heading_constraint()); + } else { + fmt::print("Adding translation waypoint {} ({}, {})\n", segment_start_offset + waypoint_idx, + waypoint.x(), waypoint.y()); + builder.TranslationWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y()); + } + + switch (waypoint.velocity_constraint_case()) { + case vts::Waypoint::VELOCITY_CONSTRAINT_NOT_SET: + // If this is the first or last waypoint, add an implicit stop if no other constraint is added + if (waypoint_idx == 0 || waypoint_idx == last_waypoint_idx) { + fmt::print("Adding implicit zero velocity constraint to waypoint {}\n", + segment_start_offset + waypoint_idx); + builder.WptZeroVelocity(segment_start_offset + waypoint_idx); + } + break; + case vts::Waypoint::kZeroVelocity: + fmt::print("Adding zero velocity constraint to waypoint {}\n", + segment_start_offset + waypoint_idx); + builder.WptZeroVelocity(segment_start_offset + waypoint_idx); + break; + case vts::Waypoint::kVehicleVelocity: + fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n", + waypoint.vehicle_velocity().vx(), waypoint.vehicle_velocity().vy(), + waypoint.vehicle_velocity().omega(), segment_start_offset + waypoint_idx); + builder.WptConstraint(segment_start_offset + waypoint_idx, + trajopt::HolonomicVelocityConstraint{ + trajopt::RectangularSet2d{ + {waypoint.vehicle_velocity().vx(), + waypoint.vehicle_velocity().vx()}, + {waypoint.vehicle_velocity().vy(), + waypoint.vehicle_velocity().vy()}, + }, + trajopt::CoordinateSystem::kField + }); + break; + } + + if (segment_idx > 0 || waypoint_idx > 0) { + unsigned int sample_count = waypoint.has_samples() ? waypoint.samples() + : guess_control_interval_count(waypoint, + *prev_waypoint, + request->model()); + fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, waypoint_idx - 1, + waypoint_idx); + control_intervals.push_back(sample_count); + } + + prev_waypoint = &waypoint; + } + + // Segment constraints + if (segment.has_max_velocity()) { + fmt::print("Adding max velocity {} to segment {} (waypoints {}-{})\n", segment.max_velocity(), + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtVelocityMagnitude(segment_start_offset, segment_start_offset + last_waypoint_idx, + segment.max_velocity()); + } + + if (segment.has_max_omega()) { + fmt::print("Adding max omega {} to segment {} (waypoints {}-{})\n", segment.max_omega(), + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtConstraint(segment_start_offset, segment_start_offset + last_waypoint_idx, + trajopt::AngularVelocityConstraint{segment.max_omega()}); + } + + if (segment.straight_line()) { + double x1 = segment.waypoints(segment_start_offset).x(); + double x2 = segment.waypoints(segment_start_offset + last_waypoint_idx).x(); + double y1 = segment.waypoints(segment_start_offset).y(); + double y2 = segment.waypoints(segment_start_offset + last_waypoint_idx).y(); + double angle = atan2(y2 - y1, x2 - x1); + fmt::print("Adding straight line constraint with angle {} to segment {} (waypoints {}-{})\n", angle, + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtVelocityDirection(segment_start_offset, segment_start_offset + last_waypoint_idx, + angle); + } + + segment_start_offset = last_waypoint_idx; + } + + builder.ControlIntervalCounts(std::move(control_intervals)); + + try { + fmt::print("Generating trajectory\n"); + trajopt::HolonomicTrajectory trajectory{trajopt::OptimalTrajectoryGenerator::Generate(builder)}; + fmt::print("Generation finished\n"); + convert_trajectory(response->mutable_trajectory(), trajectory); + std::size_t hash = std::hash{}(request->SerializeAsString()); + response->mutable_trajectory()->set_hash_code(std::to_string(hash)); + } catch (std::exception &e) { + fmt::print("Generation failed: {}\n", std::string(e.what())); + response->mutable_error()->set_reason(std::string(e.what())); + } + + return grpc::Status::OK; + } +}; + +int main([[maybe_unused]] int argc, [[maybe_unused]] char **argv) { + VehicleTrajectoryService service; + + grpc::reflection::InitProtoReflectionServerBuilderPlugin(); + ::grpc::ServerBuilder builder; + builder.AddListeningPort("0.0.0.0:56328", grpc::InsecureServerCredentials()); + + builder.RegisterService(&service); + + std::unique_ptr server(builder.BuildAndStart()); + server->Wait(); + + return 0; +} \ No newline at end of file