From f87a3bbec1b9b8bcd9d63717c9446f04fbb632a6 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 14 Feb 2025 18:12:27 -0800 Subject: [PATCH 01/26] initial commit --- .../java/org/carlmontrobotics/Constants.java | 5 ++ .../commands/CoralIntake.java | 46 +++++++++++++++++++ .../subsystems/CoralEffector.java | 33 +++++++++++++ 3 files changed, 84 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/commands/CoralIntake.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..ad0df68 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -12,4 +12,9 @@ public static final class Manipulator { public static final int port = 1; } } + public static final class CoralEffectorConstants{ + public final static int coralMotorPort = 1; + public final static int coralLimitSwitchPort = 0; + public final static int coralDistanceSensorPort = 2; + } } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java new file mode 100644 index 0000000..99f33cc --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -0,0 +1,46 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj2.command.Command; + +public class CoralIntake extends Command { + + public CoralIntake() { + // Use addRequirements() here to declare subsystem dependencies. + //addRequirements(this.CoralEffector = CoralEffector); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (CoralEffector.distanceSensorSees){ + if (CoralEffector.limitSwitchSees){ + CoralEffector.coralMotor.set(0.05); + CoralEffector.coralMotor.set(0.1); + } + else{ + CoralEffector.coralMotor.set(0); + } + + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java new file mode 100644 index 0000000..d9b33e7 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -0,0 +1,33 @@ + +package org.carlmontrobotics.subsystems; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.playingwithfusion.TimeOfFlight; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static org.carlmontrobotics.Constants.*; + +import org.carlmontrobotics.Constants.CoralEffectorConstants; + +public class CoralEffector extends SubsystemBase { + + public static SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.coralMotorPort, MotorType.kBrushless); + public static DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); + public static TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.coralDistanceSensorPort); + + public static boolean distanceSensorSees; + public static boolean limitSwitchSees; + + @Override + public void periodic() { + distanceSensorSees = distanceSensor.getRange() < 100; + limitSwitchSees = coralLimitSwitch.get(); + } +} \ No newline at end of file From 8e628b5c04a3d95e56a9cc7574df7fb699175d51 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 14 Feb 2025 19:02:04 -0800 Subject: [PATCH 02/26] h --- src/main/java/org/carlmontrobotics/RobotContainer.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 836869c..ccdb18f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,6 +9,10 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.commands.CoralIntake; +import org.carlmontrobotics.subsystems.CoralEffector; + //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -31,6 +35,8 @@ public class RobotContainer { //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.port); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); + public final CoralEffector coralEffector = new CoralEffector(); + public final CoralIntake coralIntake = new CoralIntake(); public RobotContainer() { From ad709ac05d11d4af4ef02e2d94f3c8e9005f79eb Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 15 Feb 2025 14:53:36 -0800 Subject: [PATCH 03/26] e --- .wpilib/wpilib_preferences.json | 6 +++++ .../java/org/carlmontrobotics/Constants.java | 2 +- src/main/java/org/carlmontrobotics/Robot.java | 3 ++- .../org/carlmontrobotics/RobotContainer.java | 4 ++-- .../commands/CoralIntake.java | 24 ++++++++++++------- .../subsystems/CoralEffector.java | 8 +++++-- 6 files changed, 33 insertions(+), 14 deletions(-) create mode 100644 .wpilib/wpilib_preferences.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..f0ee648 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "none", + "teamNumber": 199 +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ad0df68..13f66da 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -15,6 +15,6 @@ public static final class Manipulator { public static final class CoralEffectorConstants{ public final static int coralMotorPort = 1; public final static int coralLimitSwitchPort = 0; - public final static int coralDistanceSensorPort = 2; + public final static int coralDistanceSensorPort = 5; } } diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java index 0bd19d3..55e89f2 100644 --- a/src/main/java/org/carlmontrobotics/Robot.java +++ b/src/main/java/org/carlmontrobotics/Robot.java @@ -4,6 +4,8 @@ package org.carlmontrobotics; +import org.carlmontrobotics.commands.CoralIntake; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -17,7 +19,6 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; - @Override public void robotInit() { m_robotContainer = new RobotContainer(); diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index ccdb18f..0a7a069 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -16,7 +16,7 @@ //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; - +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -39,7 +39,7 @@ public class RobotContainer { public final CoralIntake coralIntake = new CoralIntake(); public RobotContainer() { - + SmartDashboard.putData("Coral Intake", new CoralIntake()); setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 99f33cc..332c9ba 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -2,35 +2,43 @@ import org.carlmontrobotics.subsystems.CoralEffector; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class CoralIntake extends Command { - + public static int spin; public CoralIntake() { // Use addRequirements() here to declare subsystem dependencies. - //addRequirements(this.CoralEffector = CoralEffector); + // addRequirements(this.CoralEffector = CoralEffector); } // Called when the command is initially scheduled. @Override public void initialize() { - } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { if (CoralEffector.distanceSensorSees){ + if (CoralEffector.limitSwitchSees){ - CoralEffector.coralMotor.set(0.05); - CoralEffector.coralMotor.set(0.1); + CoralEffector.coralMotor.set(0.08); + spin = 5; + } + else{ + CoralEffector.coralMotor.set(0.1); + spin = 10; + } } else{ CoralEffector.coralMotor.set(0); + spin = 0; } - - } - } + // CoralEffector.coralMotor.set(0.1); + // spin = 10; + SmartDashboard.putNumber("spin", spin); +} // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index d9b33e7..8e2d2c0 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static org.carlmontrobotics.Constants.*; @@ -27,7 +28,10 @@ public class CoralEffector extends SubsystemBase { @Override public void periodic() { - distanceSensorSees = distanceSensor.getRange() < 100; - limitSwitchSees = coralLimitSwitch.get(); + distanceSensorSees = distanceSensor.getRange() < 140; + limitSwitchSees = !coralLimitSwitch.get(); + SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); + SmartDashboard.putNumber("distance", distanceSensor.getRange()); + SmartDashboard.putBoolean("limit switch", limitSwitchSees); } } \ No newline at end of file From b1030bb7f8a8376d7854e5ee20a85ca0d216fc95 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Sat, 15 Feb 2025 15:10:47 -0800 Subject: [PATCH 04/26] code clean up --- src/main/java/org/carlmontrobotics/Constants.java | 3 +++ .../org/carlmontrobotics/commands/CoralIntake.java | 13 ++++++++----- .../carlmontrobotics/subsystems/CoralEffector.java | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 13f66da..2c232c6 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -16,5 +16,8 @@ public static final class CoralEffectorConstants{ public final static int coralMotorPort = 1; public final static int coralLimitSwitchPort = 0; public final static int coralDistanceSensorPort = 5; + public final static int coralDistanceSensorDistance = 140; + public final static double coralEffectorMotorFastSpeed = 0.12; + public final static double coralEffectorMotorSlowSpeed = 0.08; } } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 332c9ba..06416f8 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -1,12 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.CoralEffectorConstants; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import java.util.*; +import java.util.concurrent.TimeUnit; public class CoralIntake extends Command { - public static int spin; + public static double spin; public CoralIntake() { // Use addRequirements() here to declare subsystem dependencies. // addRequirements(this.CoralEffector = CoralEffector); @@ -23,12 +26,12 @@ public void execute() { if (CoralEffector.distanceSensorSees){ if (CoralEffector.limitSwitchSees){ - CoralEffector.coralMotor.set(0.08); - spin = 5; + CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorSlowSpeed); + spin = CoralEffectorConstants.coralEffectorMotorSlowSpeed; } else{ - CoralEffector.coralMotor.set(0.1); - spin = 10; + CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed); + spin = CoralEffectorConstants.coralEffectorMotorFastSpeed; } } else{ diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 8e2d2c0..84b310b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -28,7 +28,7 @@ public class CoralEffector extends SubsystemBase { @Override public void periodic() { - distanceSensorSees = distanceSensor.getRange() < 140; + distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.coralDistanceSensorDistance; limitSwitchSees = !coralLimitSwitch.get(); SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); SmartDashboard.putNumber("distance", distanceSensor.getRange()); From 00df00c87b02dcb334f3bb6e338dfbac57e3e40a Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 15 Feb 2025 16:23:23 -0800 Subject: [PATCH 05/26] adjusted speeds (note: it relies on always hitting the limit switch) --- src/main/java/org/carlmontrobotics/Constants.java | 6 +++--- src/main/java/org/carlmontrobotics/RobotContainer.java | 1 - 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 2c232c6..77dca07 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -16,8 +16,8 @@ public static final class CoralEffectorConstants{ public final static int coralMotorPort = 1; public final static int coralLimitSwitchPort = 0; public final static int coralDistanceSensorPort = 5; - public final static int coralDistanceSensorDistance = 140; - public final static double coralEffectorMotorFastSpeed = 0.12; - public final static double coralEffectorMotorSlowSpeed = 0.08; + public final static int coralDistanceSensorDistance = 150; + public final static double coralEffectorMotorFastSpeed = 0.06; + public final static double coralEffectorMotorSlowSpeed = 0.04; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 0a7a069..ea51437 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -36,7 +36,6 @@ public class RobotContainer { public final GenericHID driverController = new GenericHID(OI.Driver.port); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); public final CoralEffector coralEffector = new CoralEffector(); - public final CoralIntake coralIntake = new CoralIntake(); public RobotContainer() { SmartDashboard.putData("Coral Intake", new CoralIntake()); From 7c15e86a1edd528bda10efde3b92842f8caea037 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 16 Feb 2025 14:39:52 -0800 Subject: [PATCH 06/26] e --- .../java/org/carlmontrobotics/Constants.java | 5 ++-- .../org/carlmontrobotics/RobotContainer.java | 4 ++++ .../commands/CoralIntake.java | 24 +++++++++++++++---- .../subsystems/CoralEffector.java | 9 +++++++ 4 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 77dca07..db2f465 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -15,9 +15,10 @@ public static final class Manipulator { public static final class CoralEffectorConstants{ public final static int coralMotorPort = 1; public final static int coralLimitSwitchPort = 0; - public final static int coralDistanceSensorPort = 5; + public final static int coralDistanceSensorPort = 6; public final static int coralDistanceSensorDistance = 150; - public final static double coralEffectorMotorFastSpeed = 0.06; + public final static double coralEffectorMotorFastSpeed = 0.09; + public final static double coralEffectorMotorFastSpeed2 = 0.05; public final static double coralEffectorMotorSlowSpeed = 0.04; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index ea51437..2cc73b1 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -94,4 +94,8 @@ private double inputProcessing(double value) { private double ProcessedAxisValue(GenericHID hid, Axis axis){ return inputProcessing(getStickValue(hid, axis)); } + public Command CoralIntake(){ + return new SequentialCommandGroup(new CoralIntake()); + + } } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 06416f8..fc25e61 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -3,13 +3,17 @@ import org.carlmontrobotics.Constants.CoralEffectorConstants; import org.carlmontrobotics.subsystems.CoralEffector; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.*; -import java.util.concurrent.TimeUnit; +import edu.wpi.first.wpilibj2.command.Command;; public class CoralIntake extends Command { public static double spin; + public static boolean fast2 = false; + Timer timer = new Timer(); + Timer timer2 = new Timer(); + public static boolean coralIn; + Timer coralInTimer = new Timer(); public CoralIntake() { // Use addRequirements() here to declare subsystem dependencies. // addRequirements(this.CoralEffector = CoralEffector); @@ -24,23 +28,33 @@ public void initialize() { @Override public void execute() { if (CoralEffector.distanceSensorSees){ - + coralIn = true; if (CoralEffector.limitSwitchSees){ CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorSlowSpeed); spin = CoralEffectorConstants.coralEffectorMotorSlowSpeed; } else{ + if (timer.get() < 0.15){ CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed); spin = CoralEffectorConstants.coralEffectorMotorFastSpeed; + } + else{ + CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed2); + spin = CoralEffectorConstants.coralEffectorMotorFastSpeed2; + } } } - else{ + else{ CoralEffector.coralMotor.set(0); spin = 0; + timer.restart(); } // CoralEffector.coralMotor.set(0.1); // spin = 10; SmartDashboard.putNumber("spin", spin); + SmartDashboard.putNumber("timer", timer.get()); + // SmartDashboard.getBoolean("outakeGet", coralIn); + SmartDashboard.putBoolean("coral in", coralIn); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 84b310b..d390b27 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -4,8 +4,11 @@ import org.carlmontrobotics.lib199.MotorControllerFactory; import com.playingwithfusion.TimeOfFlight; +import com.revrobotics.servohub.ServoHub.ResetMode; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; @@ -25,6 +28,12 @@ public class CoralEffector extends SubsystemBase { public static boolean distanceSensorSees; public static boolean limitSwitchSees; + + public CoralEffector(){ + // SparkFlexConfig config = new SparkFexConfig(); + // /*config.closedLoop.*/ + // coralMotor.configure(confi, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } @Override public void periodic() { From 2adf38926f32bb3757d2e130b21287574e52776b Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Sun, 16 Feb 2025 15:14:32 -0800 Subject: [PATCH 07/26] added pid --- .../subsystems/CoralEffector.java | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index d390b27..bae931a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -8,7 +8,10 @@ import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; @@ -20,6 +23,12 @@ import org.carlmontrobotics.Constants.CoralEffectorConstants; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkBase; + public class CoralEffector extends SubsystemBase { public static SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.coralMotorPort, MotorType.kBrushless); @@ -29,10 +38,19 @@ public class CoralEffector extends SubsystemBase { public static boolean distanceSensorSees; public static boolean limitSwitchSees; + SparkFlexConfig config = new SparkFlexConfig(); public CoralEffector(){ - // SparkFlexConfig config = new SparkFexConfig(); - // /*config.closedLoop.*/ - // coralMotor.configure(confi, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + config + .inverted(true) + .idleMode(IdleMode.kBrake); + config.encoder + .positionConversionFactor(1000) + .velocityConversionFactor(1000); + config.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(1.0, 0.0, 0.0); + + coralMotor.configure(config, SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override From d4b2fcd573b2f1109300e06b97b4612d1c1c8dda Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Thu, 20 Feb 2025 15:16:03 -0800 Subject: [PATCH 08/26] added pos. PID --- .../org/carlmontrobotics/RobotContainer.java | 8 +-- .../commands/CoralIntake.java | 69 +++++++++++-------- .../commands/SetCoralOut.java | 38 ++++++++++ .../subsystems/CoralEffector.java | 62 ++++++++++++----- 4 files changed, 127 insertions(+), 50 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/SetCoralOut.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 2cc73b1..6e8267a 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -11,6 +11,7 @@ import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.commands.CoralIntake; +import org.carlmontrobotics.commands.SetCoralOut; import org.carlmontrobotics.subsystems.CoralEffector; //controllers @@ -38,7 +39,8 @@ public class RobotContainer { public final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { - SmartDashboard.putData("Coral Intake", new CoralIntake()); + SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); + SmartDashboard.putData("coral out", new SetCoralOut(coralEffector)); setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); @@ -94,8 +96,4 @@ private double inputProcessing(double value) { private double ProcessedAxisValue(GenericHID hid, Axis axis){ return inputProcessing(getStickValue(hid, axis)); } - public Command CoralIntake(){ - return new SequentialCommandGroup(new CoralIntake()); - - } } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index fc25e61..e3ff86d 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -12,11 +12,14 @@ public class CoralIntake extends Command { public static boolean fast2 = false; Timer timer = new Timer(); Timer timer2 = new Timer(); - public static boolean coralIn; Timer coralInTimer = new Timer(); - public CoralIntake() { + public static double coralMotorPosition; + private CoralEffector coralEffector; + + public CoralIntake(CoralEffector coralEffector) { // Use addRequirements() here to declare subsystem dependencies. - // addRequirements(this.CoralEffector = CoralEffector); + // addRequirements(this.CoralEffector = CoralEffector); + addRequirements(this.coralEffector = coralEffector); } // Called when the command is initially scheduled. @@ -27,35 +30,43 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (CoralEffector.distanceSensorSees){ - coralIn = true; - if (CoralEffector.limitSwitchSees){ - CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorSlowSpeed); - spin = CoralEffectorConstants.coralEffectorMotorSlowSpeed; + // if (CoralEffector.distanceSensorSees) { + // coralIn = true; + // if (CoralEffector.limitSwitchSees) { + // CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorSlowSpeed); + // spin = CoralEffectorConstants.coralEffectorMotorSlowSpeed; + // } else { + // if (timer.get() < 0.15) { + // CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed); + // spin = CoralEffectorConstants.coralEffectorMotorFastSpeed; + // } else { + // CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed2); + // spin = CoralEffectorConstants.coralEffectorMotorFastSpeed2; + // } + // } + // } else { + // CoralEffector.coralMotor.set(0); + // spin = 0; + // timer.restart(); + // } + // CoralEffector.coralMotor.set(0.1); + // spin = 10; + SmartDashboard.putNumber("spin", spin); + SmartDashboard.putNumber("timer", timer.get()); + // SmartDashboard.getBoolean("outakeGet", coralIn);r + SmartDashboard.putBoolean("coral in", coralEffector.coralIsIn()); + if (CoralEffector.distanceSensorSees){ + coralEffector.setMotorSpeed(0.07); + coralMotorPosition = coralEffector.getEncoderPos(); //rotations + coralEffector.setCoralIn(true); } - else{ - if (timer.get() < 0.15){ - CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed); - spin = CoralEffectorConstants.coralEffectorMotorFastSpeed; - } - else{ - CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed2); - spin = CoralEffectorConstants.coralEffectorMotorFastSpeed2; - } + else if (coralEffector.coralIsIn()){ + coralEffector.setReferencePosition(coralMotorPosition - 0.1); //rotations + } + else { + coralEffector.setMotorSpeed(0); } } - else{ - CoralEffector.coralMotor.set(0); - spin = 0; - timer.restart(); - } - // CoralEffector.coralMotor.set(0.1); - // spin = 10; - SmartDashboard.putNumber("spin", spin); - SmartDashboard.putNumber("timer", timer.get()); - // SmartDashboard.getBoolean("outakeGet", coralIn); - SmartDashboard.putBoolean("coral in", coralIn); -} // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/org/carlmontrobotics/commands/SetCoralOut.java b/src/main/java/org/carlmontrobotics/commands/SetCoralOut.java new file mode 100644 index 0000000..9635599 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/SetCoralOut.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SetCoralOut extends Command { + /** Creates a new SetCoralOut. */ + private CoralEffector coralEffector; + public SetCoralOut(CoralEffector coralEffector) { + addRequirements(this.coralEffector = coralEffector); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + coralEffector.setCoralIn(false); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index bae931a..64b7378 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -4,8 +4,10 @@ import org.carlmontrobotics.lib199.MotorControllerFactory; import com.playingwithfusion.TimeOfFlight; +import com.revrobotics.RelativeEncoder; import com.revrobotics.servohub.ServoHub.ResetMode; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; @@ -13,6 +15,7 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -31,34 +34,61 @@ public class CoralEffector extends SubsystemBase { - public static SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.coralMotorPort, MotorType.kBrushless); - public static DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); - public static TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.coralDistanceSensorPort); - + public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.coralMotorPort, MotorType.kBrushless); + public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); + public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.coralDistanceSensorPort); + + public static double p = 0.5; public static boolean distanceSensorSees; public static boolean limitSwitchSees; + public final RelativeEncoder coralEncoder = coralMotor.getEncoder(); + private double CoralGoalRPM = 100; + private double coralOutput = coralMotor.getAppliedOutput(); + private boolean coralIn; SparkFlexConfig config = new SparkFlexConfig(); public CoralEffector(){ config - .inverted(true) + .inverted(false) .idleMode(IdleMode.kBrake); - config.encoder - .positionConversionFactor(1000) - .velocityConversionFactor(1000); config.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(1.0, 0.0, 0.0); + .pid(p, 0.0, 0.0); - coralMotor.configure(config, SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } + coralMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public double getEncoderPos() { + return coralEncoder.getPosition(); + } + public void setMotorSpeed(double speed) { + coralMotor.set(speed); + } + public void setReferencePosition(double reference) { + coralMotor.getClosedLoopController().setReference(reference, SparkBase.ControlType.kPosition); + } + + public boolean coralIsIn() { + return coralIn; + } + public void setCoralIn(boolean coralIsInside) { + coralIn = coralIsInside; + } @Override public void periodic() { - distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.coralDistanceSensorDistance; - limitSwitchSees = !coralLimitSwitch.get(); - SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); - SmartDashboard.putNumber("distance", distanceSensor.getRange()); - SmartDashboard.putBoolean("limit switch", limitSwitchSees); + //coralMotor.getClosedLoopController().setReference(1, ControlType.kVelocity); + distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.coralDistanceSensorDistance; + limitSwitchSees = !coralLimitSwitch.get(); + CoralGoalRPM = coralEncoder.getVelocity(); + coralOutput = coralMotor.getAppliedOutput(); + + SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); + SmartDashboard.putNumber("distance", distanceSensor.getRange()); + SmartDashboard.putBoolean("limit switch", limitSwitchSees); + SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); + SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); + SmartDashboard.putNumber("coral output", coralOutput); + SmartDashboard.getNumber("P", p); } } \ No newline at end of file From 2a678e4353a1641f074ff03cc470fe348441f204 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Thu, 20 Feb 2025 16:28:05 -0800 Subject: [PATCH 09/26] code clean up and added a controller --- .../java/org/carlmontrobotics/Constants.java | 14 +++++++++++--- .../org/carlmontrobotics/RobotContainer.java | 10 +++++++--- .../carlmontrobotics/commands/CoralIntake.java | 16 ++++++++-------- .../{SetCoralOut.java => CoralOutake.java} | 6 ++++-- .../subsystems/CoralEffector.java | 11 +++++------ 5 files changed, 35 insertions(+), 22 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{SetCoralOut.java => CoralOutake.java} (80%) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index db2f465..6fc3ffe 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,7 @@ package org.carlmontrobotics; +import edu.wpi.first.wpilibj.StadiaController.Button; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -10,6 +12,7 @@ public static final class Driver { } public static final class Manipulator { public static final int port = 1; + public static final int IntakeButton = Button.kA.value; } } public static final class CoralEffectorConstants{ @@ -17,8 +20,13 @@ public static final class CoralEffectorConstants{ public final static int coralLimitSwitchPort = 0; public final static int coralDistanceSensorPort = 6; public final static int coralDistanceSensorDistance = 150; - public final static double coralEffectorMotorFastSpeed = 0.09; - public final static double coralEffectorMotorFastSpeed2 = 0.05; - public final static double coralEffectorMotorSlowSpeed = 0.04; + public final static double coralEffectorMotorInputFastSpeed = 0.07; + // public final static double coralEffectorMotorInputFastSpeed2 = 0.05; + // public final static double coralEffectorMotorInputSlowSpeed = 0.04; + public final static double coralEffectorMotorOutputSpeed = 0.2; + public final static double coralEffectorDistanceSensorOffset = -0.1; + public final static double kp = 0.04; + public final static double ki = 0.0; + public final static double kd = 0.0; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 6e8267a..c7f40f5 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -11,7 +11,7 @@ import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.commands.CoralIntake; -import org.carlmontrobotics.commands.SetCoralOut; +import org.carlmontrobotics.commands.CoralOutake; import org.carlmontrobotics.subsystems.CoralEffector; //controllers @@ -40,7 +40,7 @@ public class RobotContainer { public RobotContainer() { SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); - SmartDashboard.putData("coral out", new SetCoralOut(coralEffector)); + SmartDashboard.putData("coral out", new CoralOutake(coralEffector)); setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); @@ -56,7 +56,11 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() {} - private void setBindingsManipulator() {} + private void setBindingsManipulator() { + new JoystickButton(manipulatorController, OI.Manipulator.IntakeButton) + .whileTrue(new CoralOutake(coralEffector)) + .whileFalse(new CoralIntake(coralEffector)); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index e3ff86d..a20b540 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -9,10 +9,10 @@ public class CoralIntake extends Command { public static double spin; - public static boolean fast2 = false; - Timer timer = new Timer(); - Timer timer2 = new Timer(); - Timer coralInTimer = new Timer(); + // public static boolean fast2 = false; + // Timer timer = new Timer(); + // Timer timer2 = new Timer(); + // Timer coralInTimer = new Timer(); public static double coralMotorPosition; private CoralEffector coralEffector; @@ -52,16 +52,16 @@ public void execute() { // CoralEffector.coralMotor.set(0.1); // spin = 10; SmartDashboard.putNumber("spin", spin); - SmartDashboard.putNumber("timer", timer.get()); - // SmartDashboard.getBoolean("outakeGet", coralIn);r + //SmartDashboard.putNumber("timer", timer.get()); + // SmartDashboard.getBoolean("outakeGet", coralIn);r SmartDashboard.putBoolean("coral in", coralEffector.coralIsIn()); if (CoralEffector.distanceSensorSees){ - coralEffector.setMotorSpeed(0.07); + coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorInputFastSpeed); coralMotorPosition = coralEffector.getEncoderPos(); //rotations coralEffector.setCoralIn(true); } else if (coralEffector.coralIsIn()){ - coralEffector.setReferencePosition(coralMotorPosition - 0.1); //rotations + coralEffector.setReferencePosition(coralMotorPosition + CoralEffectorConstants.coralEffectorDistanceSensorOffset); //rotations } else { coralEffector.setMotorSpeed(0); diff --git a/src/main/java/org/carlmontrobotics/commands/SetCoralOut.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java similarity index 80% rename from src/main/java/org/carlmontrobotics/commands/SetCoralOut.java rename to src/main/java/org/carlmontrobotics/commands/CoralOutake.java index 9635599..bf451eb 100644 --- a/src/main/java/org/carlmontrobotics/commands/SetCoralOut.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -4,14 +4,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.CoralEffectorConstants; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj2.command.Command; -public class SetCoralOut extends Command { +public class CoralOutake extends Command { /** Creates a new SetCoralOut. */ private CoralEffector coralEffector; - public SetCoralOut(CoralEffector coralEffector) { + public CoralOutake(CoralEffector coralEffector) { addRequirements(this.coralEffector = coralEffector); // Use addRequirements() here to declare subsystem dependencies. } @@ -23,6 +24,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); coralEffector.setCoralIn(false); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 64b7378..80c3ee6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -38,11 +38,10 @@ public class CoralEffector extends SubsystemBase { public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.coralDistanceSensorPort); - public static double p = 0.5; public static boolean distanceSensorSees; public static boolean limitSwitchSees; public final RelativeEncoder coralEncoder = coralMotor.getEncoder(); - private double CoralGoalRPM = 100; + // private double CoralGoalRPM = 100; private double coralOutput = coralMotor.getAppliedOutput(); private boolean coralIn; @@ -53,7 +52,7 @@ public CoralEffector(){ .idleMode(IdleMode.kBrake); config.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(p, 0.0, 0.0); + .pid(CoralEffectorConstants.kp, CoralEffectorConstants.ki, CoralEffectorConstants.kd); coralMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } @@ -80,15 +79,15 @@ public void periodic() { //coralMotor.getClosedLoopController().setReference(1, ControlType.kVelocity); distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.coralDistanceSensorDistance; limitSwitchSees = !coralLimitSwitch.get(); - CoralGoalRPM = coralEncoder.getVelocity(); + // CoralGoalRPM = coralEncoder.getVelocity(); coralOutput = coralMotor.getAppliedOutput(); SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); SmartDashboard.putNumber("distance", distanceSensor.getRange()); SmartDashboard.putBoolean("limit switch", limitSwitchSees); - SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); + // SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); SmartDashboard.putNumber("coral output", coralOutput); - SmartDashboard.getNumber("P", p); + SmartDashboard.getNumber("P", CoralEffectorConstants.kp); } } \ No newline at end of file From 45d05832673c67f5b818951c1aeef107c4a1c4e1 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Fri, 21 Feb 2025 15:17:36 -0800 Subject: [PATCH 10/26] tuned PID --- .../java/org/carlmontrobotics/Constants.java | 8 ++++---- .../commands/CoralOutake.java | 19 +++++++++++++++++-- .../subsystems/CoralEffector.java | 3 +++ 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6fc3ffe..b57a16d 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -23,10 +23,10 @@ public static final class CoralEffectorConstants{ public final static double coralEffectorMotorInputFastSpeed = 0.07; // public final static double coralEffectorMotorInputFastSpeed2 = 0.05; // public final static double coralEffectorMotorInputSlowSpeed = 0.04; - public final static double coralEffectorMotorOutputSpeed = 0.2; + public final static double coralEffectorMotorOutputSpeed = 0.1; public final static double coralEffectorDistanceSensorOffset = -0.1; - public final static double kp = 0.04; - public final static double ki = 0.0; - public final static double kd = 0.0; + public final static double kp = 1.3; + public final static double ki = 0; + public final static double kd = 0; } } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java index bf451eb..e9f10a1 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -7,24 +7,39 @@ import org.carlmontrobotics.Constants.CoralEffectorConstants; import org.carlmontrobotics.subsystems.CoralEffector; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; public class CoralOutake extends Command { /** Creates a new SetCoralOut. */ private CoralEffector coralEffector; + Timer timer; public CoralOutake(CoralEffector coralEffector) { addRequirements(this.coralEffector = coralEffector); // Use addRequirements() here to declare subsystem dependencies. + timer = new Timer(); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + timer.start(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); + if (coralEffector.coralDetected()) { + coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); + timer.reset(); + timer.start(); + } + else if (timer.get() <= 0.5) { + coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); + } + else { + coralEffector.setMotorSpeed(0); + } coralEffector.setCoralIn(false); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 80c3ee6..76e972d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -73,6 +73,9 @@ public boolean coralIsIn() { public void setCoralIn(boolean coralIsInside) { coralIn = coralIsInside; } + public boolean coralDetected() { + return !coralLimitSwitch.get(); + } @Override public void periodic() { From 20735006da8b6e295339845ae9f7eaeebc209da2 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 22 Feb 2025 14:58:54 -0800 Subject: [PATCH 11/26] idk what tim did --- .../org/carlmontrobotics/RobotContainer.java | 23 +++--- .../subsystems/AlgaeEffector.java | 71 +++++++++++++++++++ 2 files changed, 82 insertions(+), 12 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index c7f40f5..ee0395a 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,15 +9,13 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; -import org.carlmontrobotics.Constants.OI; -import org.carlmontrobotics.commands.CoralIntake; -import org.carlmontrobotics.commands.CoralOutake; +import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -29,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class RobotContainer { @@ -36,11 +35,11 @@ public class RobotContainer { //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.port); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); - public final CoralEffector coralEffector = new CoralEffector(); - + + //private final CoralEffector coralEffector = new CoralEffector(); + private final AlgaeEffector algaeEffector = new AlgaeEffector(); public RobotContainer() { - SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); - SmartDashboard.putData("coral out", new CoralOutake(coralEffector)); + setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); @@ -55,11 +54,11 @@ private void setDefaultCommands() { // () -> driverController.getRawButton(OI.Driver.slowDriveButton) // )); } - private void setBindingsDriver() {} + private void setBindingsDriver() { + + } private void setBindingsManipulator() { - new JoystickButton(manipulatorController, OI.Manipulator.IntakeButton) - .whileTrue(new CoralOutake(coralEffector)) - .whileFalse(new CoralIntake(coralEffector)); + } public Command getAutonomousCommand() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java new file mode 100644 index 0000000..8e34e2c --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -0,0 +1,71 @@ +package org.carlmontrobotics.subsystems; + + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + + +import static org.carlmontrobotics.RobotContainer.*; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.RobotContainer; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; + +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class AlgaeEffector extends SubsystemBase { + private SparkMax armMotor = new SparkMax(14, MotorType.kBrushless); + private RelativeEncoder armEncoder = armMotor.getEncoder(); + private RelativeEncoder absoluteEncoder = armMotor.getAlternateEncoder(); + private SparkClosedLoopController pidArm; + public AlgaeEffector() { + SparkMaxConfig armConfig = new SparkMaxConfig(); + armConfig.encoder.positionConversionFactor(360/3); + armConfig.closedLoop.pid(0, 0, 0); + armConfig.closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder); + armMotor.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + pidArm = armMotor.getClosedLoopController(); + + + } + public void RunPID(double angle) { + pidArm.setReference(angle, ControlType.kPosition); + } + @Override + public void periodic() { + SmartDashboard.putNumber("RelativeEncoderPos", armEncoder.getPosition()); + SmartDashboard.putNumber("AbsoluteEncoderPos", absoluteEncoder.getPosition()); + } +} From fc6e4c9ce22b7a63c6af0a84ec0680868790d69b Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Sat, 22 Feb 2025 23:48:53 -0800 Subject: [PATCH 12/26] bump year in wpilib_preferences.json --- .wpilib/wpilib_preferences.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index f0ee648..d2a15b8 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "currentLanguage": "none", "enableCppIntellisense": false, - "projectYear": "none", + "projectYear": "2025", "teamNumber": 199 -} \ No newline at end of file +} From b49108633ce2c1fc71bb08c2f845d7bee969f41e Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Tue, 25 Feb 2025 16:37:30 -0800 Subject: [PATCH 13/26] Revert "idk what tim did" This reverts commit 20735006da8b6e295339845ae9f7eaeebc209da2. --- .../org/carlmontrobotics/RobotContainer.java | 23 +++--- .../subsystems/AlgaeEffector.java | 71 ------------------- 2 files changed, 12 insertions(+), 82 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index ee0395a..c7f40f5 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,13 +9,15 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; -import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.commands.CoralIntake; +import org.carlmontrobotics.commands.CoralOutake; import org.carlmontrobotics.subsystems.CoralEffector; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; - +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -27,7 +29,6 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class RobotContainer { @@ -35,11 +36,11 @@ public class RobotContainer { //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.port); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); - - //private final CoralEffector coralEffector = new CoralEffector(); - private final AlgaeEffector algaeEffector = new AlgaeEffector(); - public RobotContainer() { + public final CoralEffector coralEffector = new CoralEffector(); + public RobotContainer() { + SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); + SmartDashboard.putData("coral out", new CoralOutake(coralEffector)); setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); @@ -54,11 +55,11 @@ private void setDefaultCommands() { // () -> driverController.getRawButton(OI.Driver.slowDriveButton) // )); } - private void setBindingsDriver() { - - } + private void setBindingsDriver() {} private void setBindingsManipulator() { - + new JoystickButton(manipulatorController, OI.Manipulator.IntakeButton) + .whileTrue(new CoralOutake(coralEffector)) + .whileFalse(new CoralIntake(coralEffector)); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java deleted file mode 100644 index 8e34e2c..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ /dev/null @@ -1,71 +0,0 @@ -package org.carlmontrobotics.subsystems; - - -import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; - - -import static org.carlmontrobotics.RobotContainer.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.RobotContainer; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; - -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; - -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.simulation.SimDeviceSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - - -public class AlgaeEffector extends SubsystemBase { - private SparkMax armMotor = new SparkMax(14, MotorType.kBrushless); - private RelativeEncoder armEncoder = armMotor.getEncoder(); - private RelativeEncoder absoluteEncoder = armMotor.getAlternateEncoder(); - private SparkClosedLoopController pidArm; - public AlgaeEffector() { - SparkMaxConfig armConfig = new SparkMaxConfig(); - armConfig.encoder.positionConversionFactor(360/3); - armConfig.closedLoop.pid(0, 0, 0); - armConfig.closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder); - armMotor.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - pidArm = armMotor.getClosedLoopController(); - - - } - public void RunPID(double angle) { - pidArm.setReference(angle, ControlType.kPosition); - } - @Override - public void periodic() { - SmartDashboard.putNumber("RelativeEncoderPos", armEncoder.getPosition()); - SmartDashboard.putNumber("AbsoluteEncoderPos", absoluteEncoder.getPosition()); - } -} From 16578a33ed1df6e6030b35ae446be3ac6b6e84c1 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Tue, 25 Feb 2025 16:40:29 -0800 Subject: [PATCH 14/26] removed limit switch because won't be on the robot anymore --- .../org/carlmontrobotics/commands/CoralOutake.java | 4 ++-- .../carlmontrobotics/subsystems/CoralEffector.java | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java index e9f10a1..e4ea134 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -29,12 +29,12 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (coralEffector.coralDetected()) { +/* if (coralEffector.coralDetected()) { coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); timer.reset(); timer.start(); } - else if (timer.get() <= 0.5) { + else */if (timer.get() <= 0.5) { coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); } else { diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 76e972d..4f8425a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -35,11 +35,11 @@ public class CoralEffector extends SubsystemBase { public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.coralMotorPort, MotorType.kBrushless); - public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); + // public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.coralDistanceSensorPort); public static boolean distanceSensorSees; - public static boolean limitSwitchSees; + // public static boolean limitSwitchSees; public final RelativeEncoder coralEncoder = coralMotor.getEncoder(); // private double CoralGoalRPM = 100; private double coralOutput = coralMotor.getAppliedOutput(); @@ -73,21 +73,21 @@ public boolean coralIsIn() { public void setCoralIn(boolean coralIsInside) { coralIn = coralIsInside; } - public boolean coralDetected() { - return !coralLimitSwitch.get(); - } + // public boolean coralDetected() { + // return !coralLimitSwitch.get(); + // } @Override public void periodic() { //coralMotor.getClosedLoopController().setReference(1, ControlType.kVelocity); distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.coralDistanceSensorDistance; - limitSwitchSees = !coralLimitSwitch.get(); + // limitSwitchSees = !coralLimitSwitch.get(); // CoralGoalRPM = coralEncoder.getVelocity(); coralOutput = coralMotor.getAppliedOutput(); SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); SmartDashboard.putNumber("distance", distanceSensor.getRange()); - SmartDashboard.putBoolean("limit switch", limitSwitchSees); + // SmartDashboard.putBoolean("limit switch", limitSwitchSees); // SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); SmartDashboard.putNumber("coral output", coralOutput); From 6971246dceaf4dd111307b4555de6005663c819e Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Tue, 25 Feb 2025 17:33:58 -0800 Subject: [PATCH 15/26] made all the constants uppercase + added a todo + updated dependecies --- .../java/org/carlmontrobotics/Constants.java | 34 +++--- .../org/carlmontrobotics/RobotContainer.java | 6 +- .../commands/CoralIntake.java | 8 +- .../commands/CoralOutake.java | 2 +- .../subsystems/CoralEffector.java | 11 +- ....2.1.json => PathplannerLib-2025.2.4.json} | 8 +- ...enix6-25.1.0.json => Phoenix6-25.3.1.json} | 114 +++++++++++++----- ...Lib-2025.0.0.json => REVLib-2025.0.2.json} | 15 +-- ...ca-2025.0.0.json => Studica-2025.0.1.json} | 14 +-- ....01.04.json => playingwithfusion2025.json} | 18 ++- 10 files changed, 144 insertions(+), 86 deletions(-) rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.4.json} (87%) rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.3.1.json} (80%) rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.2.json} (86%) rename vendordeps/{Studica-2025.0.0.json => Studica-2025.0.1.json} (89%) rename vendordeps/{PlayingWithFusion-2025.01.04.json => playingwithfusion2025.json} (82%) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b57a16d..961bd94 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -3,30 +3,26 @@ import edu.wpi.first.wpilibj.StadiaController.Button; public final class Constants { - // public static final class Drivetrain { - // public static final double MAX_SPEED_MPS = 2; - // } public static final class OI { public static final class Driver { - public static final int port = 0; + public static final int PORT = 0; } public static final class Manipulator { - public static final int port = 1; - public static final int IntakeButton = Button.kA.value; + public static final int PORT = 1; + public static final int INTAKE_BUTTON = Button.kA.value; } } - public static final class CoralEffectorConstants{ - public final static int coralMotorPort = 1; - public final static int coralLimitSwitchPort = 0; - public final static int coralDistanceSensorPort = 6; - public final static int coralDistanceSensorDistance = 150; - public final static double coralEffectorMotorInputFastSpeed = 0.07; - // public final static double coralEffectorMotorInputFastSpeed2 = 0.05; - // public final static double coralEffectorMotorInputSlowSpeed = 0.04; - public final static double coralEffectorMotorOutputSpeed = 0.1; - public final static double coralEffectorDistanceSensorOffset = -0.1; - public final static double kp = 1.3; - public final static double ki = 0; - public final static double kd = 0; + public static final class CoralEffectorConstants { + public final static int CORAL_MOTOR_PORT = 1; + public final static int CORAL_LIMIT_SWITCH_PORT = 0; + public final static int CORAL_DISTANCE_SENSOR_PORT = 6; + //TODO: tune distance sensor distance, speeds, offsets, and PID on the actual coral subsystem + public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; + public final static double CORAL_EFFECTOR_MOTOR_INPUT_FAST_SPEED = 0.07; + public final static double CORAL_EFFECTOR_MOTOR_OUTPUT_SPEED = 0.1; + public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; + public final static double KP = 1.3; + public final static double KI = 0; + public final static double KD = 0; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index c7f40f5..a2d29ba 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -34,8 +34,8 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(OI.Driver.port); - public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); + public final GenericHID driverController = new GenericHID(OI.Driver.PORT); + public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.PORT); public final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { @@ -57,7 +57,7 @@ private void setDefaultCommands() { } private void setBindingsDriver() {} private void setBindingsManipulator() { - new JoystickButton(manipulatorController, OI.Manipulator.IntakeButton) + new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON) .whileTrue(new CoralOutake(coralEffector)) .whileFalse(new CoralIntake(coralEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index a20b540..7b722be 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command;; public class CoralIntake extends Command { - public static double spin; + // public static double spin; // public static boolean fast2 = false; // Timer timer = new Timer(); // Timer timer2 = new Timer(); @@ -51,17 +51,17 @@ public void execute() { // } // CoralEffector.coralMotor.set(0.1); // spin = 10; - SmartDashboard.putNumber("spin", spin); + // SmartDashboard.putNumber("spin", spin); //SmartDashboard.putNumber("timer", timer.get()); // SmartDashboard.getBoolean("outakeGet", coralIn);r SmartDashboard.putBoolean("coral in", coralEffector.coralIsIn()); if (CoralEffector.distanceSensorSees){ - coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorInputFastSpeed); + coralEffector.setMotorSpeed(CoralEffectorConstants.CORAL_EFFECTOR_MOTOR_INPUT_FAST_SPEED); coralMotorPosition = coralEffector.getEncoderPos(); //rotations coralEffector.setCoralIn(true); } else if (coralEffector.coralIsIn()){ - coralEffector.setReferencePosition(coralMotorPosition + CoralEffectorConstants.coralEffectorDistanceSensorOffset); //rotations + coralEffector.setReferencePosition(coralMotorPosition + CoralEffectorConstants.CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations } else { coralEffector.setMotorSpeed(0); diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java index e4ea134..aac8fce 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -35,7 +35,7 @@ public void execute() { timer.start(); } else */if (timer.get() <= 0.5) { - coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); + coralEffector.setMotorSpeed(CoralEffectorConstants.CORAL_EFFECTOR_MOTOR_OUTPUT_SPEED); } else { coralEffector.setMotorSpeed(0); diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 4f8425a..498b591 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -1,4 +1,3 @@ - package org.carlmontrobotics.subsystems; import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; @@ -34,9 +33,9 @@ public class CoralEffector extends SubsystemBase { - public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.coralMotorPort, MotorType.kBrushless); + public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.CORAL_MOTOR_PORT, MotorType.kBrushless); // public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); - public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.coralDistanceSensorPort); + public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.CORAL_DISTANCE_SENSOR_PORT); public static boolean distanceSensorSees; // public static boolean limitSwitchSees; @@ -52,7 +51,7 @@ public CoralEffector(){ .idleMode(IdleMode.kBrake); config.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(CoralEffectorConstants.kp, CoralEffectorConstants.ki, CoralEffectorConstants.kd); + .pid(CoralEffectorConstants.KP, CoralEffectorConstants.KI, CoralEffectorConstants.KD); coralMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } @@ -80,7 +79,7 @@ public void setCoralIn(boolean coralIsInside) { @Override public void periodic() { //coralMotor.getClosedLoopController().setReference(1, ControlType.kVelocity); - distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.coralDistanceSensorDistance; + distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.CORAL_DISTANCE_SENSOR_DISTANCE; // limitSwitchSees = !coralLimitSwitch.get(); // CoralGoalRPM = coralEncoder.getVelocity(); coralOutput = coralMotor.getAppliedOutput(); @@ -91,6 +90,6 @@ public void periodic() { // SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); SmartDashboard.putNumber("coral output", coralOutput); - SmartDashboard.getNumber("P", CoralEffectorConstants.kp); + SmartDashboard.getNumber("P", CoralEffectorConstants.KP); } } \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.4.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib-2025.2.4.json index 71e25f3..24add57 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib-2025.2.4.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib-2025.2.4.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.3.1.json similarity index 80% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-25.3.1.json index 473f6a8..a216d97 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-25.3.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-25.3.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.3.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.3.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +365,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.2.json similarity index 86% rename from vendordeps/REVLib-2025.0.0.json rename to vendordeps/REVLib-2025.0.2.json index cde6011..c29aefa 100644 --- a/vendordeps/REVLib-2025.0.0.json +++ b/vendordeps/REVLib-2025.0.2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0.json", + "fileName": "REVLib-2025.0.2.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json similarity index 89% rename from vendordeps/Studica-2025.0.0.json rename to vendordeps/Studica-2025.0.1.json index ddb0e44..5010be0 100644 --- a/vendordeps/Studica-2025.0.0.json +++ b/vendordeps/Studica-2025.0.1.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.0.0.json", + "fileName": "Studica-2025.0.1.json", "name": "Studica", - "version": "2025.0.0", + "version": "2025.0.1", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.0.0" + "version": "2025.0.1" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.0.0" + "version": "2025.0.1" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.0.0" + "version": "2025.0.1" } ] } \ No newline at end of file diff --git a/vendordeps/PlayingWithFusion-2025.01.04.json b/vendordeps/playingwithfusion2025.json similarity index 82% rename from vendordeps/PlayingWithFusion-2025.01.04.json rename to vendordeps/playingwithfusion2025.json index 580c121..61a5641 100644 --- a/vendordeps/PlayingWithFusion-2025.01.04.json +++ b/vendordeps/playingwithfusion2025.json @@ -1,7 +1,7 @@ { - "fileName": "PlayingWithFusion-2025.01.04.json", + "fileName": "playingwithfusion2025.json", "name": "PlayingWithFusion", - "version": "2025.01.04", + "version": "2025.01.23", "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", "frcYear": "2025", "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2025.json", @@ -12,21 +12,23 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-java", - "version": "2025.01.04" + "version": "2025.01.23" } ], "jniDependencies": [ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2025.01.04", + "version": "2025.01.23", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", + "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", + "linuxarm32", "osxuniversal" ] } @@ -35,32 +37,36 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-cpp", - "version": "2025.01.04", + "version": "2025.01.23", "headerClassifier": "headers", "sharedLibrary": false, "libName": "PlayingWithFusion", "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", + "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", + "linuxarm32", "osxuniversal" ] }, { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2025.01.04", + "version": "2025.01.23", "headerClassifier": "headers", "sharedLibrary": true, "libName": "PlayingWithFusionDriver", "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", + "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", + "linuxarm32", "osxuniversal" ] } From 48c01780d8be7a784b51b3ede8f6510196154a12 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Thu, 27 Feb 2025 23:07:37 -0800 Subject: [PATCH 16/26] Alex passover add slow intake speed when measuring the coral out make boolean vars functions you call (standard) added end conditions to commands instead of using if/else executes --- .../java/org/carlmontrobotics/Constants.java | 6 ++-- .../commands/CoralIntake.java | 22 ++++++------ .../commands/CoralOutake.java | 16 ++++----- .../subsystems/CoralEffector.java | 34 ++++++++++++------- 4 files changed, 43 insertions(+), 35 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 961bd94..60dbc72 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -18,8 +18,10 @@ public static final class CoralEffectorConstants { public final static int CORAL_DISTANCE_SENSOR_PORT = 6; //TODO: tune distance sensor distance, speeds, offsets, and PID on the actual coral subsystem public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; - public final static double CORAL_EFFECTOR_MOTOR_INPUT_FAST_SPEED = 0.07; - public final static double CORAL_EFFECTOR_MOTOR_OUTPUT_SPEED = 0.1; + public final static double CORAL_INTAKE_ERR = .1;//encoder units - likely rotations + public final static double INPUT_FAST_SPEED = 0.07; + public final static double INPUT_SLOW_SPEED = 0.035; + public final static double OUTPUT_SPEED = 0.1; public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; public final static double KP = 1.3; public final static double KI = 0; diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 7b722be..df2e966 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -25,6 +25,7 @@ public CoralIntake(CoralEffector coralEffector) { // Called when the command is initially scheduled. @Override public void initialize() { + coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_FAST_SPEED); } // Called every time the scheduler runs while the command is scheduled. @@ -53,30 +54,27 @@ public void execute() { // spin = 10; // SmartDashboard.putNumber("spin", spin); //SmartDashboard.putNumber("timer", timer.get()); - // SmartDashboard.getBoolean("outakeGet", coralIn);r - SmartDashboard.putBoolean("coral in", coralEffector.coralIsIn()); - if (CoralEffector.distanceSensorSees){ - coralEffector.setMotorSpeed(CoralEffectorConstants.CORAL_EFFECTOR_MOTOR_INPUT_FAST_SPEED); - coralMotorPosition = coralEffector.getEncoderPos(); //rotations - coralEffector.setCoralIn(true); - } - else if (coralEffector.coralIsIn()){ + // SmartDashboard.getBoolean("outakeGet", coralIn); + + if (coralEffector.coralIn){ coralEffector.setReferencePosition(coralMotorPosition + CoralEffectorConstants.CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations } - else { - coralEffector.setMotorSpeed(0); + else if (coralEffector.seesCoral()){ + coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_SLOW_SPEED); + coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations + coralEffector.coralIn=true; } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + coralEffector.setMotorSpeed(0); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return coralEffector.motorAtGoal();//FIXME should add a timeout timer!! } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java index aac8fce..5c05675 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -23,7 +23,9 @@ public CoralOutake(CoralEffector coralEffector) { // Called when the command is initially scheduled. @Override public void initialize() { + timer.reset(); timer.start(); + coralEffector.setMotorSpeed(CoralEffectorConstants.OUTPUT_SPEED); } // Called every time the scheduler runs while the command is scheduled. @@ -34,22 +36,18 @@ public void execute() { timer.reset(); timer.start(); } - else */if (timer.get() <= 0.5) { - coralEffector.setMotorSpeed(CoralEffectorConstants.CORAL_EFFECTOR_MOTOR_OUTPUT_SPEED); - } - else { - coralEffector.setMotorSpeed(0); - } - coralEffector.setCoralIn(false); + else */ } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + coralEffector.setMotorSpeed(0); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return timer.get() > .5 || coralEffector.seesCoral()==false; } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 498b591..3fb6164 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -35,14 +35,15 @@ public class CoralEffector extends SubsystemBase { public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.CORAL_MOTOR_PORT, MotorType.kBrushless); // public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); + //FIXME ADD THE LIMIT SWITCH!! public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.CORAL_DISTANCE_SENSOR_PORT); - public static boolean distanceSensorSees; - // public static boolean limitSwitchSees; + public final RelativeEncoder coralEncoder = coralMotor.getEncoder(); - // private double CoralGoalRPM = 100; - private double coralOutput = coralMotor.getAppliedOutput(); - private boolean coralIn; + + private double coralOutput; + public boolean coralIn; + private double targetPos=0; SparkFlexConfig config = new SparkFlexConfig(); public CoralEffector(){ @@ -63,15 +64,24 @@ public void setMotorSpeed(double speed) { coralMotor.set(speed); } public void setReferencePosition(double reference) { + targetPos = reference; coralMotor.getClosedLoopController().setReference(reference, SparkBase.ControlType.kPosition); } - - public boolean coralIsIn() { - return coralIn; + public boolean motorAtGoal(){ + return Math.abs(coralEncoder.getPosition()-targetPos) <= CoralEffectorConstants.CORAL_INTAKE_ERR; } - public void setCoralIn(boolean coralIsInside) { - coralIn = coralIsInside; + + public boolean seesCoral(){ + return distanceSensor.getRange() < CoralEffectorConstants.CORAL_DISTANCE_SENSOR_DISTANCE; } + + // public boolean coralIsIn() { + // return coralIn; + // } + // public void setCoralIn(boolean coralIsInside) { + // coralIn = coralIsInside; + // } + // public boolean coralDetected() { // return !coralLimitSwitch.get(); // } @@ -79,13 +89,13 @@ public void setCoralIn(boolean coralIsInside) { @Override public void periodic() { //coralMotor.getClosedLoopController().setReference(1, ControlType.kVelocity); - distanceSensorSees = distanceSensor.getRange() < CoralEffectorConstants.CORAL_DISTANCE_SENSOR_DISTANCE; // limitSwitchSees = !coralLimitSwitch.get(); // CoralGoalRPM = coralEncoder.getVelocity(); coralOutput = coralMotor.getAppliedOutput(); - SmartDashboard.putBoolean("Distance sensor", distanceSensorSees); + SmartDashboard.putBoolean("Distance sensor", seesCoral()); SmartDashboard.putNumber("distance", distanceSensor.getRange()); + SmartDashboard.putBoolean("coral in", coralIn); // SmartDashboard.putBoolean("limit switch", limitSwitchSees); // SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); From 77d231a7a0c6c85fea282c59580428904308e4a0 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 7 Mar 2025 20:16:18 -0800 Subject: [PATCH 17/26] stuff --- .../java/org/carlmontrobotics/Constants.java | 6 ++- .../org/carlmontrobotics/RobotContainer.java | 23 ++++++++-- .../commands/CoralIntake.java | 11 +++-- .../commands/CoralOutake.java | 3 +- .../commands/ManualCoralIntake.java | 44 +++++++++++++++++++ .../subsystems/CoralEffector.java | 12 ++--- ....2.4.json => PathplannerLib-2025.2.5.json} | 8 ++-- .../{REVLib-2025.0.2.json => REVLib.json} | 12 ++--- 8 files changed, 94 insertions(+), 25 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java rename vendordeps/{PathplannerLib-2025.2.4.json => PathplannerLib-2025.2.5.json} (87%) rename vendordeps/{REVLib-2025.0.2.json => REVLib.json} (90%) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 60dbc72..02e5883 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -9,7 +9,8 @@ public static final class Driver { } public static final class Manipulator { public static final int PORT = 1; - public static final int INTAKE_BUTTON = Button.kA.value; + public static final int OUTAKE_BUTTON = Button.kA.value; + public static final int INTAKE_BUTTON = Button.kB.value; } } public static final class CoralEffectorConstants { @@ -26,5 +27,8 @@ public static final class CoralEffectorConstants { public final static double KP = 1.3; public final static double KI = 0; public final static double KD = 0; + public final static double INTAKE_TIME_OUT = 0.5; + public final static double OUTAKE_TIME_OUT = 10; + public final static double MANUAL_INTAKE_TIME_OUT = 1; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index a2d29ba..c256a84 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,9 +9,13 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; +import java.util.function.BooleanSupplier; + import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.CoralIntake; import org.carlmontrobotics.commands.CoralOutake; +import org.carlmontrobotics.commands.ManualCoralIntake; import org.carlmontrobotics.subsystems.CoralEffector; //controllers @@ -57,10 +61,19 @@ private void setDefaultCommands() { } private void setBindingsDriver() {} private void setBindingsManipulator() { - new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON) - .whileTrue(new CoralOutake(coralEffector)) - .whileFalse(new CoralIntake(coralEffector)); + // new JoystickButton(manipulatorController, OI.Manipulator.OUTAKE_BUTTON) + // .whileTrue(new CoralOutake(coralEffector)) + // .whileFalse(new CoralIntake(coralEffector)); + // new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON) + // .whileTrue(new ManualCoralIntake()); + axisTrigger(manipulatorController, Axis.kLeftTrigger) + .whileTrue(new CoralOutake(coralEffector)) + .whileFalse(new CoralIntake(coralEffector)); + axisTrigger(manipulatorController, Axis.kRightTrigger) + .whileTrue(new ManualCoralIntake()); } + + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); @@ -100,4 +113,8 @@ private double inputProcessing(double value) { private double ProcessedAxisValue(GenericHID hid, Axis axis){ return inputProcessing(getStickValue(hid, axis)); } + + private Trigger axisTrigger(GenericHID controller, Axis axis) { + return new Trigger( (BooleanSupplier)(() -> Math.abs(getStickValue(controller, axis)) > 0.2) ); + } } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index df2e966..b3b718d 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -10,7 +10,7 @@ public class CoralIntake extends Command { // public static double spin; // public static boolean fast2 = false; - // Timer timer = new Timer(); + Timer timer = new Timer(); // Timer timer2 = new Timer(); // Timer coralInTimer = new Timer(); public static double coralMotorPosition; @@ -59,11 +59,14 @@ public void execute() { if (coralEffector.coralIn){ coralEffector.setReferencePosition(coralMotorPosition + CoralEffectorConstants.CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations } - else if (coralEffector.seesCoral()){ - coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_SLOW_SPEED); + else if (coralEffector.distanceSensorSeesCoral()){ + coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_FAST_SPEED); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; } + else if (coralEffector.limitSwitchSeesCoral()){ + coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_SLOW_SPEED); + } } // Called once the command ends or is interrupted. @@ -75,6 +78,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return coralEffector.motorAtGoal();//FIXME should add a timeout timer!! + return coralEffector.motorAtGoal() || timer.get() > CoralEffectorConstants.INTAKE_TIME_OUT; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java index 5c05675..f8c255d 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -43,11 +43,12 @@ public void execute() { @Override public void end(boolean interrupted) { coralEffector.setMotorSpeed(0); + coralEffector.coralIn = false; } // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > .5 || coralEffector.seesCoral()==false; + return timer.get() > CoralEffectorConstants.OUTAKE_TIME_OUT || coralEffector.distanceSensorSeesCoral()==false; } } diff --git a/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java b/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java new file mode 100644 index 0000000..fdd2f96 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.Constants.CoralEffectorConstants; +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +import org.carlmontrobotics.subsystems.CoralEffector; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ManualCoralIntake extends Command { + /** Creates a new ManualCoralIntake. */ + private CoralEffector coralEffector; + Timer timer = new Timer(); + public ManualCoralIntake() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.coralEffector = coralEffector); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_FAST_SPEED); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > CoralEffectorConstants.MANUAL_INTAKE_TIME_OUT; + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 3fb6164..26e0d80 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -34,7 +34,7 @@ public class CoralEffector extends SubsystemBase { public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.CORAL_MOTOR_PORT, MotorType.kBrushless); - // public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.coralLimitSwitchPort); + public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.CORAL_LIMIT_SWITCH_PORT); //FIXME ADD THE LIMIT SWITCH!! public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.CORAL_DISTANCE_SENSOR_PORT); @@ -71,7 +71,7 @@ public boolean motorAtGoal(){ return Math.abs(coralEncoder.getPosition()-targetPos) <= CoralEffectorConstants.CORAL_INTAKE_ERR; } - public boolean seesCoral(){ + public boolean distanceSensorSeesCoral(){ return distanceSensor.getRange() < CoralEffectorConstants.CORAL_DISTANCE_SENSOR_DISTANCE; } @@ -82,9 +82,9 @@ public boolean seesCoral(){ // coralIn = coralIsInside; // } - // public boolean coralDetected() { - // return !coralLimitSwitch.get(); - // } + public boolean limitSwitchSeesCoral() { + return !coralLimitSwitch.get(); + } @Override public void periodic() { @@ -93,7 +93,7 @@ public void periodic() { // CoralGoalRPM = coralEncoder.getVelocity(); coralOutput = coralMotor.getAppliedOutput(); - SmartDashboard.putBoolean("Distance sensor", seesCoral()); + SmartDashboard.putBoolean("Distance sensor", distanceSensorSeesCoral()); SmartDashboard.putNumber("distance", distanceSensor.getRange()); SmartDashboard.putBoolean("coral in", coralIn); // SmartDashboard.putBoolean("limit switch", limitSwitchSees); diff --git a/vendordeps/PathplannerLib-2025.2.4.json b/vendordeps/PathplannerLib-2025.2.5.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.4.json rename to vendordeps/PathplannerLib-2025.2.5.json index 24add57..b2a7265 100644 --- a/vendordeps/PathplannerLib-2025.2.4.json +++ b/vendordeps/PathplannerLib-2025.2.5.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.4.json", + "fileName": "PathplannerLib-2025.2.5.json", "name": "PathplannerLib", - "version": "2025.2.4", + "version": "2025.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.4" + "version": "2025.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.4", + "version": "2025.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib.json similarity index 90% rename from vendordeps/REVLib-2025.0.2.json rename to vendordeps/REVLib.json index c29aefa..ac62be8 100644 --- a/vendordeps/REVLib-2025.0.2.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.2.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.2", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.2" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 9fb699a01e00ed3c31906bc77cf287d113e7ce6b Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 10 Mar 2025 16:55:26 -0700 Subject: [PATCH 18/26] limit switch set up --- .../java/org/carlmontrobotics/Constants.java | 8 +++++--- .../org/carlmontrobotics/RobotContainer.java | 13 +++++++++++-- .../carlmontrobotics/commands/CoralIntake.java | 12 ++++++------ .../carlmontrobotics/commands/CoralOutake.java | 6 +++--- .../commands/ManualCoralIntake.java | 6 +++--- .../subsystems/CoralEffector.java | 16 ++++++++-------- 6 files changed, 36 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 02e5883..70fe72b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -5,15 +5,15 @@ public final class Constants { public static final class OI { public static final class Driver { - public static final int PORT = 0; + public static final int DRIVE_CONTROLLER_PORT = 0; } public static final class Manipulator { - public static final int PORT = 1; + public static final int MANIUPLATOR_CONTROLLER_sPORT = 1; public static final int OUTAKE_BUTTON = Button.kA.value; public static final int INTAKE_BUTTON = Button.kB.value; } } - public static final class CoralEffectorConstants { + public static final class CoralEffectorc { public final static int CORAL_MOTOR_PORT = 1; public final static int CORAL_LIMIT_SWITCH_PORT = 0; public final static int CORAL_DISTANCE_SENSOR_PORT = 6; @@ -30,5 +30,7 @@ public static final class CoralEffectorConstants { public final static double INTAKE_TIME_OUT = 0.5; public final static double OUTAKE_TIME_OUT = 10; public final static double MANUAL_INTAKE_TIME_OUT = 1; + + public final static int LIMIT_SWITCH_PORT = 7; //TODO: CHange } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index c256a84..985f99f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -18,6 +18,8 @@ import org.carlmontrobotics.commands.ManualCoralIntake; import org.carlmontrobotics.subsystems.CoralEffector; +//limit switch +import edu.wpi.first.wpilibj.DigitalInput; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -34,14 +36,21 @@ import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +//constats +import static org.carlmontrobotics.Constants.CoralEffectorc.*; +import static org.carlmontrobotics.Constants.OI.Driver.*; +import static org.carlmontrobotics.Constants.OI.Manipulator.*; + public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(OI.Driver.PORT); - public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.PORT); + public final GenericHID driverController = new GenericHID(DRIVE_CONTROLLER_PORT); + public final GenericHID manipulatorController = new GenericHID(MANIUPLATOR_CONTROLLER_sPORT); public final CoralEffector coralEffector = new CoralEffector(); + public final DigitalInput limitSwitch = new DigitalInput(LIMIT_SWITCH_PORT); + public RobotContainer() { SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); SmartDashboard.putData("coral out", new CoralOutake(coralEffector)); diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index b3b718d..c668f58 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -1,6 +1,6 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.Constants.CoralEffectorConstants; +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; @@ -25,7 +25,7 @@ public CoralIntake(CoralEffector coralEffector) { // Called when the command is initially scheduled. @Override public void initialize() { - coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_FAST_SPEED); + coralEffector.setMotorSpeed(INPUT_FAST_SPEED); } // Called every time the scheduler runs while the command is scheduled. @@ -57,15 +57,15 @@ public void execute() { // SmartDashboard.getBoolean("outakeGet", coralIn); if (coralEffector.coralIn){ - coralEffector.setReferencePosition(coralMotorPosition + CoralEffectorConstants.CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations + coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations } else if (coralEffector.distanceSensorSeesCoral()){ - coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_FAST_SPEED); + coralEffector.setMotorSpeed(INPUT_FAST_SPEED); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; } else if (coralEffector.limitSwitchSeesCoral()){ - coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_SLOW_SPEED); + coralEffector.setMotorSpeed(INPUT_SLOW_SPEED); } } @@ -78,6 +78,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return coralEffector.motorAtGoal() || timer.get() > CoralEffectorConstants.INTAKE_TIME_OUT; + return coralEffector.motorAtGoal() || timer.get() > INTAKE_TIME_OUT; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java index f8c255d..71ef215 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOutake.java @@ -4,7 +4,7 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.Constants.CoralEffectorConstants; +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; @@ -25,7 +25,7 @@ public CoralOutake(CoralEffector coralEffector) { public void initialize() { timer.reset(); timer.start(); - coralEffector.setMotorSpeed(CoralEffectorConstants.OUTPUT_SPEED); + coralEffector.setMotorSpeed(OUTPUT_SPEED); } // Called every time the scheduler runs while the command is scheduled. @@ -49,6 +49,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > CoralEffectorConstants.OUTAKE_TIME_OUT || coralEffector.distanceSensorSeesCoral()==false; + return timer.get() > OUTAKE_TIME_OUT || coralEffector.distanceSensorSeesCoral()==false; } } diff --git a/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java b/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java index fdd2f96..6dbc1d6 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java @@ -4,7 +4,7 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.Constants.CoralEffectorConstants; +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; @@ -29,7 +29,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - coralEffector.setMotorSpeed(CoralEffectorConstants.INPUT_FAST_SPEED); + coralEffector.setMotorSpeed(INPUT_FAST_SPEED); } // Called once the command ends or is interrupted. @@ -39,6 +39,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > CoralEffectorConstants.MANUAL_INTAKE_TIME_OUT; + return timer.get() > MANUAL_INTAKE_TIME_OUT; } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 26e0d80..b8b720b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import static org.carlmontrobotics.Constants.*; -import org.carlmontrobotics.Constants.CoralEffectorConstants; +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkFlex; @@ -33,10 +33,10 @@ public class CoralEffector extends SubsystemBase { - public SparkFlex coralMotor = new SparkFlex(CoralEffectorConstants.CORAL_MOTOR_PORT, MotorType.kBrushless); - public DigitalInput coralLimitSwitch = new DigitalInput(CoralEffectorConstants.CORAL_LIMIT_SWITCH_PORT); + public SparkFlex coralMotor = new SparkFlex(CORAL_MOTOR_PORT, MotorType.kBrushless); + public DigitalInput coralLimitSwitch = new DigitalInput(CORAL_LIMIT_SWITCH_PORT); //FIXME ADD THE LIMIT SWITCH!! - public TimeOfFlight distanceSensor = new TimeOfFlight(CoralEffectorConstants.CORAL_DISTANCE_SENSOR_PORT); + public TimeOfFlight distanceSensor = new TimeOfFlight(CORAL_DISTANCE_SENSOR_PORT); public final RelativeEncoder coralEncoder = coralMotor.getEncoder(); @@ -52,7 +52,7 @@ public CoralEffector(){ .idleMode(IdleMode.kBrake); config.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(CoralEffectorConstants.KP, CoralEffectorConstants.KI, CoralEffectorConstants.KD); + .pid(KP, KI, KD); coralMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } @@ -68,11 +68,11 @@ public void setReferencePosition(double reference) { coralMotor.getClosedLoopController().setReference(reference, SparkBase.ControlType.kPosition); } public boolean motorAtGoal(){ - return Math.abs(coralEncoder.getPosition()-targetPos) <= CoralEffectorConstants.CORAL_INTAKE_ERR; + return Math.abs(coralEncoder.getPosition()-targetPos) <= CORAL_INTAKE_ERR; } public boolean distanceSensorSeesCoral(){ - return distanceSensor.getRange() < CoralEffectorConstants.CORAL_DISTANCE_SENSOR_DISTANCE; + return distanceSensor.getRange() < CORAL_DISTANCE_SENSOR_DISTANCE; } // public boolean coralIsIn() { @@ -100,6 +100,6 @@ public void periodic() { // SmartDashboard.putNumber("Coral goal RPM", CoralGoalRPM); SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); SmartDashboard.putNumber("coral output", coralOutput); - SmartDashboard.getNumber("P", CoralEffectorConstants.KP); + SmartDashboard.getNumber("P", KP); } } \ No newline at end of file From 1f5340f6198d6b24f28cbde1a2f44036fae0cfb6 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Mon, 10 Mar 2025 20:22:00 -0700 Subject: [PATCH 19/26] renamed CoralOutake to CoralOuttake + renamed ManualCoralItake to CoralIntakeManual + added coments + fixed CoralIntakeManual code --- .../java/org/carlmontrobotics/Constants.java | 27 +++++++++---------- .../org/carlmontrobotics/RobotContainer.java | 12 ++++----- .../commands/CoralIntake.java | 4 +-- ...oralIntake.java => CoralIntakeManual.java} | 11 +++----- .../{CoralOutake.java => CoralOuttake.java} | 8 +++--- 5 files changed, 29 insertions(+), 33 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{ManualCoralIntake.java => CoralIntakeManual.java} (78%) rename src/main/java/org/carlmontrobotics/commands/{CoralOutake.java => CoralOuttake.java} (85%) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 70fe72b..5a14b82 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -8,29 +8,28 @@ public static final class Driver { public static final int DRIVE_CONTROLLER_PORT = 0; } public static final class Manipulator { - public static final int MANIUPLATOR_CONTROLLER_sPORT = 1; - public static final int OUTAKE_BUTTON = Button.kA.value; + public static final int MANIPULATOR_CONTROLLER_PORT = 1; + public static final int OUTTAKE_BUTTON = Button.kA.value; public static final int INTAKE_BUTTON = Button.kB.value; } } - public static final class CoralEffectorc { - public final static int CORAL_MOTOR_PORT = 1; + public static final class CoralEffectorc{ + public final static int CORAL_MOTOR_PORT = 30; public final static int CORAL_LIMIT_SWITCH_PORT = 0; public final static int CORAL_DISTANCE_SENSOR_PORT = 6; - //TODO: tune distance sensor distance, speeds, offsets, and PID on the actual coral subsystem - public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; - public final static double CORAL_INTAKE_ERR = .1;//encoder units - likely rotations - public final static double INPUT_FAST_SPEED = 0.07; - public final static double INPUT_SLOW_SPEED = 0.035; - public final static double OUTPUT_SPEED = 0.1; - public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; - public final static double KP = 1.3; + public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; //mm + public final static double CORAL_INTAKE_ERR = .1;//encoder units - rotations + public final static double INPUT_FAST_SPEED = 0.07; //TODO: tune this + public final static double INPUT_SLOW_SPEED = 0.035; //TODO: tune this + public final static double OUTPUT_SPEED = 0.1; //TODO: tune this + public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; //TOD: tune this? + public final static double KP = 1.3; //TODO: tune this public final static double KI = 0; public final static double KD = 0; public final static double INTAKE_TIME_OUT = 0.5; - public final static double OUTAKE_TIME_OUT = 10; + public final static double OUTTAKE_TIME_OUT = 10; public final static double MANUAL_INTAKE_TIME_OUT = 1; - public final static int LIMIT_SWITCH_PORT = 7; //TODO: CHange + public final static int LIMIT_SWITCH_PORT = 7; //TODO: Change } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 985f99f..5860970 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -14,8 +14,8 @@ import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.CoralIntake; -import org.carlmontrobotics.commands.CoralOutake; -import org.carlmontrobotics.commands.ManualCoralIntake; +import org.carlmontrobotics.commands.CoralOuttake; +import org.carlmontrobotics.commands.CoralIntakeManual; import org.carlmontrobotics.subsystems.CoralEffector; //limit switch @@ -46,14 +46,14 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(DRIVE_CONTROLLER_PORT); - public final GenericHID manipulatorController = new GenericHID(MANIUPLATOR_CONTROLLER_sPORT); + public final GenericHID manipulatorController = new GenericHID(MANIPULATOR_CONTROLLER_PORT); public final CoralEffector coralEffector = new CoralEffector(); public final DigitalInput limitSwitch = new DigitalInput(LIMIT_SWITCH_PORT); public RobotContainer() { SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); - SmartDashboard.putData("coral out", new CoralOutake(coralEffector)); + SmartDashboard.putData("coral out", new CoralOuttake(coralEffector)); setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); @@ -76,10 +76,10 @@ private void setBindingsManipulator() { // new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON) // .whileTrue(new ManualCoralIntake()); axisTrigger(manipulatorController, Axis.kLeftTrigger) - .whileTrue(new CoralOutake(coralEffector)) + .whileTrue(new CoralOuttake(coralEffector)) .whileFalse(new CoralIntake(coralEffector)); axisTrigger(manipulatorController, Axis.kRightTrigger) - .whileTrue(new ManualCoralIntake()); + .whileTrue(new CoralIntakeManual(coralEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index c668f58..88ce84a 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command;; +import edu.wpi.first.wpilibj2.command.Command; public class CoralIntake extends Command { // public static double spin; @@ -54,7 +54,7 @@ public void execute() { // spin = 10; // SmartDashboard.putNumber("spin", spin); //SmartDashboard.putNumber("timer", timer.get()); - // SmartDashboard.getBoolean("outakeGet", coralIn); + // SmartDashboard.getBoolean("outtakeGet", coralIn); if (coralEffector.coralIn){ coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations diff --git a/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntakeManual.java similarity index 78% rename from src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java rename to src/main/java/org/carlmontrobotics/commands/CoralIntakeManual.java index 6dbc1d6..794f697 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManualCoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntakeManual.java @@ -1,23 +1,20 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package org.carlmontrobotics.commands; import static org.carlmontrobotics.Constants.CoralEffectorc.*; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import org.carlmontrobotics.subsystems.CoralEffector; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class ManualCoralIntake extends Command { - /** Creates a new ManualCoralIntake. */ +public class CoralIntakeManual extends Command { + /** Creates a new CoralIntakeManual. */ private CoralEffector coralEffector; Timer timer = new Timer(); - public ManualCoralIntake() { + public CoralIntakeManual(CoralEffector coralEffector) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(this.coralEffector = coralEffector); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java similarity index 85% rename from src/main/java/org/carlmontrobotics/commands/CoralOutake.java rename to src/main/java/org/carlmontrobotics/commands/CoralOuttake.java index 71ef215..3e33c62 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java @@ -10,11 +10,11 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -public class CoralOutake extends Command { +public class CoralOuttake extends Command { /** Creates a new SetCoralOut. */ private CoralEffector coralEffector; Timer timer; - public CoralOutake(CoralEffector coralEffector) { + public CoralOuttake(CoralEffector coralEffector) { addRequirements(this.coralEffector = coralEffector); // Use addRequirements() here to declare subsystem dependencies. timer = new Timer(); @@ -25,7 +25,7 @@ public CoralOutake(CoralEffector coralEffector) { public void initialize() { timer.reset(); timer.start(); - coralEffector.setMotorSpeed(OUTPUT_SPEED); +coralEffector.setMotorSpeed(OUTPUT_SPEED); } // Called every time the scheduler runs while the command is scheduled. @@ -49,6 +49,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > OUTAKE_TIME_OUT || coralEffector.distanceSensorSeesCoral()==false; + return timer.get() > OUTTAKE_TIME_OUT || !coralEffector.distanceSensorSeesCoral(); } } From 36b05f8930726dad45fc021d1dd2b420c71495dc Mon Sep 17 00:00:00 2001 From: FriedLongJohns - WindowsLaptop <81837862+FriedLongJohns@users.noreply.github.com> Date: Tue, 11 Mar 2025 17:23:27 -0500 Subject: [PATCH 20/26] disable limit switch --- .../java/org/carlmontrobotics/subsystems/CoralEffector.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index b8b720b..19ea242 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -34,7 +34,7 @@ public class CoralEffector extends SubsystemBase { public SparkFlex coralMotor = new SparkFlex(CORAL_MOTOR_PORT, MotorType.kBrushless); - public DigitalInput coralLimitSwitch = new DigitalInput(CORAL_LIMIT_SWITCH_PORT); + // public DigitalInput coralLimitSwitch = new DigitalInput(CORAL_LIMIT_SWITCH_PORT); //FIXME ADD THE LIMIT SWITCH!! public TimeOfFlight distanceSensor = new TimeOfFlight(CORAL_DISTANCE_SENSOR_PORT); From 713f48ee0a49d5bf6141c812f5d1d89144b3f7a3 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Tue, 11 Mar 2025 15:39:51 -0700 Subject: [PATCH 21/26] removed pid and changed commands to no --- .../org/carlmontrobotics/commands/CoralIntake.java | 14 ++++++-------- .../carlmontrobotics/subsystems/CoralEffector.java | 14 +++++++------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 88ce84a..e7911da 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -56,17 +56,15 @@ public void execute() { //SmartDashboard.putNumber("timer", timer.get()); // SmartDashboard.getBoolean("outtakeGet", coralIn); - if (coralEffector.coralIn){ - coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations - } - else if (coralEffector.distanceSensorSeesCoral()){ - coralEffector.setMotorSpeed(INPUT_FAST_SPEED); + // if (coralEffector.coralIn){ + // coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations + // } + if (coralEffector.distanceSensorSeesCoral()){ + coralEffector.setMotorSpeed(INPUT_SLOW_SPEED); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; } - else if (coralEffector.limitSwitchSeesCoral()){ - coralEffector.setMotorSpeed(INPUT_SLOW_SPEED); - } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 19ea242..47e51bd 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -63,10 +63,10 @@ public double getEncoderPos() { public void setMotorSpeed(double speed) { coralMotor.set(speed); } - public void setReferencePosition(double reference) { - targetPos = reference; - coralMotor.getClosedLoopController().setReference(reference, SparkBase.ControlType.kPosition); - } + // public void setReferencePosition(double reference) { + // targetPos = reference; + // coralMotor.getClosedLoopController().setReference(reference, SparkBase.ControlType.kPosition); + // } public boolean motorAtGoal(){ return Math.abs(coralEncoder.getPosition()-targetPos) <= CORAL_INTAKE_ERR; } @@ -82,9 +82,9 @@ public boolean distanceSensorSeesCoral(){ // coralIn = coralIsInside; // } - public boolean limitSwitchSeesCoral() { - return !coralLimitSwitch.get(); - } + // public boolean limitSwitchSeesCoral() { + // return !coralLimitSwitch.get(); + // } @Override public void periodic() { From 1532b331e3bcd12b75d755b813bb43a6de222c84 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Tue, 11 Mar 2025 16:05:02 -0700 Subject: [PATCH 22/26] Changed bindings and commands to no pid or motorPos detect --- src/main/java/org/carlmontrobotics/Constants.java | 6 +++--- src/main/java/org/carlmontrobotics/RobotContainer.java | 8 ++++++-- .../java/org/carlmontrobotics/commands/CoralIntake.java | 3 ++- .../org/carlmontrobotics/subsystems/CoralEffector.java | 2 +- 4 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 5a14b82..3ab3156 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -19,14 +19,14 @@ public static final class CoralEffectorc{ public final static int CORAL_DISTANCE_SENSOR_PORT = 6; public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; //mm public final static double CORAL_INTAKE_ERR = .1;//encoder units - rotations - public final static double INPUT_FAST_SPEED = 0.07; //TODO: tune this - public final static double INPUT_SLOW_SPEED = 0.035; //TODO: tune this + public final static double INPUT_FAST_SPEED = 0.5; //TODO: tune this + public final static double INPUT_SLOW_SPEED = 0.15; //TODO: tune this public final static double OUTPUT_SPEED = 0.1; //TODO: tune this public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; //TOD: tune this? public final static double KP = 1.3; //TODO: tune this public final static double KI = 0; public final static double KD = 0; - public final static double INTAKE_TIME_OUT = 0.5; + public final static double INTAKE_TIME_OUT = 0.25; public final static double OUTTAKE_TIME_OUT = 10; public final static double MANUAL_INTAKE_TIME_OUT = 1; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 5860970..e546aa2 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -23,6 +23,7 @@ //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //commands import edu.wpi.first.wpilibj2.command.Command; @@ -76,10 +77,13 @@ private void setBindingsManipulator() { // new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON) // .whileTrue(new ManualCoralIntake()); axisTrigger(manipulatorController, Axis.kLeftTrigger) - .whileTrue(new CoralOuttake(coralEffector)) - .whileFalse(new CoralIntake(coralEffector)); + .whileTrue(new CoralOuttake(coralEffector)); + + axisTrigger(manipulatorController, Axis.kRightTrigger) .whileTrue(new CoralIntakeManual(coralEffector)); + + new JoystickButton(manipulatorController, (Button.kA.value)).whileTrue(new CoralIntake(coralEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index e7911da..50ef110 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -63,6 +63,7 @@ public void execute() { coralEffector.setMotorSpeed(INPUT_SLOW_SPEED); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; + timer.start(); } } @@ -76,6 +77,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return coralEffector.motorAtGoal() || timer.get() > INTAKE_TIME_OUT; + return timer.get() > INTAKE_TIME_OUT; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 47e51bd..167c71c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -74,7 +74,7 @@ public boolean motorAtGoal(){ public boolean distanceSensorSeesCoral(){ return distanceSensor.getRange() < CORAL_DISTANCE_SENSOR_DISTANCE; } - + // public boolean coralIsIn() { // return coralIn; // } From 010905c4a118bc2c7c3cbb727c7c1e5dd4dad0e6 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Tue, 11 Mar 2025 17:41:00 -0700 Subject: [PATCH 23/26] did funny stuff to get around pid --- .../java/org/carlmontrobotics/Constants.java | 8 ++++---- .../org/carlmontrobotics/RobotContainer.java | 12 ++++++++---- .../commands/CoralIntake.java | 19 +++++++++++++++---- .../commands/CoralOuttake.java | 2 +- .../commands/hkasjhkasdg.java | 16 ++++++++++++++++ .../subsystems/CoralEffector.java | 9 ++++++--- 6 files changed, 50 insertions(+), 16 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 3ab3156..d61d53b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -19,8 +19,8 @@ public static final class CoralEffectorc{ public final static int CORAL_DISTANCE_SENSOR_PORT = 6; public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; //mm public final static double CORAL_INTAKE_ERR = .1;//encoder units - rotations - public final static double INPUT_FAST_SPEED = 0.5; //TODO: tune this - public final static double INPUT_SLOW_SPEED = 0.15; //TODO: tune this + public final static double INPUT_FAST_SPEED = 0.2; //TODO: tune this + public final static double INPUT_SLOW_SPEED = 0.1; //TODO: tune this public final static double OUTPUT_SPEED = 0.1; //TODO: tune this public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; //TOD: tune this? public final static double KP = 1.3; //TODO: tune this @@ -29,7 +29,7 @@ public static final class CoralEffectorc{ public final static double INTAKE_TIME_OUT = 0.25; public final static double OUTTAKE_TIME_OUT = 10; public final static double MANUAL_INTAKE_TIME_OUT = 1; - - public final static int LIMIT_SWITCH_PORT = 7; //TODO: Change + public final static double COAST_VELOCITY_AT_INPUT = 400; + //public final static int LIMIT_SWITCH_PORT = 7; //TODO: Change } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index e546aa2..d898d99 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -15,6 +15,7 @@ import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.CoralIntake; import org.carlmontrobotics.commands.CoralOuttake; +import org.carlmontrobotics.commands.hkasjhkasdg; import org.carlmontrobotics.commands.CoralIntakeManual; import org.carlmontrobotics.subsystems.CoralEffector; @@ -50,7 +51,7 @@ public class RobotContainer { public final GenericHID manipulatorController = new GenericHID(MANIPULATOR_CONTROLLER_PORT); public final CoralEffector coralEffector = new CoralEffector(); - public final DigitalInput limitSwitch = new DigitalInput(LIMIT_SWITCH_PORT); + //public final DigitalInput limitSwitch = new DigitalInput(LIMIT_SWITCH_PORT); public RobotContainer() { SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector)); @@ -76,12 +77,15 @@ private void setBindingsManipulator() { // .whileFalse(new CoralIntake(coralEffector)); // new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON) // .whileTrue(new ManualCoralIntake()); - axisTrigger(manipulatorController, Axis.kLeftTrigger) - .whileTrue(new CoralOuttake(coralEffector)); + axisTrigger(manipulatorController, Axis.kLeftTrigger) + .whileTrue(new CoralIntake(coralEffector)) + .onFalse(new hkasjhkasdg(coralEffector)); + axisTrigger(manipulatorController, Axis.kRightTrigger) - .whileTrue(new CoralIntakeManual(coralEffector)); + .whileTrue(new CoralIntakeManual(coralEffector)) + .onFalse(new hkasjhkasdg(coralEffector)); new JoystickButton(manipulatorController, (Button.kA.value)).whileTrue(new CoralIntake(coralEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 50ef110..9b9a23e 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -15,6 +15,7 @@ public class CoralIntake extends Command { // Timer coralInTimer = new Timer(); public static double coralMotorPosition; private CoralEffector coralEffector; + private boolean isIn = false; public CoralIntake(CoralEffector coralEffector) { // Use addRequirements() here to declare subsystem dependencies. @@ -26,11 +27,18 @@ public CoralIntake(CoralEffector coralEffector) { @Override public void initialize() { coralEffector.setMotorSpeed(INPUT_FAST_SPEED); + timer.stop(); + timer.reset(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(coralEffector.getVel() < COAST_VELOCITY_AT_INPUT && coralEffector.distanceSensorSeesCoral()) { + isIn=true; + } else { + isIn = false; + } // if (CoralEffector.distanceSensorSees) { // coralIn = true; // if (CoralEffector.limitSwitchSees) { @@ -59,11 +67,13 @@ public void execute() { // if (coralEffector.coralIn){ // coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations // } - if (coralEffector.distanceSensorSeesCoral()){ - coralEffector.setMotorSpeed(INPUT_SLOW_SPEED); + if (coralEffector.distanceSensorSeesCoral() && isIn == true){ + coralEffector.setMotorSpeed(INPUT_SLOW_SPEED/2); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; - timer.start(); + // timer.start(); + } else { + coralEffector.setMotorSpeed(INPUT_FAST_SPEED); } } @@ -72,11 +82,12 @@ public void execute() { @Override public void end(boolean interrupted) { coralEffector.setMotorSpeed(0); + isIn = false; } // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > INTAKE_TIME_OUT; + return false; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java index 3e33c62..239d6c6 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java @@ -25,7 +25,7 @@ public CoralOuttake(CoralEffector coralEffector) { public void initialize() { timer.reset(); timer.start(); -coralEffector.setMotorSpeed(OUTPUT_SPEED); +coralEffector.setMotorSpeed(-OUTPUT_SPEED+0.4); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java b/src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java new file mode 100644 index 0000000..2a00a76 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java @@ -0,0 +1,16 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj2.command.Command; + +public class hkasjhkasdg extends Command { + CoralEffector uk; + public hkasjhkasdg(CoralEffector uk) { + this.uk=uk; + } + @Override + public void initialize() { + uk.setMotorSpeed(0); + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 167c71c..b7fdc88 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -48,7 +48,7 @@ public class CoralEffector extends SubsystemBase { SparkFlexConfig config = new SparkFlexConfig(); public CoralEffector(){ config - .inverted(false) + .inverted(true) .idleMode(IdleMode.kBrake); config.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) @@ -70,9 +70,11 @@ public void setMotorSpeed(double speed) { public boolean motorAtGoal(){ return Math.abs(coralEncoder.getPosition()-targetPos) <= CORAL_INTAKE_ERR; } - + public double getVel() { + return coralEncoder.getVelocity(); + } public boolean distanceSensorSeesCoral(){ - return distanceSensor.getRange() < CORAL_DISTANCE_SENSOR_DISTANCE; + return distanceSensor.getRange() < CORAL_DISTANCE_SENSOR_DISTANCE-50; } // public boolean coralIsIn() { @@ -101,5 +103,6 @@ public void periodic() { SmartDashboard.putNumber("Coral Speed", coralEncoder.getVelocity()); SmartDashboard.putNumber("coral output", coralOutput); SmartDashboard.getNumber("P", KP); +//coralMotor.set(INPUT_FAST_SPEED); } } \ No newline at end of file From 6185dee7cae05d7a60b3473e22da36c63ea20be4 Mon Sep 17 00:00:00 2001 From: aaronC34 Date: Wed, 12 Mar 2025 00:22:40 -0700 Subject: [PATCH 24/26] sleep deprived ooga booga solution TODO: test --- .../commands/CoralIntake.java | 16 +++--- .../carlmontrobotics/commands/TapCoralIn.java | 55 +++++++++++++++++++ 2 files changed, 64 insertions(+), 7 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/TapCoralIn.java diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 9b9a23e..9c6ffe3 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -15,7 +15,8 @@ public class CoralIntake extends Command { // Timer coralInTimer = new Timer(); public static double coralMotorPosition; private CoralEffector coralEffector; - private boolean isIn = false; + private boolean isSeen = false; + private boolean isTouching = false; public CoralIntake(CoralEffector coralEffector) { // Use addRequirements() here to declare subsystem dependencies. @@ -34,10 +35,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(coralEffector.getVel() < COAST_VELOCITY_AT_INPUT && coralEffector.distanceSensorSeesCoral()) { - isIn=true; + isSeen = coralEffector.distanceSensorSeesCoral(); + if(coralEffector.getVel() < COAST_VELOCITY_AT_INPUT && isSeen) { + isTouching=true; } else { - isIn = false; + isTouching = false; } // if (CoralEffector.distanceSensorSees) { // coralIn = true; @@ -67,7 +69,7 @@ public void execute() { // if (coralEffector.coralIn){ // coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations // } - if (coralEffector.distanceSensorSeesCoral() && isIn == true){ + if (coralEffector.distanceSensorSeesCoral() && isSeen == true){ coralEffector.setMotorSpeed(INPUT_SLOW_SPEED/2); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; @@ -82,12 +84,12 @@ public void execute() { @Override public void end(boolean interrupted) { coralEffector.setMotorSpeed(0); - isIn = false; + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return isTouching && isSeen; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java b/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java new file mode 100644 index 0000000..94265ae --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + + +import org.carlmontrobotics.Constants.CoralEffectorc; +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class TapCoralIn extends Command { + /** Creates a new SetCoralOut. */ + private CoralEffector coralEffector; + Timer timer; + public TapCoralIn(CoralEffector coralEffector) { + addRequirements(this.coralEffector = coralEffector); + // Use addRequirements() here to declare subsystem dependencies. + timer = new Timer(); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + coralEffector.setMotorSpeed(CoralEffectorc.INPUT_SLOW_SPEED); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { +/* if (coralEffector.coralDetected()) { + coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed); + timer.reset(); + timer.start(); + } + else */ + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + coralEffector.setMotorSpeed(0); + //coralEffector.setCoralIn(false); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > .2 || coralEffector.coralIn == true; + } +} From 633ff9e9d54c2f1359decde7a7d734fc77c204c0 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Wed, 12 Mar 2025 09:44:04 -0700 Subject: [PATCH 25/26] renamed hkasjhkasdg to StopCoralMotor because... hkasjhkasdg is not a good name --- .../java/org/carlmontrobotics/Constants.java | 2 +- .../org/carlmontrobotics/RobotContainer.java | 6 +++--- .../carlmontrobotics/commands/CoralOuttake.java | 2 +- .../commands/StopCoralMotor.java | 16 ++++++++++++++++ .../carlmontrobotics/commands/hkasjhkasdg.java | 16 ---------------- 5 files changed, 21 insertions(+), 21 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/StopCoralMotor.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index d61d53b..02ca956 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -21,7 +21,7 @@ public static final class CoralEffectorc{ public final static double CORAL_INTAKE_ERR = .1;//encoder units - rotations public final static double INPUT_FAST_SPEED = 0.2; //TODO: tune this public final static double INPUT_SLOW_SPEED = 0.1; //TODO: tune this - public final static double OUTPUT_SPEED = 0.1; //TODO: tune this + public final static double OUTPUT_SPEED = 0.5; //TODO: tune this public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; //TOD: tune this? public final static double KP = 1.3; //TODO: tune this public final static double KI = 0; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index d898d99..885e7d1 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -15,7 +15,7 @@ import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.CoralIntake; import org.carlmontrobotics.commands.CoralOuttake; -import org.carlmontrobotics.commands.hkasjhkasdg; +import org.carlmontrobotics.commands.StopCoralMotor; import org.carlmontrobotics.commands.CoralIntakeManual; import org.carlmontrobotics.subsystems.CoralEffector; @@ -80,12 +80,12 @@ private void setBindingsManipulator() { axisTrigger(manipulatorController, Axis.kLeftTrigger) .whileTrue(new CoralIntake(coralEffector)) - .onFalse(new hkasjhkasdg(coralEffector)); + .onFalse(new StopCoralMotor(coralEffector)); axisTrigger(manipulatorController, Axis.kRightTrigger) .whileTrue(new CoralIntakeManual(coralEffector)) - .onFalse(new hkasjhkasdg(coralEffector)); + .onFalse(new StopCoralMotor(coralEffector)); new JoystickButton(manipulatorController, (Button.kA.value)).whileTrue(new CoralIntake(coralEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java index 239d6c6..ad07c49 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java @@ -25,7 +25,7 @@ public CoralOuttake(CoralEffector coralEffector) { public void initialize() { timer.reset(); timer.start(); -coralEffector.setMotorSpeed(-OUTPUT_SPEED+0.4); +coralEffector.setMotorSpeed(-OUTPUT_SPEED); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/org/carlmontrobotics/commands/StopCoralMotor.java b/src/main/java/org/carlmontrobotics/commands/StopCoralMotor.java new file mode 100644 index 0000000..37f26a4 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/StopCoralMotor.java @@ -0,0 +1,16 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj2.command.Command; + +public class StopCoralMotor extends Command { + CoralEffector coralEffector; + public StopCoralMotor(CoralEffector coralEffector) { + this.coralEffector=coralEffector; + } + @Override + public void initialize() { + coralEffector.setMotorSpeed(0); + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java b/src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java deleted file mode 100644 index 2a00a76..0000000 --- a/src/main/java/org/carlmontrobotics/commands/hkasjhkasdg.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.subsystems.CoralEffector; - -import edu.wpi.first.wpilibj2.command.Command; - -public class hkasjhkasdg extends Command { - CoralEffector uk; - public hkasjhkasdg(CoralEffector uk) { - this.uk=uk; - } - @Override - public void initialize() { - uk.setMotorSpeed(0); - } -} From 1764fbf290d803ae83550a90c21824297cb1b731 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Wed, 12 Mar 2025 18:36:32 -0700 Subject: [PATCH 26/26] adeed mre monkey stuff --- .../java/org/carlmontrobotics/Constants.java | 2 +- .../org/carlmontrobotics/RobotContainer.java | 3 +- .../commands/CoralIntake.java | 30 ++++++++++++------- .../commands/CoralOuttake.java | 1 + .../carlmontrobotics/commands/TapCoralIn.java | 2 +- .../subsystems/CoralEffector.java | 9 ++++-- 6 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 02ca956..553e56c 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -29,7 +29,7 @@ public static final class CoralEffectorc{ public final static double INTAKE_TIME_OUT = 0.25; public final static double OUTTAKE_TIME_OUT = 10; public final static double MANUAL_INTAKE_TIME_OUT = 1; - public final static double COAST_VELOCITY_AT_INPUT = 400; + public final static double COAST_VELOCITY_AT_INPUT = 300; //public final static int LIMIT_SWITCH_PORT = 7; //TODO: Change } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 885e7d1..904a4fc 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -16,6 +16,7 @@ import org.carlmontrobotics.commands.CoralIntake; import org.carlmontrobotics.commands.CoralOuttake; import org.carlmontrobotics.commands.StopCoralMotor; +import org.carlmontrobotics.commands.TapCoralIn; import org.carlmontrobotics.commands.CoralIntakeManual; import org.carlmontrobotics.subsystems.CoralEffector; @@ -87,7 +88,7 @@ private void setBindingsManipulator() { .whileTrue(new CoralIntakeManual(coralEffector)) .onFalse(new StopCoralMotor(coralEffector)); - new JoystickButton(manipulatorController, (Button.kA.value)).whileTrue(new CoralIntake(coralEffector)); + new JoystickButton(manipulatorController, (Button.kA.value)).onTrue(new TapCoralIn(coralEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java index 9c6ffe3..5409184 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralIntake.java @@ -17,7 +17,8 @@ public class CoralIntake extends Command { private CoralEffector coralEffector; private boolean isSeen = false; private boolean isTouching = false; - + private boolean firstTime = true; + private boolean bruh = false; public CoralIntake(CoralEffector coralEffector) { // Use addRequirements() here to declare subsystem dependencies. // addRequirements(this.CoralEffector = CoralEffector); @@ -35,12 +36,18 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - isSeen = coralEffector.distanceSensorSeesCoral(); - if(coralEffector.getVel() < COAST_VELOCITY_AT_INPUT && isSeen) { - isTouching=true; - } else { - isTouching = false; + if(firstTime) { + if(coralEffector.distanceSensorSeesCoral()) { + firstTime = false; + bruh = coralEffector.distanceSensorSeesCoral(); + } + } + // if(coralEffector.getVel() < COAST_VELOCITY_AT_INPUT && isSeen) { + // isTouching=true; + // } else { + // isTouching = false; + //} // if (CoralEffector.distanceSensorSees) { // coralIn = true; // if (CoralEffector.limitSwitchSees) { @@ -69,11 +76,11 @@ public void execute() { // if (coralEffector.coralIn){ // coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations // } - if (coralEffector.distanceSensorSeesCoral() && isSeen == true){ + if (coralEffector.distanceSensorSeesCoral()){ coralEffector.setMotorSpeed(INPUT_SLOW_SPEED/2); coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations coralEffector.coralIn=true; - // timer.start(); + timer.start(); } else { coralEffector.setMotorSpeed(INPUT_FAST_SPEED); } @@ -84,12 +91,15 @@ public void execute() { @Override public void end(boolean interrupted) { coralEffector.setMotorSpeed(0); - + bruh = false; + if(interrupted != true) { + firstTime = true; + } } // Returns true when the command should end. @Override public boolean isFinished() { - return isTouching && isSeen; + return !coralEffector.distanceSensorSeesCoral() && bruh; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java index ad07c49..a2452a9 100644 --- a/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java +++ b/src/main/java/org/carlmontrobotics/commands/CoralOuttake.java @@ -44,6 +44,7 @@ public void execute() { public void end(boolean interrupted) { coralEffector.setMotorSpeed(0); coralEffector.coralIn = false; + coralEffector.setIn(false); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java b/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java index 94265ae..b9c006d 100644 --- a/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java +++ b/src/main/java/org/carlmontrobotics/commands/TapCoralIn.java @@ -50,6 +50,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > .2 || coralEffector.coralIn == true; + return timer.get() > .2; //|| coralEffector.coralIn == true; } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index b7fdc88..3498ae2 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -38,7 +38,7 @@ public class CoralEffector extends SubsystemBase { //FIXME ADD THE LIMIT SWITCH!! public TimeOfFlight distanceSensor = new TimeOfFlight(CORAL_DISTANCE_SENSOR_PORT); - + private boolean isIn = false; public final RelativeEncoder coralEncoder = coralMotor.getEncoder(); private double coralOutput; @@ -87,7 +87,12 @@ public boolean distanceSensorSeesCoral(){ // public boolean limitSwitchSeesCoral() { // return !coralLimitSwitch.get(); // } - + public boolean getIn() { + return isIn; + } + public void setIn(boolean in) { + isIn = in; + } @Override public void periodic() { //coralMotor.getClosedLoopController().setReference(1, ControlType.kVelocity);