diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c10edb4..7acc053 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,7 +112,7 @@ public RobotContainer() { new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); sFlywheel = new Flywheel(new FlywheelIOSparkMax()); - sIntake = new Intake(new IntakeIOSparkMax() {}); + sIntake = new Intake(); sArm = new Arm(new ArmIOSparkMax() {}); sArmElevator = new ArmElevator(new ArmElevatorIOSparkMax() {}); sWrist = new Wrist(new WristIOSparkMax() {}); @@ -120,28 +120,6 @@ public RobotContainer() { // sSticks = new Sticks(new SticksIOSparkMax() {}); break; - - case SIM: - // Sim robot, instantiate physics sim IO implementations - // Note that most of these are broken and useless, and I don't think we have time to fix - // them - sDrive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - sFlywheel = new Flywheel(new FlywheelIOSim()); - sIntake = new Intake(new IntakeIOSim() {}); - sArm = new Arm(new ArmIOSim() {}); - sArmElevator = new ArmElevator(new ArmElevatorIOSim() {}); - sWrist = new Wrist(new WristIOSim() {}); - sClimber = new Climber(new ClimberIOSim() {}); - // sSticks = new Sticks(new SticksIOSim() {}); - - break; - default: // Replayed robot, disable IO implementations sDrive = @@ -152,7 +130,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); sFlywheel = new Flywheel(new FlywheelIO() {}); - sIntake = new Intake(new IntakeIO() {}); + sIntake = new Intake(); sArm = new Arm(new ArmIO() {}); sArmElevator = new ArmElevator(new ArmElevatorIO() {}); sWrist = new Wrist(new WristIO() {}); @@ -171,12 +149,7 @@ public RobotContainer() { .withTimeout(5.0)); NamedCommands.registerCommand( - "Run Intake", - Commands.startEnd( - () -> sIntake.runVelocity(Constants.IntakeConstants.velocity), - sIntake::stop, - sIntake) - .withTimeout(5.0)); + "Run Intake", Commands.startEnd(() -> sIntake.setPercent(0.5), sIntake::stop, sIntake)); // Build SmartDashboard auto chooser if (!AutoBuilder.isConfigured()) { @@ -291,13 +264,13 @@ private void configureButtonBindings() { // () -> operatorController.getLeftTriggerAxis(), // () -> operatorController.getRightTriggerAxis())); - sIntake.setDefaultCommand( - IntakeShooterControls.intakeShooterDrive( - sIntake, - sFlywheel, - () -> operatorController.getLeftTriggerAxis(), - () -> operatorController.getRightTriggerAxis(), - operatorController.leftBumper())); + // sIntake.setDefaultCommand( + // IntakeShooterControls.intakeShooterDrive( + // sIntake, + // sFlywheel, + // () -> operatorController.getLeftTriggerAxis(), + // () -> operatorController.getRightTriggerAxis(), + // operatorController.leftBumper())); operatorController .a() .whileTrue( diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java index 60f4b11..894fd63 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java +++ b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java @@ -42,7 +42,7 @@ public static Command intakeShooterDrive( if (Math.abs(flywheel.getVelocityRPMBottom()) > Math.abs(Constants.FlywheelConstants.shootingVelocity + 2000)) { - intake.runVelocity(Constants.IntakeConstants.velocity * 5); + intake.setPercent(.70); // ALEX } } else { @@ -50,7 +50,7 @@ public static Command intakeShooterDrive( double stickMagnitude = MathUtil.applyDeadband(leftTriggerSupplier.getAsDouble(), DEADBAND) + -MathUtil.applyDeadband(rightTriggerSupplier.getAsDouble(), DEADBAND); - intake.runVelocity(Constants.IntakeConstants.velocity * stickMagnitude); + intake.setPercent(0.70 * stickMagnitude); // ALEX } }, intake, diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 290e8ae..e54a0d1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,85 +1,35 @@ -// Copyright 2021-2024 FRC 6328, FRC 5829 -// 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.intake; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.EnvironmentalConstants; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; +import frc.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { - private final IntakeIO io; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; + // private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal - /** Creates a new Flywheel. */ - public Intake(IntakeIO io) { - this.io = io; + private final CANSparkMax intakeMotor = + new CANSparkMax(IntakeConstants.kIntakeSparkMaxCanId, MotorType.kBrushless); + private final CANSparkMax followerIntakeMotor = + new CANSparkMax(IntakeConstants.kFollowerIntakeSparkMaxCanId, MotorType.kBrushless); - io.configurePID( - Constants.IntakeConstants.kP, Constants.IntakeConstants.kI, Constants.IntakeConstants.kD); + public Intake() { + intakeMotor.restoreFactoryDefaults(); + followerIntakeMotor.restoreFactoryDefaults(); - switch (EnvironmentalConstants.currentMode) { - case REAL: - case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - break; - case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - break; - default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); - break; - } - } + intakeMotor.setInverted(false); - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); - } + followerIntakeMotor.follow(intakeMotor, false); - /** Run open loop at the specified voltage. */ - public void runVolts(double volts) { - io.setVoltage(volts); + intakeMotor.burnFlash(); + followerIntakeMotor.burnFlash(); } - /** Run closed loop at the specified velocity. */ - public void runVelocity(double velocityRPM) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); - - // Log flywheel setpoint - Logger.recordOutput("Intake/SetpointRPM", velocityRPM); + public void setPercent(double pct) { + intakeMotor.set(pct); } - /** Stops the flywheel. */ public void stop() { - io.stop(); - } - - /** Returns the current velocity in RPM. */ - @AutoLogOutput - public double getVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); - } - - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; + intakeMotor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 5c52077..7fb3725 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -13,10 +13,7 @@ package frc.robot.subsystems.intake; -import org.littletonrobotics.junction.AutoLog; - public interface IntakeIO { - @AutoLog public static class IntakeIOInputs { public double positionRad = 0.0; public double velocityRadPerSec = 0.0; @@ -28,7 +25,7 @@ public static class IntakeIOInputs { public default void updateInputs(IntakeIOInputs inputs) {} /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} + public default void setPercent(double volts) {} /** Run closed loop at the specified velocity. */ public default void setVelocity(double velocityRadPerSec, double ffVolts) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java deleted file mode 100644 index 80cbd5b..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021-2024 FRC 6328, FRC 5829 -// 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.intake; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; - -public class IntakeIOSim implements IntakeIO { - private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); - private PIDController pid = new PIDController(0.0, 0.0, 0.0); - - private boolean closedLoop = false; - private double ffVolts = 0.0; - private double appliedVolts = 0.0; - - @Override - public void updateInputs(IntakeIOInputs inputs) { - if (closedLoop) { - appliedVolts = - MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); - sim.setInputVoltage(appliedVolts); - } - - sim.update(0.02); - - inputs.positionRad = 0.0; - inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; - } - - @Override - public void setVoltage(double volts) { - closedLoop = false; - appliedVolts = 0.0; - sim.setInputVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - closedLoop = true; - pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; - } - - @Override - public void stop() { - setVoltage(0.0); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java deleted file mode 100644 index aa94bc1..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2021-2024 FRC 6328, FRC 5829 -// 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.intake; - -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants.IntakeConstants; - -/** - * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with - * "CANSparkFlex". - */ -public class IntakeIOSparkMax implements IntakeIO { - private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal - - private final CANSparkMax intakeMotor = - new CANSparkMax(IntakeConstants.kIntakeSparkMaxCanId, MotorType.kBrushless); - private final CANSparkMax followerIntakeMotor = - new CANSparkMax(IntakeConstants.kFollowerIntakeSparkMaxCanId, MotorType.kBrushless); - - private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); - private final RelativeEncoder followerIntakeEncoder = followerIntakeMotor.getEncoder(); - - private final SparkPIDController intakePID = intakeMotor.getPIDController(); - - public IntakeIOSparkMax() { - intakeMotor.restoreFactoryDefaults(); - followerIntakeMotor.restoreFactoryDefaults(); - - intakeMotor.setCANTimeout(250); - followerIntakeMotor.setCANTimeout(250); - - intakeMotor.setInverted(false); - - intakeMotor.enableVoltageCompensation(12.0); - intakeMotor.setSmartCurrentLimit(30); - followerIntakeMotor.enableVoltageCompensation(12.0); - followerIntakeMotor.setSmartCurrentLimit(30); - - followerIntakeMotor.follow(intakeMotor, false); - - intakeMotor.burnFlash(); - followerIntakeMotor.burnFlash(); - } - - @Override - public void updateInputs(IntakeIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(intakeEncoder.getPosition() / GEAR_RATIO); - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(intakeEncoder.getVelocity() / GEAR_RATIO); - inputs.appliedVolts = intakeMotor.getAppliedOutput() * intakeMotor.getBusVoltage(); - inputs.currentAmps = new double[] {intakeMotor.getOutputCurrent()}; - } - - @Override - public void setVoltage(double volts) { - intakeMotor.setVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - // intakePID.setReference( - // (6 / 458.0) * Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, - // ControlType.kPercent, - // 0, - // ffVolts, - // ArbFFUnits.kVoltage); - - intakeMotor.set( - (0.3 / 458.0) * Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO); - } - - @Override - public void stop() { - intakeMotor.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - intakePID.setP(kP, 0); - intakePID.setI(kI, 0); - intakePID.setD(kD, 0); - intakePID.setFF(0, 0); - } -}