diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/checkstyle-idea.xml b/.idea/checkstyle-idea.xml new file mode 100644 index 0000000..c370431 --- /dev/null +++ b/.idea/checkstyle-idea.xml @@ -0,0 +1,16 @@ + + + + 10.5.0 + JavaOnly + true + + + \ No newline at end of file diff --git a/.idea/compiler.xml b/.idea/compiler.xml new file mode 100644 index 0000000..fb7f4a8 --- /dev/null +++ b/.idea/compiler.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/gradle.xml b/.idea/gradle.xml new file mode 100644 index 0000000..ce1c62c --- /dev/null +++ b/.idea/gradle.xml @@ -0,0 +1,16 @@ + + + + + + + \ No newline at end of file diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml new file mode 100644 index 0000000..71cab4d --- /dev/null +++ b/.idea/jarRepositories.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..79e72b1 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/.idea/uiDesigner.xml b/.idea/uiDesigner.xml new file mode 100644 index 0000000..2b63946 --- /dev/null +++ b/.idea/uiDesigner.xml @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/pathplanner/deploy/pathplanner/pathtest.path b/pathplanner/deploy/pathplanner/pathtest.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/pathplanner/deploy/pathplanner/pathtest.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/pathplanner/settings.json b/pathplanner/settings.json new file mode 100644 index 0000000..8167cc8 --- /dev/null +++ b/pathplanner/settings.json @@ -0,0 +1,7 @@ +{ + "robotWidth": 0.7112, + "robotLength": 0.762, + "holonomicMode": false, + "generateJSON": false, + "generateCSV": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/TesterPath1.path b/src/main/deploy/pathplanner/TesterPath1.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/TesterPath1.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d4a3707..8334f08 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,9 @@ package frc.robot; +import com.pathplanner.lib.PathConstraints; +import edu.wpi.first.math.util.Units; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -26,6 +29,10 @@ public final class Constants { public static final int CLIMBER_LEFT = 12; public static final int CLIMBER_RIGHT = 13; + // path constraints + public static final PathConstraints DEFAULT_PATH_CONSTRAINTS = new PathConstraints(Units.inchesToMeters(48), Units.inchesToMeters(48)); + + // DIO public static final int LEFT_LIMIT_SWITCH = 1; public static final int RIGHT_LIMIT_SWITCH = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a6c32e1..fdb32b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,11 +7,10 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.autos.AutonomousFactory; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -43,6 +42,8 @@ public class RobotContainer { private final ShooterLookupTable m_shooterLookupTable = new ShooterLookupTable(); + private final AutonomousFactory m_autonomousFactory = new AutonomousFactory(m_drivetrainSubsystem, m_shooterSubsystem); + private final RunIntakeCommand m_intakeInCommand = new RunIntakeCommand(m_intakeSubsystem, 0.5, m_hopperSubsystem, m_towerSubsystem, 0.5); private final RunIntakeCommand m_intakeOutCommand = @@ -62,7 +63,7 @@ public RobotContainer() { m_shooterSubsystem.setDefaultCommand(new SetShooterSpeedCommand(m_shooterSubsystem, 0)); m_drivetrainSubsystem.setDefaultCommand(m_driveCommand); m_climberSubsystem.setDefaultCommand(new ClimberCommand(m_climberSubsystem, 0)); - ShuffleboardTab testCommands = Shuffleboard.getTab("test commands"); +// ShuffleboardTab testCommands = Shuffleboard.getTab("test commands"); // testCommands.add("ClimbUp", new ClimberCommand(m_climberSubsystem, 0.25)); // testCommands.add("ClimbDown", new ClimberCommand(m_climberSubsystem, -0.25)); @@ -86,6 +87,16 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + /* FOR SIENNA :) + moving drivetrain + intake out/in + shooter + kicker logic + conveyor up/down + climber up/down + kicker separately + */ + + // driver joystick m_operatorCommand.rightTrigger().whileTrue(m_intakeInCommand); @@ -111,6 +122,7 @@ private void configureButtonBindings() { .whileTrue(new TowerDownCommand(m_towerSubsystem)); } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -118,6 +130,6 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return null; + return m_autonomousFactory.getAutonomousCommand(); } } diff --git a/src/main/java/frc/robot/autos/AutonomousFactory.java b/src/main/java/frc/robot/autos/AutonomousFactory.java new file mode 100644 index 0000000..cd26c40 --- /dev/null +++ b/src/main/java/frc/robot/autos/AutonomousFactory.java @@ -0,0 +1,70 @@ +package frc.robot.autos; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.*; + +import java.util.HashMap; +import java.util.Map; + +public class AutonomousFactory{ + private static final AutonMode DEFAULT_MODE = AutonMode.SHOOT_4; + + private final SendableChooser m_chooseAutoOption; + + //MAKE SURE YOU INCLUDE DO NOTHING and also JUST SHOOTING + public enum AutonMode { + LINE_TEST, + CURVE_TEST, + S_CURVE_TEST, + SHOOT_LEAVE_COMMUNITY_1, + SHOOT_ENGAGE_4, + SHOOT_LEAVE_COMMUNITY_7, + SHOOT_LEAVE_COMMUNITY_4, + SHOOT_1, + SHOOT_4, + SHOOT_7 + + + } + + private final Map m_autoOptions = new HashMap<>(); + + //in parameters, pass in all subsystems you're going to use +public AutonomousFactory(DrivetrainSubsystem drivetrainSubsystem, ShooterSubsytem shooterSubsytem) { + m_chooseAutoOption = new SendableChooser<>(); + + //m_autoOptions.put(AutonMode.LINE_TEST, new lineTest(drivetrainSubsystem, "lineTest")); +// m_autoOptions.put(AutonMode.CURVE_TEST, new curveTest(drivetrainSubsystem, "curveTest")); +// m_autoOptions.put(AutonMode.S_CURVE_TEST, new SCurveTest(drivetrainSubsystem,"sCurveTest")); +// m_autoOptions.put(AutonMode.SHOOT_ENGAGE_4,new ShootEngage(drivetrainSubsystem, shooterSubsytem, "dockAndEngage4")); +// m_autoOptions.put(AutonMode.SHOOT_LEAVE_COMMUNITY_1,new ShootLeaveCommunity(drivetrainSubsystem,shooterSubsytem,"leaveCommunity1")); +// m_autoOptions.put(AutonMode.SHOOT_LEAVE_COMMUNITY_7,new ShootLeaveCommunity(drivetrainSubsystem,shooterSubsytem,"leaveCommunity7")); + m_autoOptions.put(AutonMode.SHOOT_4,new Shoot(shooterSubsytem)); + m_autoOptions.put(AutonMode.SHOOT_1,new Shoot(shooterSubsytem)); + m_autoOptions.put(AutonMode.SHOOT_7,new Shoot(shooterSubsytem)); + + for (AutonMode auto: AutonMode.values()) { + if (auto == DEFAULT_MODE) { + m_chooseAutoOption.setDefaultOption(auto.toString(), auto); + } + else { + m_chooseAutoOption.addOption(auto.toString(), auto); + } + } + SmartDashboard.putData("Auto Mode Select", m_chooseAutoOption); + } + + public Command getAutonomousCommand() { + AutonMode mode = m_chooseAutoOption.getSelected(); + return m_autoOptions.get(mode); + } + + + + + + + +} diff --git a/src/main/java/frc/robot/autos/ExampleAuto.java b/src/main/java/frc/robot/autos/ExampleAuto.java deleted file mode 100644 index 083d10c..0000000 --- a/src/main/java/frc/robot/autos/ExampleAuto.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ExampleSubsystem; - -/** An example command that uses an example subsystem. */ -public class ExampleAuto extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new TwoBallAuto. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleAuto(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/autos/SCurveTest.java b/src/main/java/frc/robot/autos/SCurveTest.java new file mode 100644 index 0000000..039600c --- /dev/null +++ b/src/main/java/frc/robot/autos/SCurveTest.java @@ -0,0 +1,20 @@ +package frc.robot.autos; + +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.subsystems.*; + +import java.util.HashMap; + +public class SCurveTest extends SequentialCommandGroup { + + public SCurveTest(DrivetrainSubsystem drivetrain, String path) { + PathPlannerTrajectory SCurveTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true); + Command SCurveTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(SCurveTest); + + addCommands(SCurveTest1); + } +} diff --git a/src/main/java/frc/robot/autos/Shoot.java b/src/main/java/frc/robot/autos/Shoot.java new file mode 100644 index 0000000..b4dbfbb --- /dev/null +++ b/src/main/java/frc/robot/autos/Shoot.java @@ -0,0 +1,11 @@ +package frc.robot.autos; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.ShooterPIDCommand; +import frc.robot.subsystems.ShooterSubsytem; + +public class Shoot extends SequentialCommandGroup { + public Shoot(ShooterSubsytem shooterSubsytem){ + addCommands(new ShooterPIDCommand(shooterSubsytem)); + } +} diff --git a/src/main/java/frc/robot/autos/ShootEngage.java b/src/main/java/frc/robot/autos/ShootEngage.java new file mode 100644 index 0000000..de98518 --- /dev/null +++ b/src/main/java/frc/robot/autos/ShootEngage.java @@ -0,0 +1,31 @@ +package frc.robot.autos; + +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.ShooterPIDCommand; +import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.subsystems.ShooterSubsytem; + +import java.util.HashMap; + +public class ShootEngage extends SequentialCommandGroup { + public ShootEngage(DrivetrainSubsystem drivetrain, ShooterSubsytem shooter, String path){ + PathPlannerTrajectory shootAndEngage = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true); + Command shootEngage = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(shootAndEngage); + + // STEP 1: shoot a ball + addCommands(new ShooterPIDCommand(shooter)); + + // STEP 2: move back onto charging station + addCommands(shootEngage); + + + + + } + + +} diff --git a/src/main/java/frc/robot/autos/ShootLeaveCommunity.java b/src/main/java/frc/robot/autos/ShootLeaveCommunity.java new file mode 100644 index 0000000..548530e --- /dev/null +++ b/src/main/java/frc/robot/autos/ShootLeaveCommunity.java @@ -0,0 +1,24 @@ +package frc.robot.autos; + +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.ShooterPIDCommand; +import frc.robot.subsystems.*; + +import java.util.HashMap; + +public class ShootLeaveCommunity extends SequentialCommandGroup { + //CONSTRUCTOR + public ShootLeaveCommunity(DrivetrainSubsystem drivetrain, ShooterSubsytem shooter, String path) { + PathPlannerTrajectory ShootLeaveCommunity = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true); + Command ShootAndLeaveCommunity = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(ShootLeaveCommunity); + //shoot a ball + addCommands(new ShooterPIDCommand(shooter)); + + //leave community + addCommands(ShootAndLeaveCommunity); + } +} diff --git a/src/main/java/frc/robot/autos/curveTest.java b/src/main/java/frc/robot/autos/curveTest.java new file mode 100644 index 0000000..a03eb78 --- /dev/null +++ b/src/main/java/frc/robot/autos/curveTest.java @@ -0,0 +1,19 @@ +package frc.robot.autos; + +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.subsystems.DrivetrainSubsystem; +import java.util.HashMap; + +public class curveTest extends SequentialCommandGroup { + public curveTest(DrivetrainSubsystem drivetrain, String path){ + PathPlannerTrajectory curveTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true); + Command curveTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(curveTest); + + // STEP 1: drive!! + addCommands(curveTest1); + } +} diff --git a/src/main/java/frc/robot/autos/lineTest.java b/src/main/java/frc/robot/autos/lineTest.java new file mode 100644 index 0000000..7dce922 --- /dev/null +++ b/src/main/java/frc/robot/autos/lineTest.java @@ -0,0 +1,21 @@ +package frc.robot.autos; + +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.subsystems.*; + +import java.util.HashMap; + +public class lineTest extends SequentialCommandGroup { + //CONSTRUCTOR + public lineTest(DrivetrainSubsystem drivetrain, String path) { + PathPlannerTrajectory LineTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true); + Command LineTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(LineTest); + // STEP 1: drive! + addCommands(LineTest1); + + } +} diff --git a/src/main/java/frc/robot/commands/DriveCommandWithPID.java b/src/main/java/frc/robot/commands/DriveCommandWithPID.java new file mode 100644 index 0000000..c401a6b --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommandWithPID.java @@ -0,0 +1,20 @@ +package frc.robot.commands; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class DriveCommandWithPID extends CommandBase { + DrivetrainSubsystem m_drivetrain; + + public DriveCommandWithPID(DrivetrainSubsystem drivetrainSubsystem) { + m_drivetrain = drivetrainSubsystem; + addRequirements(m_drivetrain); + } + + @Override + public void execute() { + m_drivetrain.smartVelocityControl(Units.inchesToMeters(48), Units.inchesToMeters(48)); + } + +} diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 315498f..b32ad1f 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -5,11 +5,13 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.RamseteAutoBuilder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; +import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -20,19 +22,23 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.TunableNumber; +import java.util.Map; +import java.util.function.Consumer; + // Drive train public class DrivetrainSubsystem extends SubsystemBase { - private CANSparkMax m_leftLeader = + private final CANSparkMax m_leftLeader = new CANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless); - private CANSparkMax m_leftFollower = + private final CANSparkMax m_leftFollower = new CANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless); - private CANSparkMax m_rightLeader = + private final CANSparkMax m_rightLeader = new CANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless); - private CANSparkMax m_rightFollower = + private final CANSparkMax m_rightFollower = new CANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless); private DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader); private DifferentialDriveKinematics m_kinematics = @@ -85,12 +91,18 @@ public DrivetrainSubsystem() { if (m_velocityP.hasChanged() || m_velocityI.hasChanged() || m_velocityD.hasChanged()) { updateDrivePID(); } + } public void control(double speed, double rotation) { m_drive.curvatureDrive(speed, rotation, Math.abs(speed) < 0.05); } + public void smartVelocityControl(double leftVelocity, double rightVelocity){ + m_leftController.setReference(leftVelocity, ControlType.kVelocity); + m_rightController.setReference(rightVelocity, ControlType.kVelocity); + } + public void applyChassisSpeed(ChassisSpeeds speeds) { DifferentialDriveWheelSpeeds wheelSpeeds = m_kinematics.toWheelSpeeds(speeds); applyWheelSpeed(wheelSpeeds); @@ -114,12 +126,39 @@ public void updateDrivePID() { m_rightController.setD(m_velocityD.get()); } + public double leftVelocity() { + return m_leftEncoder.getVelocity(); + } + + public double rightVelocity() { + return m_rightEncoder.getVelocity(); + } + + private RamseteAutoBuilder createRamseteAutoBuilder(Map eventMap, Consumer poseSetter) { + return new RamseteAutoBuilder( + this::getPose, // Pose supplier + poseSetter, + new RamseteController(), + m_kinematics, // DifferentialDriveKinematics + this::smartVelocityControl, // DifferentialDriveWheelSpeeds supplier + eventMap, + true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + this); + } + + public RamseteAutoBuilder ramseteAutoBuilderNoPoseReset(Map eventMap) { + return createRamseteAutoBuilder(eventMap, (Pose2d pose) -> {}); + } + @Override public void periodic() { // This method will be called once per scheduler run Rotation2d gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle()); m_odometry.update(gyroAngle, m_leftEncoder.getPosition(), m_rightEncoder.getPosition()); m_field.setRobotPose(m_odometry.getPoseMeters()); + SmartDashboard.putNumber("Chassis Left Velocity", leftVelocity()); + SmartDashboard.putNumber("Chassis Right Velocity", rightVelocity()); + } @Override diff --git a/src/main/java/org/littletonrobotics/FieldConstants.java b/src/main/java/org/littletonrobotics/FieldConstants.java new file mode 100644 index 0000000..526d6f2 --- /dev/null +++ b/src/main/java/org/littletonrobotics/FieldConstants.java @@ -0,0 +1,198 @@ +//// Copyright (c) 2023 FRC 6328 +//// http://github.com/Mechanical-Advantage +//// +//// Use of this source code is governed by an MIT-style +//// license that can be found in the LICENSE file at +//// the root directory of this project. +// +//package org.littletonrobotics; +// +//import edu.wpi.first.math.geometry.Pose2d; +//import edu.wpi.first.math.geometry.Translation2d; +//import edu.wpi.first.math.geometry.Translation3d; +//import edu.wpi.first.math.util.Units; +// +///** +// * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets +// * of corners start in the lower left moving clockwise. +// * +// *

All translations and poses are stored with the origin at the rightmost point on the BLUE +// * ALLIANCE wall. Use the {@link #allianceFlip(Translation2d)} and {@link #allianceFlip(Pose2d)} +// * methods to flip these values based on the current alliance color. +// */ +//public final class FieldConstants { +// public static final double FIELD_LENGTH = Units.inchesToMeters(651.25); +// public static final double FIELD_WIDTH = Units.inchesToMeters(315.5); +// public static final double TAPE_WIDTH = Units.inchesToMeters(2.0); +// public static final double APRIL_TAG_WIDTH = Units.inchesToMeters(6.0); +// +// // Dimensions for community and charging station, including the tape. +// public static final class Community { +// // Region dimensions +// public static final double INNER_X = 0.0; +// public static final double MID_X = +// Units.inchesToMeters(132.375); // Tape to the left of charging station +// public static final double OUTER_X = +// Units.inchesToMeters(193.25); // Tape to the right of charging station +// public static final double LEFT_Y = Units.feetToMeters(18.0); +// public static final double MID_Y = LEFT_Y - Units.inchesToMeters(59.39) + TAPE_WIDTH; +// public static final double RIGHT_Y = 0.0; +// public static final Translation2d[] REGION_CORNERS = { +// new Translation2d(INNER_X, RIGHT_Y), +// new Translation2d(INNER_X, LEFT_Y), +// new Translation2d(MID_X, LEFT_Y), +// new Translation2d(MID_X, MID_Y), +// new Translation2d(OUTER_X, MID_Y), +// new Translation2d(OUTER_X, RIGHT_Y), +// }; +// +// // Charging station dimensions +// public static final double CHARGING_STATION_LENGTH = Units.inchesToMeters(76.125); +// public static final double CHARGING_STATION_WIDTH = Units.inchesToMeters(97.25); +// public static final double CHARGING_STATION_OUTER_X = OUTER_X - TAPE_WIDTH; +// public static final double CHARGING_STATION_INNER_X = +// CHARGING_STATION_OUTER_X - CHARGING_STATION_LENGTH; +// public static final double CHARGING_STATION_LEFT_Y = MID_Y - TAPE_WIDTH; +// public static final double CHARGING_STATION_RIGHT_Y = CHARGING_STATION_LEFT_Y - CHARGING_STATION_WIDTH; +// public static final Translation2d[] CHARGING_STATION_CORNERS = { +// new Translation2d(CHARGING_STATION_INNER_X, CHARGING_STATION_RIGHT_Y), +// new Translation2d(CHARGING_STATION_INNER_X, CHARGING_STATION_LEFT_Y), +// new Translation2d(CHARGING_STATION_OUTER_X, CHARGING_STATION_RIGHT_Y), +// new Translation2d(CHARGING_STATION_OUTER_X, CHARGING_STATION_LEFT_Y) +// }; +// +// // Cable bump +// public static final double CABLE_BUMP_INNER_X = +// INNER_X + Grids.OUTER_X + Units.inchesToMeters(95.25); +// public static final double CABLE_BUMP_OUTER_X = CABLE_BUMP_INNER_X + Units.inchesToMeters(7); +// public static final Translation2d[] CABLE_BUMP_CORNERS = { +// new Translation2d(CABLE_BUMP_INNER_X, 0.0), +// new Translation2d(CABLE_BUMP_INNER_X, CHARGING_STATION_RIGHT_Y), +// new Translation2d(CABLE_BUMP_OUTER_X, 0.0), +// new Translation2d(CABLE_BUMP_OUTER_X, CHARGING_STATION_RIGHT_Y) +// }; +// } +// +// // Dimensions for grids and nodes +// public static final class Grids { +// // X layout +// public static final double OUTER_X = Units.inchesToMeters(54.25); +// public static final double LOW_X = +// OUTER_X - (Units.inchesToMeters(14.25) / 2.0); // Centered when under cube nodes +// public static final double MID_X = OUTER_X - Units.inchesToMeters(22.75); +// public static final double HIGH_X = OUTER_X - Units.inchesToMeters(39.75); +// +// // Y layout +// public static final int NODE_ROW_COUNT = 9; +// public static final double NODE_FIRST_Y = Units.inchesToMeters(20.19); +// public static final double NODE_SEPARATION_Y = Units.inchesToMeters(22.0); +// +// // Z layout +// public static final double CUBE_EDGE_HIGH = Units.inchesToMeters(3.0); +// public static final double HIGH_CUBE_Z = Units.inchesToMeters(35.5) - CUBE_EDGE_HIGH; +// public static final double MID_CUBE_Z = Units.inchesToMeters(23.5) - CUBE_EDGE_HIGH; +// public static final double HIGH_CONE_Z = Units.inchesToMeters(46.0); +// public static final double MID_CONE_Z = Units.inchesToMeters(34.0); +// +// // Translations (all nodes in the same column/row have the same X/Y coordinate) +// public static final Translation2d[] LOW_TRANSLATIONS = new Translation2d[NODE_ROW_COUNT]; +// public static final Translation2d[] MID_TRANSLATIONS = new Translation2d[NODE_ROW_COUNT]; +// public static final Translation3d[] MID_3D_TRANSLATIONS = new Translation3d[NODE_ROW_COUNT]; +// public static final Translation2d[] HIGH_TRANSLATIONS = new Translation2d[NODE_ROW_COUNT]; +// public static final Translation3d[] HIGH_3D_TRANSLATIONS = new Translation3d[NODE_ROW_COUNT]; +// +// static { +// for (int i = 0; i < NODE_ROW_COUNT; i++) { +// boolean isCube = i == 1 || i == 4 || i == 7; +// LOW_TRANSLATIONS[i] = new Translation2d(LOW_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i); +// MID_TRANSLATIONS[i] = new Translation2d(MID_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i); +// MID_3D_TRANSLATIONS[i] = +// new Translation3d(MID_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i, isCube ? MID_CUBE_Z : MID_CONE_Z); +// HIGH_3D_TRANSLATIONS[i] = +// new Translation3d( +// HIGH_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i, isCube ? HIGH_CUBE_Z : HIGH_CONE_Z); +// HIGH_TRANSLATIONS[i] = new Translation2d(HIGH_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i); +// } +// } +// +// // Complex low layout (shifted to account for cube vs cone rows and wide edge nodes) +// public static final double COMPLEX_LOW_X_CONES = +// OUTER_X - Units.inchesToMeters(16.0) / 2.0; // Centered X under cone nodes +// public static final double COMPLEX_LOW_X_CUBES = LOW_X; // Centered X under cube nodes +// public static final double COMPLEX_LOW_OUTER_Y_OFFSET = +// NODE_FIRST_Y - Units.inchesToMeters(3.0) - (Units.inchesToMeters(25.75) / 2.0); +// +// public static final Translation2d[] COMPLEX_LOW_TRANSLATIONS = { +// new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y - COMPLEX_LOW_OUTER_Y_OFFSET), +// new Translation2d(COMPLEX_LOW_X_CUBES, NODE_FIRST_Y + NODE_SEPARATION_Y * 1), +// new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 2), +// new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 3), +// new Translation2d(COMPLEX_LOW_X_CUBES, NODE_FIRST_Y + NODE_SEPARATION_Y * 4), +// new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 5), +// new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 6), +// new Translation2d(COMPLEX_LOW_X_CUBES, NODE_FIRST_Y + NODE_SEPARATION_Y * 7), +// new Translation2d( +// COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 8 + COMPLEX_LOW_OUTER_Y_OFFSET), +// }; +// } +// +// // Dimensions for loading zone and substations, including the tape +// public static final class LoadingZone { +// // Region dimensions +// public static final double WIDTH = Units.inchesToMeters(99.0); +// public static final double INNER_X = FieldConstants.FIELD_LENGTH; +// public static final double MID_X = FIELD_LENGTH - Units.inchesToMeters(132.25); +// public static final double OUTER_X = FIELD_LENGTH - Units.inchesToMeters(264.25); +// public static final double LEFT_Y = FieldConstants.FIELD_WIDTH; +// public static final double MID_Y = LEFT_Y - Units.inchesToMeters(50.5); +// public static final double RIGHT_Y = LEFT_Y - WIDTH; +// public static final Translation2d[] REGION_CORNERS = { +// new Translation2d( +// MID_X, RIGHT_Y), // Start at lower left next to border with opponent community +// new Translation2d(MID_X, MID_Y), +// new Translation2d(OUTER_X, MID_Y), +// new Translation2d(OUTER_X, LEFT_Y), +// new Translation2d(INNER_X, LEFT_Y), +// new Translation2d(INNER_X, RIGHT_Y), +// }; +// +// // Double substation dimensions +// public static final double DOUBLE_SUBSTATION_LENGTH = Units.inchesToMeters(14.0); +// public static final double DOUBLE_SUBSTATION_X = INNER_X - DOUBLE_SUBSTATION_LENGTH; +// public static final double DOUBLE_SUBSTATION_SHELF_Z = Units.inchesToMeters(37.375); +// public static final double DOUBLE_SUBSTATION_CENTER_Y = Units.inchesToMeters(265.74); +// +// // Single substation dimensions +// public static final double SINGLE_SUBSTATION_WIDTH = Units.inchesToMeters(22.75); +// public static final double SINGLE_SUBSTATION_LEFT_X = +// FieldConstants.FIELD_LENGTH - DOUBLE_SUBSTATION_LENGTH - Units.inchesToMeters(88.77); +// public static final double SINGLE_SUBSTATION_CENTER_X = +// SINGLE_SUBSTATION_LEFT_X + (SINGLE_SUBSTATION_WIDTH / 2.0); +// public static final double SINGLE_SUBSTATION_RIGHT_X = +// SINGLE_SUBSTATION_LEFT_X + SINGLE_SUBSTATION_WIDTH; +// public static final Translation2d SINGLE_SUBSTATION_TRANSLATION = +// new Translation2d(SINGLE_SUBSTATION_CENTER_X, LEFT_Y); +// +// public static final double SINGLE_SUBSTATION_HEIGHT = Units.inchesToMeters(18.0); +// public static final double SINGLE_SUBSTATION_LOW_Z = Units.inchesToMeters(27.125); +// public static final double SINGLE_SUBSTATION_CENTER_Z = +// SINGLE_SUBSTATION_LOW_Z + (SINGLE_SUBSTATION_HEIGHT / 2.0); +// public static final double SINGLE_SUBSTATION_HIGH_Z = +// SINGLE_SUBSTATION_LOW_Z + SINGLE_SUBSTATION_HEIGHT; +// } +// +// // Locations of staged game pieces +// public static final class StagingLocations { +// public static final double CENTER_OFFSET_X = Units.inchesToMeters(47.36); +// public static final double POSITION_X = FIELD_LENGTH / 2.0 - Units.inchesToMeters(47.36); +// public static final double FIRST_Y = Units.inchesToMeters(36.19); +// public static final double SEPARATION_Y = Units.inchesToMeters(48.0); +// public static final Translation2d[] TRANSLATIONS = new Translation2d[4]; +// +// static { +// for (int i = 0; i < TRANSLATIONS.length; i++) { +// TRANSLATIONS[i] = new Translation2d(POSITION_X, FIRST_Y + (i * SEPARATION_Y)); +// } +// } +// } +//} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..9e91343 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,35 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2023.4.4", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2023.4.4" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2023.4.4", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena" + ] + } + ] +}