From b4133c33a055f1b0de8bc4150603f43885f8ea96 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Fri, 1 Dec 2023 18:14:30 -0500 Subject: [PATCH] Set brake mode, add auton, add reset angle and bindings --- src/main/java/com/stuypulse/robot/Robot.java | 3 ++ .../com/stuypulse/robot/RobotContainer.java | 15 ++++++-- .../robot/commands/auton/EightFootAuton.java | 37 +++++++++++++++++++ .../swerve/SwerveDriveResetHeading.java | 22 +++++++++++ .../subsystems/swerve/SwerveModuleImpl.java | 4 ++ 5 files changed, 78 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/EightFootAuton.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d910ccb..88d9e30 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -6,6 +6,7 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.TeleopInit; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -72,6 +73,8 @@ public void disabledPeriodic() {} public void autonomousInit() { auto = robot.getAutonomousCommand(); + new SwerveDriveResetHeading().schedule(); + if (auto != null) { auto.schedule(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1cc68f0..1b6452d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,15 +7,18 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.DriveAndTurnBump; +import com.stuypulse.robot.commands.auton.EightFootAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.MobilityBump; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.swerve.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.BootlegXbox; import com.stuypulse.stuylib.input.Gamepad; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -58,15 +61,21 @@ private void configureDefaultCommands() { /*** BUTTONS ***/ /***************/ - private void configureButtonBindings() {} + private void configureButtonBindings() { + driver.getDPadUp().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180))); + driver.getDPadDown().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0))); + driver.getDPadLeft().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270))); + driver.getDPadRight().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90))); + } /**************/ /*** AUTONS ***/ /**************/ public void configureAutons() { - autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - autonChooser.addOption("Mobility", new Mobility()); + autonChooser.addOption("Do Nothing", new DoNothingAuton()); + autonChooser.addOption("8 Feet", new EightFootAuton()); + autonChooser.setDefaultOption("Mobility", new Mobility()); autonChooser.addOption("Mobility Bump", new MobilityBump()); autonChooser.addOption("Drive And Turn Bump", new DriveAndTurnBump()); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/EightFootAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/EightFootAuton.java new file mode 100644 index 0000000..ff212c8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/EightFootAuton.java @@ -0,0 +1,37 @@ +/************************ PROJECT PHIL ************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.auton; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import java.util.Arrays; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPoint; +import com.stuypulse.robot.subsystems.swerve.SwerveDriveFollowTrajectory; + +public class EightFootAuton extends SequentialCommandGroup { + private static final PathConstraints CONSTRAINTS = new PathConstraints(0.01, 0.01); + private static final PathPlannerTrajectory PATH = PathPlanner.generatePath(CONSTRAINTS, Arrays.asList( + new PathPoint(new Translation2d(), new Rotation2d(), new Rotation2d()), + new PathPoint(new Translation2d(0, 0), new Rotation2d(), new Rotation2d()) + )); + + public EightFootAuton() { + addCommands( + new SwerveDriveFollowTrajectory(PATH) + .robotRelative() + .withStop() + ); + } + + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java new file mode 100644 index 0000000..56a0c66 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.Odometry; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SwerveDriveResetHeading extends InstantCommand { + + public SwerveDriveResetHeading(Rotation2d heading) { + super(() -> { + Odometry o = Odometry.getInstance(); + o.reset(new Pose2d(o.getTranslation(), heading)); + }); + } + + public SwerveDriveResetHeading() { + this(new Rotation2d()); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java index cac0adb..e56803f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java @@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax.IdleMode; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.MatchState; import com.stuypulse.robot.constants.Settings.Swerve; @@ -52,6 +53,9 @@ public SwerveModuleImpl(String id, Translation2d translationOffset, Rotation2d a turnMotor = new CANSparkMax(turnID, MotorType.kBrushless); driveMotor = new CANSparkMax(driveID, MotorType.kBrushless); + turnMotor.setIdleMode(IdleMode.kBrake); + driveMotor.setIdleMode(IdleMode.kBrake); + turnEncoder = new CANCoder(encoderID); driveEncoder = driveMotor.getEncoder();