From fb2c0ff866adb6eddc33c69f5bcdca8a94f19f2c Mon Sep 17 00:00:00 2001 From: Stu-PID Date: Wed, 27 Mar 2024 21:02:27 -0400 Subject: [PATCH 1/5] Feeder Current Limit Increase --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a2eed4e..e967a45 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,7 +62,7 @@ public static final class FeederConstants { public static final int kFeederRight = 22; // Speed Controller Speeds public static final double kFeederSpeed = 1; - public static final int kFeederCurrentLimit = 20; + public static final int kFeederCurrentLimit = 40; } public static final class IncrementerConstants { From 426025008313c565795f2cda25177dd111d4b15d Mon Sep 17 00:00:00 2001 From: Stu-PID Date: Wed, 27 Mar 2024 21:03:01 -0400 Subject: [PATCH 2/5] Climber No Longer Inverted --- src/main/java/frc/robot/subsystems/Climber.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 809d0a8..23dc82c 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -58,7 +58,7 @@ public Climber() { climber = new TalonFX(50); // configure motor(s) - climber.setInverted(true); + climber.setInverted(false); // Define what signals we need from the Talon(s) // BaseStatusSignal.setUpdateFrequencyForAll( From 5e5dca78e383e5b786d2b3a45a6e2b775581f18d Mon Sep 17 00:00:00 2001 From: Stu-PID Date: Wed, 27 Mar 2024 21:04:25 -0400 Subject: [PATCH 3/5] Code Formatter Changes Can't commit code before a build because it runs formatter that conflicts with VS Code version. --- src/main/java/frc/robot/subsystems/Feeder.java | 4 ++-- src/main/java/frc/robot/subsystems/Incrementer.java | 1 - src/main/java/frc/robot/subsystems/Intake.java | 5 +++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Feeder.java b/src/main/java/frc/robot/subsystems/Feeder.java index 5b0e791..07f1f92 100644 --- a/src/main/java/frc/robot/subsystems/Feeder.java +++ b/src/main/java/frc/robot/subsystems/Feeder.java @@ -5,7 +5,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -118,7 +117,8 @@ public void periodic() { if (currentTask == Task.TRANSFER) { // If the Feeder Sensor is Intaking and finds the Note go IDLE - if (!RobotContainer.m_topFeederSensor.get() || RobotContainer.m_topIncrementerSensor.get() + if (!RobotContainer.m_topFeederSensor.get() + || RobotContainer.m_topIncrementerSensor.get() || DriverStation.isDisabled()) { currentTask = Task.IDLE; System.out.println("Note Transfered"); diff --git a/src/main/java/frc/robot/subsystems/Incrementer.java b/src/main/java/frc/robot/subsystems/Incrementer.java index 0f53625..9255c5a 100644 --- a/src/main/java/frc/robot/subsystems/Incrementer.java +++ b/src/main/java/frc/robot/subsystems/Incrementer.java @@ -5,7 +5,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1d0caa3..0330f88 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -116,8 +116,9 @@ public void periodic() { setIntakeSpeed(currentTask.getSpeed()); // If the FeederSensor or IncrementerSensor is on got back to idle. - if ((RobotContainer.m_topFeederSensor.get() || RobotContainer.m_topIncrementerSensor.get() - || DriverStation.isDisabled()) + if ((RobotContainer.m_topFeederSensor.get() + || RobotContainer.m_topIncrementerSensor.get() + || DriverStation.isDisabled()) && DriverStation.isTeleop()) { currentTask = Task.IDLE; } From 7d0109cb028da1353beaa5daf900bd05f46fba14 Mon Sep 17 00:00:00 2001 From: Stu-PID Date: Wed, 27 Mar 2024 21:38:12 -0400 Subject: [PATCH 4/5] Format for settings.json --- .pathplanner/settings.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 5058db0..0ad39ab 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -16,4 +16,4 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 2.25 -} \ No newline at end of file +} From cb8ce0e01725a646ee9d01fd567de52bdceff1a9 Mon Sep 17 00:00:00 2001 From: Stu-PID Date: Thu, 28 Mar 2024 10:47:04 -0400 Subject: [PATCH 5/5] Tweaks and Adjustments to Mast --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/Mast.java | 23 ++++++++++---------- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e967a45..ff81b7f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static final class MastConstants { public static final int kMastRight = 41; // Speed Controller Speeds public static final double kMastSpeed = 1; - public static final int kMastCurrentLimit = 20; + public static final int kMastCurrentLimit = 30; } public static final class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dd12557..7edbe21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -222,8 +222,8 @@ private void configureButtonBindings() { DriveCommands.joystickDrive( drive, () -> (MathUtil.applyDeadband(-driverPad.getLeftY(), .07)), - () -> (MathUtil.applyDeadband(-driverPad.getLeftX() * .65, .07)), - () -> (MathUtil.applyDeadband(-driverPad.getRightX() * .65, .07)))); + () -> (MathUtil.applyDeadband(-driverPad.getLeftX() * .75, .07)), + () -> (MathUtil.applyDeadband(-driverPad.getRightX() * .75, .07)))); // Intake Note and Load into Launcher /* diff --git a/src/main/java/frc/robot/subsystems/Mast.java b/src/main/java/frc/robot/subsystems/Mast.java index 7390061..5ec474b 100644 --- a/src/main/java/frc/robot/subsystems/Mast.java +++ b/src/main/java/frc/robot/subsystems/Mast.java @@ -27,11 +27,11 @@ public class Mast extends SubsystemBase { public enum Task { INTAKING("Intaking", 35.0), LAUNCHSUB("Launch Subwoofer", 40.0), - LAUCNHKEY("Launch Key", 60.5), + LAUCNHKEY("Launch Key", 64.5), LAUCNHPASS("Launch Pass", 61.0), LAUNCHAUTO("Launch Auto", 6.0), PUTAMP("PutAmp", -26.0), // tested - PUTTRAP("PutTrap", 7.0), + PUTTRAP("PutTrap", 2.5), CLEARJAM("Clear", 40.0), CLIMBING("Climbing", -7.0), TESTING("Testing", 0.0), @@ -100,13 +100,13 @@ public Mast() { // rightEncAngle = 0.0; testingAngle = 0.0; - mastKp = 0.8; - mastKi = 0.002; - mastKd = 2.0; - double mastIWind = 100; + mastKp = 0.88; + mastKi = 0.00; + mastKd = 0.0; + double mastIWind = 150; mastFF = 0.0; - mastMaxPosOut = 0.3; - mastMaxNegOut = -0.3; + mastMaxPosOut = 0.45; + mastMaxNegOut = -0.45; mastSetpoint = 0.0; mastLeft = new CANSparkMax(kMastLeft, MotorType.kBrushless); @@ -202,10 +202,11 @@ public void periodic() { leftEncAngle = (relmastLeftEncoder.getPosition() * 360 / gearRatio); // + mastStartingAngleOffset; // rightEncAngle = - // (relmastRightEncoder.getPosition() * 360 / gearRatio); // + mastStartingAngleOffset; + // (relmastRightEncoder.getPosition() * 360 / gearRatio); // + + // mastStartingAngleOffset; // Remove backlash from Launcher by syncing constantly - if (counts == 50) { + if (counts == 40) { double ABStoRel = getAbsoluteEncoderDegrees() * 37.5 / 360; relmastLeftEncoder.setPosition(-ABStoRel); // relmastRightEncoder.setPosition(-ABStoRel); @@ -224,7 +225,7 @@ public void periodic() { // Logger.recordOutput("Mast/Right_Enc", rightEncAngle); Logger.recordOutput("Mast/Abs_Enc", getAbsoluteEncoderDegrees()); Logger.recordOutput("Mast/LeftMotorOutput", mastLeft.get()); - // Logger.recordOutput("Mast/RightMotorOutput", mastRight.get()); + Logger.recordOutput("Mast/Target", currentTask.getAngle()); Logger.recordOutput("Mast/Current_Tsk", currentTask.getTaskName()); }