From 791b7d5b760d1172a04622129b81b86b9b1dfe79 Mon Sep 17 00:00:00 2001 From: taj-maharj08 Date: Mon, 19 Feb 2024 11:07:42 -0500 Subject: [PATCH] add climber code --- build.gradle | 2 +- src/main/java/frc/robot/Robot.java | 87 ++++++++----------- src/main/java/frc/robot/Subsystems.java | 11 +++ src/main/java/frc/robot/climber/Climber.java | 38 ++++++++ .../java/frc/robot/climber/ClimberIO.java | 24 +++++ .../frc/robot/climber/ClimberIOSparkMAX.java | 50 +++++++++++ 6 files changed, 158 insertions(+), 54 deletions(-) create mode 100644 src/main/java/frc/robot/climber/Climber.java create mode 100644 src/main/java/frc/robot/climber/ClimberIO.java create mode 100644 src/main/java/frc/robot/climber/ClimberIOSparkMAX.java diff --git a/build.gradle b/build.gradle index 3ceb2d9..bca596a 100755 --- a/build.gradle +++ b/build.gradle @@ -141,7 +141,7 @@ gversion { } // Spotless formatting -// project.compileJava.dependsOn(spotlessApply) +project.compileJava.dependsOn(spotlessApply) spotless { java { target fileTree(".") { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cad5191..e0e4bb5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,7 +2,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -15,13 +14,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.arm.Arm; +import frc.robot.climber.Climber; import frc.robot.drivetrain.Drivetrain; import frc.robot.intake.Intake; import frc.robot.shooter.Shooter; import frc.robot.vision.Vision; - -import javax.naming.PartialResultException; - import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -38,6 +35,7 @@ public class Robot extends LoggedRobot { private final Intake m_intake = new Intake(); private final Shooter m_shooter = new Shooter(); private final Arm m_arm = new Arm(); + private final Climber m_climber; private final CommandXboxController m_driverController = new CommandXboxController(0); private final CommandXboxController m_operatorController = new CommandXboxController(1); @@ -71,15 +69,17 @@ public Robot() { case kReal: m_drivetrain = Subsystems.createTalonFXDrivetrain(); m_vision = Subsystems.createFourCameraVision(); + m_climber = Subsystems.createSparkMAXClimber(); break; case kSim: m_drivetrain = Subsystems.createTalonFXDrivetrain(); m_vision = Subsystems.createFourCameraVision(); + m_climber = Subsystems.createBlankClimber(); break; default: m_drivetrain = Subsystems.createBlankDrivetrain(); - m_vision = Subsystems.createBlankFourCameraVision(); + m_climber = Subsystems.createBlankClimber(); break; } m_vision.setVisionPoseConsumer(m_drivetrain.getVisionPoseConsumer()); @@ -87,7 +87,6 @@ public Robot() { m_drivetrain.joystickDrive( () -> -m_driverController.getLeftY(), () -> -m_driverController.getLeftX(), - // This needs to be getRawAxis(2) when using sim on a Mac () -> -m_driverController.getRightX())); // NamedCommands.registerCommand("stop", m_shooter.Stop().withTimeout(0.5)); // NamedCommands.registerCommand("sensorIntake", SensorIntake()); @@ -103,9 +102,8 @@ public Robot() { // NamedCommands.registerCommand("ArmDown", m_arm.goDown()); NamedCommands.registerCommand("goDownAuto", m_arm.goToDegSeq(10, 0, -2)); NamedCommands.registerCommand("2ndNoteShot", m_arm.goToDeg(10, 12)); - NamedCommands.registerCommand("Stop Intake Shooter Feeder", Commands.sequence( - m_intake.Stop(), - m_shooter.Stop())); + NamedCommands.registerCommand( + "Stop Intake Shooter Feeder", Commands.sequence(m_intake.Stop(), m_shooter.Stop())); NamedCommands.registerCommand("Arm Go Down", m_arm.goDown()); m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser", AutoBuilder.buildAutoChooser()); @@ -145,20 +143,17 @@ public Robot() { .onTrue(new ParallelCommandGroup(m_intake.intake(false), m_shooter.FeederOn()).repeatedly()) .onFalse( new ParallelRaceGroup( - m_shooter.PullBack(), m_intake.intake(speakerShot).withTimeout(0.25)).finallyDo(() -> m_shooter.Stop())); + m_shooter.PullBack(), m_intake.intake(speakerShot).withTimeout(0.25)) + .finallyDo(() -> m_shooter.Stop())); // ___________________OperatorController______________________\\ m_operatorController .leftTrigger() - .onTrue(Commands.parallel(m_intake.release(),m_arm.goToAmp(), m_shooter.ShooterOn())) - .onFalse(Commands.sequence( - m_arm.goDown(), - m_shooter.ShooterOff())); + .onTrue(Commands.parallel(m_intake.release(), m_arm.goToAmp(), m_shooter.ShooterOn())) + .onFalse(Commands.sequence(m_arm.goDown(), m_shooter.ShooterOff())); // prep for climb - m_operatorController.rightBumper() - .onTrue(m_arm.goToDegSeq(110, 0, 0)) - .onFalse(m_arm.goDown()); + m_operatorController.rightBumper().onTrue(m_arm.goToDegSeq(110, 0, 0)).onFalse(m_arm.goDown()); m_operatorController.rightTrigger().onTrue(m_arm.goDown()).onFalse(m_arm.goDown()); @@ -169,9 +164,7 @@ public Robot() { m_arm.goToDeg(20, 25), new InstantCommand(() -> speakerShot = true), m_shooter.ShooterOn())) - .onFalse(Commands.sequence( - m_arm.goDown(), - m_shooter.ShooterOff())); + .onFalse(Commands.sequence(m_arm.goDown(), m_shooter.ShooterOff())); // turn on the shooter wheels m_operatorController.y().onTrue(m_shooter.Shoot()); @@ -179,31 +172,27 @@ public Robot() { m_operatorController .leftBumper() .onTrue(new ParallelCommandGroup(m_arm.goToDeg(0, 20), m_shooter.Catch().repeatedly())) - .onFalse(new ParallelCommandGroup( m_arm.goDown(), m_shooter.Stop())); + .onFalse(new ParallelCommandGroup(m_arm.goDown(), m_shooter.Stop())); + + m_operatorController.povDown().whileTrue(m_climber.winchDown()); } public Command SensorIntake() { return new ParallelRaceGroup( - new RepeatCommand(m_intake.intake(speakerShot)), - new RepeatCommand(m_shooter.FeederOn()) - .until( - () -> { - return m_shooter.hasNote(); - })) - .andThen(Commands.sequence( - m_intake.Stop(), - m_shooter.FeederOff())); + new RepeatCommand(m_intake.intake(speakerShot)), + new RepeatCommand(m_shooter.FeederOn()) + .until( + () -> { + return m_shooter.hasNote(); + })) + .andThen(Commands.sequence(m_intake.Stop(), m_shooter.FeederOff())); } /** - * This function is called every robot packet, no matter the mode. Use this for - * items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and - * test. + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. * - *

