Testing changes 2/8/2025

Team-4795 · Feb 9, 2025 · 81b87e1 · 81b87e1
1 parent 845e83f
commit 81b87e1
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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();
     }
 }