diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8764184..cef5f41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIORealVortex; import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.CANCoderTest; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.arm.ArmIOSim; @@ -36,6 +37,7 @@ public RobotContainer() throws IOException, ParseException { elevator = Elevator.initialize(new ElevatorIOReal()); intake = Intake.initialize(new IntakeIORealVortex()); Arm.initialize(new ArmIOReal()); + CANCoderTest canCoderTest = new CANCoderTest(); // drive = new Drive( // new GyroIOPigeon(), @@ -162,25 +164,27 @@ private void configureBindings() { // () -> Arm.getInstance().manualVoltage(0), // Arm.getInstance() // )); - OIConstants.driverController.leftTrigger().whileTrue( + OIConstants.driverController.y().whileTrue( Commands.run( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + () -> Arm.getInstance().manualVoltage( 6), Arm.getInstance() ) ); - OIConstants.driverController.rightTrigger().whileTrue( + // up + OIConstants.driverController.a().whileTrue( Commands.run( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * -6 + ArmConstants.DEFAULTkG), + () -> Arm.getInstance().manualVoltage(-6), Arm.getInstance() ) ); - OIConstants.driverController.leftBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L1)); + OIConstants.driverController.leftBumper().onTrue(Arm.getInstance().setGoal(0)); OIConstants.driverController.rightBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L2)); } public Command getAutonomousCommand() { - return Commands.runOnce(() -> elevator.setVoltage(ElevatorConstants.kg)); + return Commands.sequence( + ); } } diff --git a/src/main/java/frc/robot/subsystems/CANCoderTest.java b/src/main/java/frc/robot/subsystems/CANCoderTest.java new file mode 100644 index 0000000..637e484 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CANCoderTest.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class CANCoderTest extends SubsystemBase { + private CANcoder encoder = new CANcoder(19); + @Override + public void periodic() { + Logger.recordOutput("CANCoder Angle", encoder.getAbsolutePosition().getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 2e7772b..f9c5716 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -51,6 +51,7 @@ public void resetAbsoluteEncoder() { } public void manualVoltage(double voltage) { + Logger.recordOutput("isHolding", false); setOpenLoop(true); io.setVoltage(voltage); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 27ef5f4..1ca28ef 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -11,6 +11,8 @@ public static class ArmIOInputs{ public double goalAngle = 0.0; public boolean openLoop = true; public double setpointVelocity = 0.0; + public double appliedOutput = 0.0; + public double busVoltage = 0.0; } public default void updateInputs(ArmIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 6f17569..58cf60b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkClosedLoopController; @@ -35,16 +37,20 @@ public ArmIOReal(){ config.absoluteEncoder.velocityConversionFactor(Math.PI / 60); config.absoluteEncoder.inverted(false); - config.softLimit.forwardSoftLimitEnabled(true); - config.softLimit.reverseSoftLimitEnabled(false); - config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); - config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); + config.openLoopRampRate(1); + + // config.softLimit.forwardSoftLimitEnabled(true); + // config.softLimit.reverseSoftLimitEnabled(false); + // config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); + // config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); config.closedLoop.p(0.0); config.closedLoop.i(0.0); config.closedLoop.d(0.0); + config.inverted(true); + armMotor.clearFaults(); armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -63,8 +69,11 @@ public void setFFValues(double kS, double kG, double kV, double kA) { @Override public void hold() { - double ffvolts = ffmodel.calculate(armEncoder.getPosition(), 0); - onboardController.setReference(ffvolts, ControlType.kVoltage); + double ffvolts = ffmodel.calculate(getOffsetAngle(), 0); + Logger.recordOutput("arm ffvolts", ffvolts); + Logger.recordOutput("isHolding", true); + Logger.recordOutput("arm angle", getOffsetAngle()); + setVoltage(ffvolts); } @Override @@ -78,19 +87,26 @@ public void setVoltage(double voltage) { // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); } + public double getOffsetAngle() { + return armEncoder.getPosition() - ArmConstants.ARM_OFFSET; + } + @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity)); - double ffvolts = ffmodel.calculate(armEncoder.getPosition(), setpoint.velocity); + double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity); onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.angularPosition = armEncoder.getPosition(); + inputs.angularPosition = getOffsetAngle(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); inputs.voltage = armMotor.getAppliedOutput() * armMotor.getBusVoltage(); inputs.setpointVelocity = setpoint.velocity; + inputs.goalAngle = goal.position; + inputs.busVoltage = armMotor.getBusVoltage(); + inputs.appliedOutput = armMotor.getAppliedOutput(); } }