- * This runs after the mode specific periodic functions, but before LiveWindow - * and` + *

This runs after the mode specific periodic functions, but before LiveWindow and` * SmartDashboard integrated updating. */ @Override @@ -224,36 +213,28 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() { - } + public void autonomousPeriodic() {} @Override - public void teleopInit() { - } + public void teleopInit() {} @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} @Override - public void disabledInit() { - } + public void disabledInit() {} @Override - public void disabledPeriodic() { - } + public void disabledPeriodic() {} @Override - public void testInit() { - } + public void testInit() {} @Override - public void testPeriodic() { - } + public void testPeriodic() {} @Override - public void simulationInit() { - } + public void simulationInit() {} @Override public void simulationPeriodic() { diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index 72ac92e..a8bb993 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -3,6 +3,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.climber.Climber; +import frc.robot.climber.ClimberIO; +import frc.robot.climber.ClimberIOSparkMAX; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.ImuIO; import frc.robot.drivetrain.ImuIOPigeon2; @@ -37,4 +40,12 @@ public static Vision createFourCameraVision() { public static Vision createBlankFourCameraVision() { return new Vision(new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, new VisionIO() {}); } + + public static Climber createSparkMAXClimber() { + return new Climber(new ClimberIOSparkMAX()); + } + + public static Climber createBlankClimber() { + return new Climber(new ClimberIO() {}); + } } diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java new file mode 100644 index 0000000..62ae80b --- /dev/null +++ b/src/main/java/frc/robot/climber/Climber.java @@ -0,0 +1,38 @@ +package frc.robot.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Climber extends SubsystemBase { + private final ClimberIO m_climberIO; + private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged(); + + public Climber(ClimberIO climberIO) { + m_climberIO = climberIO; + + setDefaultCommand(idle()); + } + + @Override + public void periodic() { + m_climberIO.updateInputs(m_inputs); + Logger.processInputs("ClimberInputs", m_inputs); + } + + public Command idle() { + return this.run( + () -> { + m_climberIO.setLeftDutyCycle(0.0); + m_climberIO.setRightDutyCycle(0.0); + }); + } + + public Command winchDown() { + return this.run( + () -> { + m_climberIO.setLeftDutyCycle(0.5); + m_climberIO.setRightDutyCycle(0.5); + }); + } +} diff --git a/src/main/java/frc/robot/climber/ClimberIO.java b/src/main/java/frc/robot/climber/ClimberIO.java new file mode 100644 index 0000000..5d7410a --- /dev/null +++ b/src/main/java/frc/robot/climber/ClimberIO.java @@ -0,0 +1,24 @@ +package frc.robot.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public class ClimberIOInputs { + public double leftWinchPositionRad = 0.0; + public double leftWinchVelocityRadPerSec = 0.0; + public double leftWinchAppliedVolts = 0.0; + public double leftWinchCurrentAmps = 0.0; + + public double rightWinchPositionRad = 0.0; + public double rightWinchVelocityRadPerSec = 0.0; + public double rightWinchAppliedVolts = 0.0; + public double rightWinchCurrentAmps = 0.0; + } + + public default void updateInputs(ClimberIOInputs inputs) {} + + public default void setLeftDutyCycle(double outputDutyCycle) {} + + public default void setRightDutyCycle(double outputDutyCycle) {} +} diff --git a/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java new file mode 100644 index 0000000..a44f7cf --- /dev/null +++ b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java @@ -0,0 +1,50 @@ +package frc.robot.climber; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.util.Units; + +public class ClimberIOSparkMAX implements ClimberIO { + + private final double kWinchReduction = 1.0; + private final CANSparkMax m_leftWinchMotor, m_rightWinchMotor; + private final RelativeEncoder m_leftWinchEncoder, m_rightWinchEncoder; + + public ClimberIOSparkMAX() { + + m_leftWinchMotor = new CANSparkMax(8, CANSparkMax.MotorType.kBrushless); + m_rightWinchMotor = new CANSparkMax(9, CANSparkMax.MotorType.kBrushless); + + m_leftWinchEncoder = m_leftWinchMotor.getEncoder(); + m_rightWinchEncoder = m_rightWinchMotor.getEncoder(); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + inputs.leftWinchPositionRad = + Units.rotationsToRadians(m_leftWinchEncoder.getPosition() / kWinchReduction); + inputs.leftWinchVelocityRadPerSec = + Units.rotationsToRadians(m_leftWinchEncoder.getVelocity() / kWinchReduction); + inputs.leftWinchAppliedVolts = + m_leftWinchMotor.getAppliedOutput() * m_leftWinchMotor.getBusVoltage(); + inputs.leftWinchCurrentAmps = m_leftWinchMotor.getOutputCurrent(); + + inputs.rightWinchPositionRad = + Units.rotationsToRadians(m_rightWinchEncoder.getPosition() / kWinchReduction); + inputs.rightWinchVelocityRadPerSec = + Units.rotationsToRadians(m_rightWinchEncoder.getVelocity() / kWinchReduction); + inputs.rightWinchAppliedVolts = + m_rightWinchMotor.getAppliedOutput() * m_rightWinchMotor.getBusVoltage(); + inputs.rightWinchCurrentAmps = m_rightWinchMotor.getOutputCurrent(); + } + + @Override + public void setLeftDutyCycle(double outputDutyCycle) { + m_leftWinchMotor.set(outputDutyCycle); + } + + @Override + public void setRightDutyCycle(double outputDutyCycle) { + m_rightWinchMotor.set(outputDutyCycle); + } +}