From 56ab3e87dfc0a4b6b80b2ee82465f52cf854b199 Mon Sep 17 00:00:00 2001 From: 24cirinom <112493487+24cirinom@users.noreply.github.com> Date: Fri, 23 Feb 2024 17:07:33 -0600 Subject: [PATCH] absolute arm encoder + other stuff --- src/main/java/frc/robot/Constants.java | 9 +++++---- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/arm/ArmIOSparkMax.java | 7 ++++--- .../robot/subsystems/intake/IntakeIOSparkMax.java | 15 +++++++++++++-- 4 files changed, 23 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a492266..de9b114 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,7 +57,7 @@ public static final class FlywheelConstants { // CAN ID's public static final int kTopFlywheelSparkMaxCanId = 3; - public static final int kBottomFlywheelSparkMaxCanId = 14; + public static final int kBottomFlywheelSparkMaxCanId = 6; // TODO implement the below public static final int kCurrentLimit = 30; @@ -78,6 +78,7 @@ public static final class FlywheelConstants { public static final class IntakeConstants { public static final int kIntakeSparkMaxCanId = 7; + public static final int kFollowerIntakeSparkMaxCanId = 14; public static final int kCurrentLimit = 30; @@ -113,8 +114,8 @@ public static final class ArmConstants { public static final double ks = 0.10; // TODO TUNE public static final double kv = 0.05; // TODO TUNE - public static final double minimumAngle = -1.441; - public static final double maximumAngle = 1.441; + public static final double minimumAngle = -2 * 1.441; + public static final double maximumAngle = 2 * 1.441; // do not use // public static final double kFF = 0.0; @@ -149,7 +150,7 @@ public static final class Presets { } public static final class WristConstants { - public static final int kWristMotorId = 6; // ! Change before testing + public static final int kWristMotorId = 60; // ! Change before testing public static final int kCurrentLimit = 30; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5dcf7d8..6b3ef46 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ public class RobotContainer { private final ArmElevator sArmElevator; private final Wrist sWrist; private final Climber sClimber; -// private final Sticks sSticks; + // private final Sticks sSticks; // Controllers private final CommandXboxController driverController = new CommandXboxController(0); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index 9275569..b3b1748 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -28,7 +28,7 @@ * "CANSparkFlex". */ public class ArmIOSparkMax implements ArmIO { - private static final double GEAR_RATIO = 40.0 * (60.0 / 12.0); // May be reciprocal + private static final double GEAR_RATIO = 1; // May be reciprocal private final CANSparkMax leftMotor = new CANSparkMax(ArmConstants.kLeftArmMotorId, MotorType.kBrushless); @@ -63,6 +63,7 @@ public ArmIOSparkMax() { rightMotor.setSmartCurrentLimit(ArmConstants.kCurrentLimit); rightMotor.follow(leftMotor, true); + pid.setFeedbackDevice(leftMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)); // leftMotor.burnFlash(); // rightMotor.burnFlash(); @@ -70,9 +71,9 @@ public ArmIOSparkMax() { @Override public void updateInputs(ArmIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(leftRelativeEncoder.getPosition() / GEAR_RATIO); + inputs.positionRad = Units.rotationsToRadians(leftAbsoluteEncoder.getPosition() / GEAR_RATIO); inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(leftRelativeEncoder.getVelocity() / GEAR_RATIO); + Units.rotationsPerMinuteToRadiansPerSecond(leftAbsoluteEncoder.getVelocity() / GEAR_RATIO); inputs.appliedVolts = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage(); inputs.currentAmps = new double[] {leftMotor.getOutputCurrent()}; inputs.targetPositionRad = targetAngle; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 06d814a..3fedf8f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -31,21 +31,32 @@ public class IntakeIOSparkMax implements IntakeIO { 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.setInverted(true); intakeMotor.enableVoltageCompensation(12.0); intakeMotor.setSmartCurrentLimit(30); + followerIntakeMotor.enableVoltageCompensation(12.0); + followerIntakeMotor.setSmartCurrentLimit(30); + + followerIntakeMotor.follow(intakeMotor, false); intakeMotor.burnFlash(); + followerIntakeMotor.burnFlash(); } @Override @@ -65,7 +76,7 @@ public void setVoltage(double volts) { @Override public void setVelocity(double velocityRadPerSec, double ffVolts) { intakePID.setReference( - (6.0 / 458.0) * Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + (1.5 / 458.0) * Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, ControlType.kVoltage, 0, ffVolts,