diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/Indexer.java b/src/main/java/frc/robot/Indexer.java deleted file mode 100644 index 4742305..0000000 --- a/src/main/java/frc/robot/Indexer.java +++ /dev/null @@ -1,8 +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; - -/** Add your docs here. */ -public class Indexer {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d34ef8..9d8382e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,29 +2,15 @@ import com.ctre.phoenix6.swerve.SwerveRequest; -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.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.driver.DriverXbox; import frc.robot.drivetrain.CommandSwerveDrivetrain; import frc.robot.drivetrain.TunerConstants; -// import frc.robot.drivetrain2.Drivetrain; -// import frc.robot.drivetrain2.DrivetrainConfig; -// import frc.robot.drivetrain2.DrivetrainConfig.DriveConstants; -// import frc.robot.drivetrain2.commands.DrivetrainCommands; -import frc.robot.drivetrain.commands.DrivetrainCommands; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import frc.robot.subsystems.AlgaeSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -33,29 +19,21 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - + private final double kMaxVelocity = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private final double kMaxAngularVelocity = RotationsPerSecond.of(0.75).in(RadiansPerSecond); /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - //AutonMaster mAutonMaster = new AutonMaster(); - - // Gamepads - + .withDeadband(kMaxVelocity * 0.1) + .withRotationalDeadband(kMaxAngularVelocity * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - /* Subsystems */ - - /* Auton Chooser */ public static SendableChooser mAutonChooser; + private final AlgaeSubsystem algaeSubsystem = AlgaeSubsystem.getInstance(); + public RobotContainer() { - //mAutonChooser = mAutonMaster.getAutonSelector(); setDefaultCommands(); - - } /** @@ -69,28 +47,16 @@ public Command getAutonomousCommand() { public void setDefaultCommands() { final var driver = DriverXbox.getInstance(); - // final var driver = DriverXbox.getInstance(); - - // CommandSwerveDrivetrain.getInstance().setDefaultCommand( - // DrivetrainCommands.drive( - // driver.getDriveTranslation().getX() * MaxSpeed, - // driver.getDriveTranslation().getY() * MaxSpeed, - // driver.getDriveRotation() * MaxAngularRate - // ) - // ); - - CommandSwerveDrivetrain.getInstance().setDefaultCommand( - CommandSwerveDrivetrain.getInstance().applyRequest(() -> - drive.withVelocityX(driver.getDriveTranslation().getX() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(driver.getDriveTranslation().getY() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(driver.getDriveRotation() * MaxAngularRate) // Drive counterclockwise with negative X (left) - ) - ); - - // Drivetrain.getInstance().setDefaultCommand(DrivetrainCommands.drive( - // driver::getDriveTranslation, - // driver::getDriveRotation - // )); +// CommandSwerveDrivetrain.getInstance().setDefaultCommand( +// CommandSwerveDrivetrain.getInstance().applyRequest(() -> +// drive.withVelocityX(driver.getDriveTranslation().getX() * kMaxVelocity) // Drive forward with negative Y (forward) +// .withVelocityY(driver.getDriveTranslation().getY() * kMaxVelocity) // Drive left with negative X (left) +// .withRotationalRate(driver.getDriveRotation() * kMaxAngularVelocity) // Drive counterclockwise with negative X (left) +// ) +// ); + + //algaeSubsystem.setDefaultCommand(new AlgaeSubsystem.DefaultCommand(algaeSubsystem, () -> (driver.getRightX() + 1) / 2.0)); + algaeSubsystem.setDefaultCommand(new AlgaeSubsystem.DefaultCommand(algaeSubsystem, () -> 1.0)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/algaepivot/commands/AlgaePivotCommands.java b/src/main/java/frc/robot/algaepivot/commands/AlgaePivotCommands.java index 30fca32..6dd78fa 100644 --- a/src/main/java/frc/robot/algaepivot/commands/AlgaePivotCommands.java +++ b/src/main/java/frc/robot/algaepivot/commands/AlgaePivotCommands.java @@ -4,21 +4,15 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.AlgaeSubsystem; public class AlgaePivotCommands { - public static Command setAlgaePivotAngle(Supplier targetSupplier) { - return new SetAngleAlgaePivot(targetSupplier); + public static Command setAlgaePivotAngle(AlgaeSubsystem.State state) { + return new SetAngleAlgaePivot(state); } - public static Command setAlgaePivotAngle(SetAngleAlgaePivot.Preset state) { - return new SetAngleAlgaePivot(state.getRotation2d()); - } - - public static Command setAlgaePivotAngle(Rotation2d angle) { - return new SetAngleAlgaePivot(angle); - } - - public static Command setAlgaePivotAngle(Supplier velocitySupplier, boolean openLoop) { - return new SetAngleAlgaePivot(velocitySupplier, openLoop); - } + // public static Command setAlgaePivotAngle(Supplier velocitySupplier, boolean openLoop) { + // return new SetAngleAlgaePivot(velocitySupplier, openLoop); + // } + } diff --git a/src/main/java/frc/robot/algaepivot/commands/SetAngleAlgaePivot.java b/src/main/java/frc/robot/algaepivot/commands/SetAngleAlgaePivot.java index 9bebb09..a8fc7e6 100644 --- a/src/main/java/frc/robot/algaepivot/commands/SetAngleAlgaePivot.java +++ b/src/main/java/frc/robot/algaepivot/commands/SetAngleAlgaePivot.java @@ -7,68 +7,43 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.algaepivot.AlgaeShooterPivot; +import frc.robot.subsystems.AlgaeSubsystem; public class SetAngleAlgaePivot extends Command { - AlgaeShooterPivot mAlgaeShooterPivot; - Supplier targetSupplier; - boolean singleShot = true; + AlgaeSubsystem mAlgaeSubsystem; + AlgaeSubsystem.State targetState; - public enum Preset { - kZero(Rotation2d.fromDegrees(2)), - kStowed(Rotation2d.fromDegrees(0)), - kAlgaeL2(Rotation2d.fromDegrees(0)), - kAlgaeL3(Rotation2d.fromDegrees(0)), - kCoralL1(Rotation2d.fromDegrees(0)), - kCoralL2(Rotation2d.fromDegrees(5)), - kCoralL3(Rotation2d.fromDegrees(0)), - kCoralL4(Rotation2d.fromDegrees(22.25)), - kAlgaeShoot(Rotation2d.fromDegrees(0)); + // public enum State { + // kFloorIntake(Rotation2d.fromRotations(-0.04)), + // kReefIntake(Rotation2d.fromRotations(0)), + // kScore(Rotation2d.fromRotations(0.15)), + // kStow(Rotation2d.fromRotations(0.23)); - Rotation2d target; + // State(Rotation2d pos) { + // this.pos = pos; + // } + // public final Rotation2d pos; + - Preset(Rotation2d target) { - this.target = target; - } + // public double getDegrees() { + // return pos.getDegrees(); + // } - public double getDegrees() { - return target.getDegrees(); - } + // public Rotation2d getRotation2d() { + // return pos; + // } + // } - public Rotation2d getRotation2d() { - return target; - } - } - - public SetAngleAlgaePivot(Rotation2d angleIn) { - mAlgaeShooterPivot = AlgaeShooterPivot.getInstance(); - targetSupplier = ()-> angleIn; - } - public SetAngleAlgaePivot(Supplier angleIn) { - mAlgaeShooterPivot = AlgaeShooterPivot.getInstance(); - targetSupplier = angleIn; - } - - public SetAngleAlgaePivot(Supplier angleIn, boolean singleSet) { - mAlgaeShooterPivot = AlgaeShooterPivot.getInstance(); - targetSupplier = angleIn; - singleShot = singleSet; + public SetAngleAlgaePivot(AlgaeSubsystem.State targetState) { + mAlgaeSubsystem = AlgaeSubsystem.getInstance(); + this.targetState = targetState; + addRequirements(mAlgaeSubsystem); } @Override public void initialize() { - - - } - - @Override - public void execute() { - mAlgaeShooterPivot.setTargetAngle(targetSupplier.get()); - } - + mAlgaeSubsystem.setTargetPosition(targetState.pos); - @Override - public boolean isFinished() { - return singleShot; } @Override diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java index 36aad5f..7aac1b5 100644 --- a/src/main/java/frc/robot/driver/DriverXbox.java +++ b/src/main/java/frc/robot/driver/DriverXbox.java @@ -1,6 +1,7 @@ package frc.robot.driver; import java.util.Optional; +import java.util.Set; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -17,8 +18,11 @@ import frc.robot.algaeflywheel.AlgaeFlyWheel; import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands; import frc.robot.algaeflywheel.commands.SetVelocityAlgaeFlyWheel; +import frc.robot.algaepivot.commands.AlgaePivotCommands; +import frc.robot.algaepivot.commands.SetAngleAlgaePivot; import frc.robot.drivetrain2.Drivetrain; import frc.robot.indexer.commands.IndexerCommands; +import frc.robot.subsystems.AlgaeSubsystem; public class DriverXbox extends XboxGamepad { @@ -36,6 +40,7 @@ private static class Settings { private static ExpCurve rotationStickCurve; public boolean autoAim; private double reqAngularVel; + private static AlgaeSubsystem mAlgaeSubsystem; private DriverXbox() { @@ -51,6 +56,7 @@ public static DriverXbox getInstance() { } return mInstance; } + @Override public void setupTeleopButtons() { @@ -64,6 +70,13 @@ public void setupTeleopButtons() { // controller.b().whileTrue(IndexerCommands.setOutput(() ->0.10)); // controller.y().onTrue(new InstantCommand(() -> Drivetrain.getInstance().zeroHeading())); + + + controller.a().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kFloorIntake)); + controller.b().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kReefIntake)); + controller.x().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kScore)); + controller.y().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kStow)); + } @Override @@ -76,7 +89,7 @@ public Translation2d getDriveTranslation() { final var xComponent = translationStickCurve.calculate(controller.getLeftX()); final var yComponent = translationStickCurve.calculate(controller.getLeftY()); // Components are reversed because field coordinates are opposite of joystick coordinates - // System.out.println("Colin Is GAY!!! " + new Translation2d(yComponent, xComponent).toString()); + return new Translation2d(yComponent, xComponent); } @@ -88,4 +101,6 @@ public void setDriveRotation(double requestedAngularVel) { public double getDriveRotation() { return rotationStickCurve.calculate(-controller.getRightX()); } + + public double getRightX() { return controller.getRightX(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem.java new file mode 100644 index 0000000..bf4169f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem.java @@ -0,0 +1,147 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import java.util.function.Supplier; + +public class AlgaeSubsystem extends SubsystemBase { + public static class Settings { + static final int kTalonPivotID = 14; + static final int kCANcoderPivotID = 25; + + static final double kG = 0.19; // V + static final double kS = 0.0; // V / rad + static final double kV = 0; // V * sec / rad + static final double kA = 0; // V * sec^2 / rad +// static final double kV = 1.77; // V * sec / rad +// static final double kA = 0.01; // V * sec^2 / rad + + static final Rotation2d kMaxVelocity = Rotation2d.fromDegrees(300); + static final Rotation2d kMaxAcceleration = Rotation2d.fromDegrees(600); + static final double kP = 15.0; + static final double kI = 0.0; + static final double kD = 0; + + static final double kZeroOffset = 0.3225; // rotations + + // TODO: Enable lower min-pos to bring down CoG when elevator is up. We should be able to tuck the shooter into the elevator. + static final Rotation2d kMinPos = Rotation2d.fromRotations(-0.04); + static final Rotation2d kMaxPos = Rotation2d.fromRotations(0.23); + } + + public enum State { + kFloorIntake(Settings.kMinPos), + kReefIntake(Rotation2d.fromRotations(0)), + kScore(Rotation2d.fromRotations(0.15)), + kStow(Rotation2d.fromRotations(0.18)), + kTuck(Settings.kMaxPos); + + State(Rotation2d pos) { + this.pos = pos; + } + public final Rotation2d pos; + } + + private final TalonFX mTalonPivot; + private final CANcoder mCANcoderPivot; + private final ArmFeedforward mFFController; + private final ProfiledPIDController mPPIDController; + + private AlgaeSubsystem() { + mTalonPivot = new TalonFX(Settings.kTalonPivotID); + mTalonPivot.getConfigurator().apply(new TalonFXConfiguration().withMotorOutput(new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + )); + + mCANcoderPivot = new CANcoder(Settings.kCANcoderPivotID); + mCANcoderPivot.getConfigurator().apply(new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs(). + withSensorDirection(SensorDirectionValue.Clockwise_Positive). + withMagnetOffset(Settings.kZeroOffset) + )); + + mFFController = new ArmFeedforward(Settings.kS, Settings.kG, Settings.kV, Settings.kA); + mPPIDController = new ProfiledPIDController(Settings.kP, Settings.kI, Settings.kD, new TrapezoidProfile.Constraints( + Settings.kMaxVelocity.getRadians(), + Settings.kMaxAcceleration.getRadians() + )); + } + + private static AlgaeSubsystem mInstance; + public static AlgaeSubsystem getInstance() { + if (mInstance == null) { + mInstance = new AlgaeSubsystem(); + } + return mInstance; + } + + public void setTargetState(State targetState) { + setTargetPosition(targetState.pos); + } + + public void setTargetPosition(Rotation2d targetPosition) { + // NOTE: Use radians for target goal to align with re:calc constant units + mPPIDController.setGoal(targetPosition.getRadians()); + } + + public Rotation2d getWristPosition() { + var pos = mCANcoderPivot.getAbsolutePosition().getValueAsDouble(); + return Rotation2d.fromRotations(pos); + } + + public Rotation2d getWristVelocity() { + var vel = mCANcoderPivot.getVelocity().getValueAsDouble(); + return Rotation2d.fromRotations(vel); + } + + @Override + public void periodic() { + var voltage = mPPIDController.calculate(getWristPosition().getRadians()); +// voltage += mFFController.calculate(getWristPosition().getRadians(), mPPIDController.getSetpoint().velocity); + mTalonPivot.setVoltage(voltage); + + // Telemetry + SmartDashboard.putNumber("Algae Pivot Pos (rotations)", getWristPosition().getRotations()); + SmartDashboard.putNumber("Algae Pivot Target Pos (rotations)", Rotation2d.fromRadians(mPPIDController.getSetpoint().position).getRotations()); + SmartDashboard.putNumber("Algae Pivot Vel (rotations / sec)", getWristVelocity().getRotations()); + SmartDashboard.putNumber("Algae Pivot Target Vel (rotations / sec)", Rotation2d.fromRadians(mPPIDController.getSetpoint().velocity).getRotations()); + SmartDashboard.putNumber("Algae Pivot Applied Voltage", voltage); + } + + public static class DefaultCommand extends Command { + private final Supplier percentSupplier; + private final AlgaeSubsystem subsystem; + + public DefaultCommand(AlgaeSubsystem subsystem, Supplier percentSupplier) { + this.percentSupplier = percentSupplier; + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void execute() { + var tp = getTargetPosition(); + subsystem.setTargetPosition(tp); + } + + private Rotation2d getTargetPosition() { + return Rotation2d.fromRotations((Settings.kMaxPos.getRotations() - Settings.kMinPos.getRotations()) * percentSupplier.get() + Settings.kMinPos.getRotations()); + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/CoralatorSubsystem.java b/src/main/java/frc/robot/subsystems/CoralatorSubsystem.java new file mode 100644 index 0000000..0bb4760 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CoralatorSubsystem.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class CoralatorSubsystem extends SubsystemBase { + public static class Settings { + + } + + private TalonFX mTalonArm, mTalonWrist, mTalonRoller; + private CANcoder mCANcoderArm, mCANcoderWrist; + + @Override + public void periodic() { + + } + + public void setWristTarget() { + + } + + public void setArmTarget() { + + } + + // Returns the angle of the wrist relative to the ground + public Rotation2d getWristPos() { + return null; + } + + // Returns the angle of the arm relative to the ground + public Rotation2d getArmPos() { + return null; + } + + // Returns the angle of the wrist relative to the arm. The wrist has a soft stop which moves based on its position + // relative to the arm - this function should be used to ensure we don't violate that constraint. + public Rotation2d getWristPosRelativeArm() { + return null; + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..03138a6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ElevatorSubsystem extends SubsystemBase { +}