From 7aa53e24cff1a943a05b2d7276701336db5a86ad Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Sun, 28 Jan 2024 21:10:48 -0700 Subject: [PATCH 01/10] Update README.md added a list of tele-op operation modes. --- README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/README.md b/README.md index 5dc92e3..0681577 100644 --- a/README.md +++ b/README.md @@ -7,3 +7,21 @@ our base code is derived from [this example swerve code](https://github.com/dirt ## Play Structure Our robot is designed to be a fast cycle, floor pickup, and speaker shooting bot. We are (planning) on using a 4 bar extension with 2 pairs of bars with 2 inch compliant wheels as our intake. The intake will directly feed to our launcher, which will have 2 pairs of wheels connected with a belt. The launcher itself will be variable angle, allowing us to be able to shoot from up close, and afar. We will have a camera on the front of the launcher to see the april tags on the speaker, and will use auto alignment to shoot. + +| Color | Code | Name | Function | +| --- | --- | --- | --- | +| 🟪 Purple | D | Default | Default Mode. Intake in, no note, normal operation. | +| Intake Modes | +| 🟥 Red Blink | IO | Input Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| +| 🟥 Red | IL | Input Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| +| 🟧 Orange | R | Ready | Intake in with note, searching for speaker april tags | +| Scoring Modes | +| 🟧 Orange Blink | R | Ready | Intake in with note, speaker april tags found, ready to lock | +| 🟨 Yellow | KP | Lock Pending| Locking on speaker, speed greatly reduced, no rotation control | +| 🟩 Green | K | Locked | locked, preparing to shoot. | +| 🟦 Blue | L | Launching | Cycling launcher | +| Endgame Modes | +| 🟨🟥 Yellow Red | CE | Climber Extending | Extending climber decreased speed | +| 🟨🟧 Yellow Orange | CR | Climber Ready | Climber ready to climb, greatly decreased speed | +| 🟨🟦 Yellow Blue | C | Climbing | Climbing chain, movement disabled | +| 🟨🟩 Yellow Green | C | Climber Locked | Climbing commplete, locked | From 1d4d66c221da8a6e080f717b31fecefa6fcf7e9e Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Tue, 30 Jan 2024 16:53:34 -0700 Subject: [PATCH 02/10] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 0681577..bf15ec5 100644 --- a/README.md +++ b/README.md @@ -12,8 +12,8 @@ Our robot is designed to be a fast cycle, floor pickup, and speaker shooting bot | --- | --- | --- | --- | | 🟪 Purple | D | Default | Default Mode. Intake in, no note, normal operation. | | Intake Modes | -| 🟥 Red Blink | IO | Input Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| -| 🟥 Red | IL | Input Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| +| 🟥 Red Blink | IO | Intake Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| +| 🟥 Red | IL | Intake Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| | 🟧 Orange | R | Ready | Intake in with note, searching for speaker april tags | | Scoring Modes | | 🟧 Orange Blink | R | Ready | Intake in with note, speaker april tags found, ready to lock | From 96f74bd7bb79f85a38a98e2f8edefb75d10b1f8b Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Tue, 6 Feb 2024 16:24:00 -0700 Subject: [PATCH 03/10] work --- README.md | 6 + src/main/java/frc/robot/Constants.java | 54 +++++++ src/main/java/frc/robot/Robot.java | 5 - src/main/java/frc/robot/RobotContainer.java | 20 ++- .../java/frc/robot/commands/IntakeRun.java | 43 +++++ .../java/frc/robot/commands/TeleopSwerve.java | 16 +- .../java/frc/robot/subsystems/Intake.java | 105 ++++++++++++ src/main/java/frc/robot/subsystems/LEDS.java | 110 ++++++++++++- .../java/frc/robot/subsystems/Launcher.java | 56 +++++++ .../java/frc/robot/subsystems/Swerve.java | 2 +- .../java/frc/robot/subsystems/camera.java | 15 ++ vendordeps/Phoenix5.json | 151 ++++++++++++++++++ vendordeps/REVLib.json | 74 +++++++++ 13 files changed, 640 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IntakeRun.java create mode 100644 src/main/java/frc/robot/subsystems/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/Launcher.java create mode 100644 src/main/java/frc/robot/subsystems/camera.java create mode 100644 vendordeps/Phoenix5.json create mode 100644 vendordeps/REVLib.json diff --git a/README.md b/README.md index bf15ec5..057cc70 100644 --- a/README.md +++ b/README.md @@ -12,9 +12,15 @@ Our robot is designed to be a fast cycle, floor pickup, and speaker shooting bot | --- | --- | --- | --- | | 🟪 Purple | D | Default | Default Mode. Intake in, no note, normal operation. | | Intake Modes | +<<<<<<< Updated upstream | 🟥 Red Blink | IO | Intake Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| | 🟥 Red | IL | Intake Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| | 🟧 Orange | R | Ready | Intake in with note, searching for speaker april tags | +======= +| 🟥 Red Blink | IO | Input Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| +| 🟥 Red | IL | Input Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| +| 🟧 Orange | N | Note | Intake in with note, searching for speaker april tags | +>>>>>>> Stashed changes | Scoring Modes | | 🟧 Orange Blink | R | Ready | Intake in with note, speaker april tags found, ready to lock | | 🟨 Yellow | KP | Lock Pending| Locking on speaker, speed greatly reduced, no rotation control | diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 339347b..5b45c68 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; import frc.lib.util.COTSTalonFXSwerveConstants; import frc.lib.util.SwerveModuleConstants; @@ -129,6 +130,58 @@ public static final class Mod3 { //TODOx: This must be tuned to specific robot D } } + public static final class Intake { + public static final double noteDetectionDistance = 30; + public static final double barP = 5e-5; + public static final double barI = 1e-6; + public static final double barD = 0; + + public static final double barMaxOut = 1; + public static final double barMinOut = -1; + + public static final double barMaxRPM = 5700; + public static final double barMaxVel = 2000; + public static final double barMaxAcc = 1500; + + public static final double barOutPoint = 1000; + public static final double barInPoint = -1000; + } + + // holds constants for mode system + public static final class Mode { + public static final double blinkTime = 5.0; + // Mode modifiers, in percentages, controlor input is multiplied by these + // max forward speed (1) * 40% speed (.40) = .40 output + public static final class Modifiers { + public static final class Intake { + public static final double driveSpeed = 0.4; + public static final double rotSpeed = 0.4; + } + public static final class Lock { + public static final double driveSpeed = 0.3; + public static final double rotSpeed = 0.0; + } + } + + public static final class Colors { + public static final Color D = Color.kPurple; // Default Color + public static final Color I = Color.kRed; // Intake Color + public static final Color IO = Color.kDarkRed; // Intake Out Blink Color + public static final Color R = Color.kOrange; // Ready/Note Color + public static final Color RB = Color.kDarkOrange; // Ready Blink Color + public static final Color KP = Color.kYellow;// Lock Pending color + public static final Color K = Color.kGreen; // Lock color + public static final Color L = Color.kSkyBlue;// Launching color + + public static final Color C = Color.kYellow; // Climber Blink Color + public static final Color CE = Color.kRed; // Climber Extenidng other Blink Color + public static final Color CR = Color.kOrange; // Climber Ready other Blink Color + public static final Color CC = Color.kSkyBlue; // Climber Climbing other Blink Color + public static final Color CL = Color.kGreen; // Climber Locked other Blink Color + + } + } + public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot public static final double kMaxSpeedMetersPerSecond = 2; public static final double kMaxAccelerationMetersPerSecondSquared = 2; @@ -144,4 +197,5 @@ public static final class AutoConstants { //TODO: The below constants are used i new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); } + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d5fec09..555f917 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,11 +4,6 @@ package frc.robot; -import com.ctre.phoenix6.Orchestra; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f901f3..07066de 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,6 +3,9 @@ import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -28,15 +31,16 @@ public class RobotContainer { // This needs to be cleaned up to a sendable choser for the smart dashboard to use // also once perephials are added, will need to add 2nd note manipulation controler - // private final Joystick driver = new Joystick(1); private final Joystick driver = new Joystick(0); + private final XboxController secondary = new XboxController(1); // /* XBOX CONTROLOR */ - // /* Drive Controls */ - // private final int translationAxis = XboxController.Axis.kLeftY.value; - // private final int strafeAxis = XboxController.Axis.kLeftX.value; - // private final int rotationAxis = XboxController.Axis.kRightX.value; + /* Drive Controls */ + private final int sLy = XboxController.Axis.kLeftY.value; + private final int sLX = XboxController.Axis.kLeftX.value; + private final int sRX = XboxController.Axis.kRightX.value; + private final int sRY = XboxController.Axis.kRightY.value; // /* Driver Buttons */ // private final JoystickButton zeroGyro = new JoystickButton(driver, XboxController.Button.kY.value); @@ -51,6 +55,7 @@ public class RobotContainer { /* Driver Buttons */ private final JoystickButton zeroGyro = new JoystickButton(driver, Joystick.ButtonType.kTrigger.value); private final JoystickButton robotCentric = new JoystickButton(driver, Joystick.ButtonType.kTop.value); + private final JoystickButton Test = new JoystickButton(driver, 3); /* Subsystems */ private final LEDS theLEDs = new LEDS(9,60); @@ -77,7 +82,8 @@ public RobotContainer() { () -> -driver.getRawAxis(translationAxis), () -> -driver.getRawAxis(strafeAxis), () -> -driver.getRawAxis(rotationAxis), - () -> robotCentric.getAsBoolean() + () -> robotCentric.getAsBoolean(), + theLEDs ) ); @@ -93,7 +99,6 @@ public RobotContainer() { SmartDashboard.putData("Music Choser",musiChooser); theLEDs.SetFull(255, 60, 0); - } /** @@ -105,6 +110,7 @@ public RobotContainer() { private void configureButtonBindings() { /** Driver Buttons */ zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); + } diff --git a/src/main/java/frc/robot/commands/IntakeRun.java b/src/main/java/frc/robot/commands/IntakeRun.java new file mode 100644 index 0000000..099af13 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeRun.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Intake; + +public class IntakeRun extends Command { + + private Intake s_Intake; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeRun(Intake s_Intake) { + this.s_Intake = s_Intake; + } + + @Override + public void initialize() { + // tell the intake to extend + s_Intake.setDeploy(true); + } + + @Override + public void execute() { + // spin the intake + s_Intake.runIntake(); + } + + @Override + public void end(boolean interrupted) { + // tell the intake to retratct + s_Intake.setDeploy(false); + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + return(s_Intake.checkForNote() || false/*<--- insert manual overide check here*/); + } + +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index c4fa75e..7bd0fc3 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,6 +1,7 @@ package frc.robot.commands; import frc.robot.Constants; +import frc.robot.subsystems.LEDS; import frc.robot.subsystems.Swerve; import java.util.function.BooleanSupplier; @@ -17,8 +18,18 @@ public class TeleopSwerve extends Command { private DoubleSupplier strafeSup; private DoubleSupplier rotationSup; private BooleanSupplier robotCentricSup; - - public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, BooleanSupplier robotCentricSup) { + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, LEDS led) { this.s_Swerve = s_Swerve; addRequirements(s_Swerve); @@ -26,6 +37,7 @@ public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli this.strafeSup = strafeSup; this.rotationSup = rotationSup; this.robotCentricSup = robotCentricSup; + this.led = led; } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..959d09b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,105 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; + +public class Intake extends SubsystemBase{ + + public VictorSPX m_intakeMotor1; + public VictorSPX m_intakeMotor2; + public CANSparkMax m_barMotor; + public RelativeEncoder barEncoder; + public SparkPIDController barPID; + public boolean deployed; + public AnalogInput ultrasonicSensor; + + public double kP, kI, kD, kMaxOut, kMinOut, maxRPM, maxVel, maxAcc; + + + public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, int ultrasonicSensorID) { + SmartDashboard.putNumber("test",3); + m_barMotor = new CANSparkMax(m_barMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + m_intakeMotor1 = new VictorSPX(m_intakeMotor1ID); + m_intakeMotor2 = new VictorSPX(m_intakeMotor2ID); + ultrasonicSensor = new AnalogInput(ultrasonicSensorID); + + barEncoder = m_barMotor.getEncoder(); + barPID = m_barMotor.getPIDController(); + + kP = Constants.Intake.barP; + kI = Constants.Intake.barI; + kD = Constants.Intake.barD; + + kMaxOut = Constants.Intake.barMaxOut; + kMinOut = Constants.Intake.barMinOut; + + maxRPM = Constants.Intake.barMaxRPM; + maxVel = Constants.Intake.barMaxVel; + maxAcc = Constants.Intake.barMaxAcc; + + // set PID coefficients + barPID.setP(kP); + barPID.setI(kI); + barPID.setD(kD); + barPID.setOutputRange(kMinOut, kMaxOut); + + + + int smartMotionSlot = 0; + barPID.setSmartMotionMaxVelocity(maxVel, smartMotionSlot); + barPID.setSmartMotionMinOutputVelocity(0, smartMotionSlot); + barPID.setSmartMotionMaxAccel(maxAcc, smartMotionSlot); + barPID.setSmartMotionAllowedClosedLoopError(0, smartMotionSlot); + + } + + public void runIntake() { + m_intakeMotor1.set(VictorSPXControlMode.Velocity, Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.Velocity, Constants.Intake.intakeVel); + } + + public void breakIntake(){ + m_intakeMotor1.setNeutralMode(NeutralMode.Brake); + m_intakeMotor2.setNeutralMode(NeutralMode.Brake); + } + + public boolean checkForNote() { + return (ultrasonicSensor.getValue() <= Constants.Intake.noteDetectionDistance); + } + + public void toggleDeploy() { + if(!deployed) { + // refrence this code for precice neo control through the spark max + // https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Smart%20Motion%20Example/src/main/java/frc/robot/Robot.java + + barPID.setReference(Constants.Intake.barOutPoint, CANSparkMax.ControlType.kSmartMotion); + deployed = true; + } else { + barPID.setReference(Constants.Intake.barInPoint, CANSparkMax.ControlType.kSmartMotion); + deployed = false; + } + } + + public void setDeploy(boolean mode) { + if(mode && !deployed) { + barPID.setReference(Constants.Intake.barOutPoint, CANSparkMax.ControlType.kSmartMotion); + deployed = true; + } else if(!mode && deployed) { + barPID.setReference(Constants.Intake.barInPoint, CANSparkMax.ControlType.kSmartMotion); + deployed = false; + } + } + + + +} diff --git a/src/main/java/frc/robot/subsystems/LEDS.java b/src/main/java/frc/robot/subsystems/LEDS.java index 53c0044..2a723cc 100644 --- a/src/main/java/frc/robot/subsystems/LEDS.java +++ b/src/main/java/frc/robot/subsystems/LEDS.java @@ -1,13 +1,25 @@ package frc.robot.subsystems; +import java.util.Arrays; +import java.util.logging.LogManager; + import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.Constants; + public class LEDS { private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; private int rainbowValue = 0; + private Timer timer; + private String mode; + private String lastMode; + private double nextBlink; + private boolean blink; /** * Master LED Controler. manages the ligts for ease of use. use WS2812B individualy addressable LED strips @@ -17,13 +29,19 @@ public class LEDS { */ public LEDS(int PWM, int length) { - m_led = new AddressableLED(9); + m_led = new AddressableLED(PWM); - m_ledBuffer = new AddressableLEDBuffer(60); + m_ledBuffer = new AddressableLEDBuffer(length); m_led.setLength(m_ledBuffer.getLength()); m_led.setData(m_ledBuffer); m_led.start(); + + timer = new Timer(); + nextBlink = timer.get() + Constants.Mode.blinkTime; + + mode = "D"; + lastMode = "N"; } // public int[] HEXtoRGB(String hex){ @@ -48,6 +66,20 @@ public void SetFull(int R, int G, int B) { m_led.setData(m_ledBuffer); } + /** + * Set the full strip using a {@link Color} + * + * @param clr {@link Color} to set to + */ + public void SetFull(Color clr) { + // For every Pixel + for (var i = 0; i < m_ledBuffer.getLength(); i++) { + // Sets the specified LED to the Color provided + m_ledBuffer.setLED(i, clr); + } + m_led.setData(m_ledBuffer); + } + /** * Set the entire strip to one volor using HSV * @@ -64,6 +96,80 @@ public void SetFullHSV(int H, int S, int V) { m_led.setData(m_ledBuffer); } + public void updateMode() { + if (mode != lastMode) { + switch (mode) { + case "D": + SetFull(Constants.Mode.Colors.D); + case "IO": + SetFull(Constants.Mode.Colors.I); + case "IL": + SetFull(Constants.Mode.Colors.I); + case "N": + SetFull(Constants.Mode.Colors.R); + case "R": + SetFull(Constants.Mode.Colors.R); + case "K": + SetFull(Constants.Mode.Colors.K); + case "KP": + SetFull(Constants.Mode.Colors.KP); + case "L": + SetFull(Constants.Mode.Colors.L); + case "CE": + SetFull(Constants.Mode.Colors.C); + case "CR": + SetFull(Constants.Mode.Colors.C); + case "C": + SetFull(Constants.Mode.Colors.C); + case "CL": + SetFull(Constants.Mode.Colors.C); + default: + break; + } + } + + if(timer.get() >= nextBlink) { + blink = !blink; + switch (mode) { + case "IO": + if(blink) SetFull(Constants.Mode.Colors.I); + else SetFull(Constants.Mode.Colors.IO); + case "R": + if(blink) SetFull(Constants.Mode.Colors.I); + else SetFull(Constants.Mode.Colors.IO); + case "CE": + if(blink) SetFull(Constants.Mode.Colors.C); + else SetFull(Constants.Mode.Colors.CE); + case "CR": + if(blink) SetFull(Constants.Mode.Colors.C); + else SetFull(Constants.Mode.Colors.CR); + case "C": + if(blink) SetFull(Constants.Mode.Colors.C); + else SetFull(Constants.Mode.Colors.CC); + case "CL": + if(blink) SetFull(Constants.Mode.Colors.C); + else SetFull(Constants.Mode.Colors.CL); + default: + break; + } + + } + + + + } + + public void setMode(String modeID) { + String[] vModes = {"D","IO","IL","N","R","K","KP","L","CE","CR","C","CL"}; + if (Arrays.asList(vModes).contains(modeID)) { + mode = modeID; + } + } + + public String getMode() { + return mode; + } + /* * Rainbow function, called perodicly, shifts a rainbow through the strip */ diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java new file mode 100644 index 0000000..0efb1a6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Launcher extends SubsystemBase { + public VictorSPX m_flyMotor1; + public VictorSPX m_flyMotor2; + public CANSparkMax m_screwMotor; + public RelativeEncoder screwEncoder; + public SparkPIDController screwPID; + + public double kP, kI, kD, kMaxOut, kMinOut, maxRPM, maxVel, maxAcc; + + public Launcher(int m_screwMotorId, int m_flyMotor1ID, int m_flyMotor2ID) { + + m_screwMotor = new CANSparkMax(m_screwMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + m_flyMotor1 = new VictorSPX(m_flyMotor1ID); + m_flyMotor2 = new VictorSPX(m_flyMotor2ID); + + screwEncoder = m_screwMotor.getEncoder(); + screwPID = m_screwMotor.getPIDController(); + + kP = Constants.Launcher.screwP; + kI = Constants.Launcher.screwI; + kD = Constants.Launcher.screwD; + + kMaxOut = Constants.Launcher.screwMaxOut; + kMinOut = Constants.Launcher.screwMinOut; + + maxRPM = Constants.Launcher.screwMaxRPM; + maxVel = Constants.Launcher.screwMaxVel; + maxAcc = Constants.Launcher.screwMaxAcc; + + // set PID coefficients + screwPID.setP(kP); + screwPID.setI(kI); + screwPID.setD(kD); + screwPID.setOutputRange(kMinOut, kMaxOut); + + + + int smartMotionSlot = 0; + screwPID.setSmartMotionMaxVelocity(maxVel, smartMotionSlot); + screwPID.setSmartMotionMinOutputVelocity(0, smartMotionSlot); + screwPID.setSmartMotionMaxAccel(maxAcc, smartMotionSlot); + screwPID.setSmartMotionAllowedClosedLoopError(0, smartMotionSlot); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f99cc7c..1026e07 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -194,8 +194,8 @@ public ChassisSpeeds getChasisHeading(){ @Override public void periodic(){ + //set Module dashboard values swerveOdometry.update(getGyroYaw(), getModulePositions()); - for(SwerveModule mod : mSwerveMods){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); diff --git a/src/main/java/frc/robot/subsystems/camera.java b/src/main/java/frc/robot/subsystems/camera.java new file mode 100644 index 0000000..2669261 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems; + +import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class camera { + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable table = inst.getTable("datatable"); + DoubleTopic dblTopic = inst.getDoubleTopic("/datatable/X"); + + + +} diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88a68dd --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 522c9c96de184232b0ed03e55f208c2a6b3e5343 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Wed, 7 Feb 2024 13:40:05 -0700 Subject: [PATCH 04/10] work --- README.md | 6 ---- src/main/java/frc/robot/Constants.java | 21 ++++++++++++++ src/main/java/frc/robot/Robot.java | 6 +++- src/main/java/frc/robot/RobotContainer.java | 29 +++++++++++++++++-- src/main/java/frc/robot/subsystems/Band.java | 10 +++---- .../java/frc/robot/subsystems/Intake.java | 2 +- 6 files changed, 59 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 057cc70..bf15ec5 100644 --- a/README.md +++ b/README.md @@ -12,15 +12,9 @@ Our robot is designed to be a fast cycle, floor pickup, and speaker shooting bot | --- | --- | --- | --- | | 🟪 Purple | D | Default | Default Mode. Intake in, no note, normal operation. | | Intake Modes | -<<<<<<< Updated upstream | 🟥 Red Blink | IO | Intake Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| | 🟥 Red | IL | Intake Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| | 🟧 Orange | R | Ready | Intake in with note, searching for speaker april tags | -======= -| 🟥 Red Blink | IO | Input Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| -| 🟥 Red | IL | Input Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| -| 🟧 Orange | N | Note | Intake in with note, searching for speaker april tags | ->>>>>>> Stashed changes | Scoring Modes | | 🟧 Orange Blink | R | Ready | Intake in with note, speaker april tags found, ready to lock | | 🟨 Yellow | KP | Lock Pending| Locking on speaker, speed greatly reduced, no rotation control | diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5b45c68..6098bef 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -145,6 +145,27 @@ public static final class Intake { public static final double barOutPoint = 1000; public static final double barInPoint = -1000; + + public static final double intakeVel = 2; + } + + + public static final class Launcher { + public static final double noteDetectionDistance = 30; + public static final double screwP = 5e-5; + public static final double screwI = 1e-6; + public static final double screwD = 0; + + public static final double screwMaxOut = 1; + public static final double screwMinOut = -1; + + public static final double screwMaxRPM = 3000; + public static final double screwMaxVel = 750; + public static final double screwMaxAcc = 500; + + public static final double screwRotPerDeg = 5; + + public static final double launcerVel = 2; } // holds constants for mode system diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 555f917..a2c07ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -46,11 +46,14 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + m_robotContainer.updateInfo(); } /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + m_robotContainer.disabledInit(); + } @Override public void disabledPeriodic() {} @@ -102,4 +105,5 @@ public void testInit() { public void testPeriodic() { m_robotContainer.testPeriodic(); } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07066de..dce938b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import edu.wpi.first.networktables.DoubleTopic; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -64,6 +65,10 @@ public class RobotContainer { private final Swerve s_Swerve = new Swerve(theLEDs); + // private final Intake s_Intake = new Intake(#, #, #, #); + + // private final Launcher s_Launcher = new Launcher(#, #, #); + // dashboard selector for autos and music private final SendableChooser autoChooser; private final SendableChooser musiChooser; @@ -113,7 +118,26 @@ private void configureButtonBindings() { } - + public void updateInfo() { + SmartDashboard.putBoolean("HasNote", false /*s_Intake.checkForNote()*/); + SmartDashboard.putBoolean("IntakeDeployed", false/*s_Intake.deployed*/); + SmartDashboard.putNumber("LauncherAngle", 45/*s_Launcher.getAngle()*/); + + // {"D","IO","IL","N","R","K","KP","L","CE","CR","C","CL"}; + SmartDashboard.putBoolean("D", theLEDs.getMode() == "D"); + SmartDashboard.putBoolean("IO", theLEDs.getMode() == "IO"); + SmartDashboard.putBoolean("IL", theLEDs.getMode() == "IL"); + SmartDashboard.putBoolean("N", theLEDs.getMode() == "N"); + SmartDashboard.putBoolean("R", theLEDs.getMode() == "R"); + SmartDashboard.putBoolean("K", theLEDs.getMode() == "K"); + SmartDashboard.putBoolean("KP", theLEDs.getMode() == "KP"); + SmartDashboard.putBoolean("L", theLEDs.getMode() == "L"); + SmartDashboard.putBoolean("CE", theLEDs.getMode() == "CE"); + SmartDashboard.putBoolean("CR", theLEDs.getMode() == "CR"); + SmartDashboard.putBoolean("C", theLEDs.getMode() == "C"); + SmartDashboard.putBoolean("CL", theLEDs.getMode() == "CL"); + + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -141,7 +165,7 @@ public void teleopPeriodic() { public void autonomousPeriodic() { theLEDs.rainbow(); } - + public void testInit() { theBand.play(musiChooser.getSelected()); } @@ -152,6 +176,7 @@ public void testPeriodic() { public void disabledInit () { theLEDs.SetFull(255, 60, 0); + theBand.orchestra.stop(); } // public void PlayMusic(int sel) { diff --git a/src/main/java/frc/robot/subsystems/Band.java b/src/main/java/frc/robot/subsystems/Band.java index 7583ea5..6eb2dd6 100644 --- a/src/main/java/frc/robot/subsystems/Band.java +++ b/src/main/java/frc/robot/subsystems/Band.java @@ -13,7 +13,7 @@ public class Band { - private Orchestra _orchestra; + public Orchestra orchestra; private List songs; TalonFX[] _fxes = { @@ -33,10 +33,10 @@ public class Band { public Band() { songs = new ArrayList(); - _orchestra = new Orchestra(); + orchestra = new Orchestra(); for (TalonFX motor : _fxes) { - _orchestra.addInstrument(motor); + orchestra.addInstrument(motor); } File[] musicFiles = new File(Filesystem.getDeployDirectory(), "music").listFiles(); @@ -64,7 +64,7 @@ public SendableChooser Buildchoser(){ * @param songName song file name with .chrp extention. */ public void play(String songName){ - _orchestra.loadMusic("music/"+songName); - _orchestra.play(); + orchestra.loadMusic("music/"+songName); + orchestra.play(); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 959d09b..1f8044e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -27,7 +27,7 @@ public class Intake extends SubsystemBase{ public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, int ultrasonicSensorID) { - SmartDashboard.putNumber("test",3); + // SmartDashboard.putNumber("test",3); m_barMotor = new CANSparkMax(m_barMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); m_intakeMotor1 = new VictorSPX(m_intakeMotor1ID); m_intakeMotor2 = new VictorSPX(m_intakeMotor2ID); From cb088559efccd85e266a08bcc49195adf1bb9f2a Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Thu, 8 Feb 2024 16:04:20 -0700 Subject: [PATCH 05/10] network --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dce938b..11739e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,6 +58,8 @@ public class RobotContainer { private final JoystickButton robotCentric = new JoystickButton(driver, Joystick.ButtonType.kTop.value); private final JoystickButton Test = new JoystickButton(driver, 3); + private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault(); + /* Subsystems */ private final LEDS theLEDs = new LEDS(9,60); From 10ebb4d587826f5fa1b5d2b4ca4a2e9dfe393fd6 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Sat, 24 Feb 2024 16:36:32 -0700 Subject: [PATCH 06/10] Work --- src/main/java/frc/robot/Constants.java | 67 +++++++---- src/main/java/frc/robot/RobotContainer.java | 3 + .../frc/robot/commands/IntakeDefault.java | 55 +++++++++ .../java/frc/robot/commands/IntakeRun.java | 28 ++++- .../java/frc/robot/commands/LaunchASAP.java | 59 +++++++++ .../java/frc/robot/subsystems/Intake.java | 113 +++++++++++++----- .../java/frc/robot/subsystems/Launcher.java | 108 +++++++++++------ 7 files changed, 333 insertions(+), 100 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IntakeDefault.java create mode 100644 src/main/java/frc/robot/commands/LaunchASAP.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6098bef..35b363b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -131,41 +131,56 @@ public static final class Mod3 { //TODOx: This must be tuned to specific robot D } public static final class Intake { + public static final int RollerCAN1 = 9; + public static final int RollerCAN2 = 10; + public static final int ActuatorCAN = 11; + + public static final int forwardLim = 60; + public static final int reversLim = 0; + public static final double noteDetectionDistance = 30; public static final double barP = 5e-5; public static final double barI = 1e-6; public static final double barD = 0; - + public static final double barMaxOut = 1; public static final double barMinOut = -1; - - public static final double barMaxRPM = 5700; - public static final double barMaxVel = 2000; - public static final double barMaxAcc = 1500; - + + public static final double barMaxRPM = 570; + public static final double barMaxVel = 200; + public static final double barMaxAcc = 150; + public static final double barOutPoint = 1000; public static final double barInPoint = -1000; - - public static final double intakeVel = 2; + + public static final double intakeVel = 0.75; + public static final double acuateVel = 1; } - - - public static final class Launcher { - public static final double noteDetectionDistance = 30; - public static final double screwP = 5e-5; - public static final double screwI = 1e-6; - public static final double screwD = 0; - - public static final double screwMaxOut = 1; - public static final double screwMinOut = -1; - - public static final double screwMaxRPM = 3000; - public static final double screwMaxVel = 750; - public static final double screwMaxAcc = 500; - - public static final double screwRotPerDeg = 5; - - public static final double launcerVel = 2; + public static final class Launcher { + public static final int RightCAN = 13; + public static final int LeftCAN = 12; + + public static final double LaunchV = 100; + public static final double LaunchP = 1; + + public static final double LaunchASAPTime = 2; + public static final double LaunchStopTime = 1; + + public static final double P = 0.08; + public static final double I = 0.05; + public static final double D = 0.00; + + public static final double FF = 0.000156; + + public static final double MaxOut = 1; + public static final double MinOut = -1; + + public static final double MaxRPM = 2000; + public static final double MaxVel = 1500; + public static final double MaxAcc = 500; + + public static final double OutPoint = 1000; + public static final double InPoint = -1000; } // holds constants for mode system diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11739e4..7304763 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,6 +67,9 @@ public class RobotContainer { private final Swerve s_Swerve = new Swerve(theLEDs); + public Intake intake = new Intake(Constants.Intake.ActuatorCAN, Constants.Intake.RollerCAN1, Constants.Intake.RollerCAN2); + public Launcher launcher = new Launcher(Constants.Launcher.LeftCAN,Constants.Launcher.RightCAN); + // private final Intake s_Intake = new Intake(#, #, #, #); // private final Launcher s_Launcher = new Launcher(#, #, #); diff --git a/src/main/java/frc/robot/commands/IntakeDefault.java b/src/main/java/frc/robot/commands/IntakeDefault.java new file mode 100644 index 0000000..a00a04b --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeDefault.java @@ -0,0 +1,55 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; +import frc.robot.subsystems.Intake; + +public class IntakeDefault extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + // private Boolean end; + private Boolean first; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeDefault(Intake s_Intake, DigitalInput LIM) { + this.s_Intake = s_Intake; + this.LIM = LIM; + + // this.end = false; + this.first = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + System.out.println("Start IntakeDefault"); + s_Intake.setDeploy(false); + s_Intake.endIntake(); + } + + @Override + public void execute() { + + } + + @Override + public void end(boolean interrupted) { + System.out.println("Start IntakeDefault"); + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + return false; + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeRun.java b/src/main/java/frc/robot/commands/IntakeRun.java index 099af13..013e723 100644 --- a/src/main/java/frc/robot/commands/IntakeRun.java +++ b/src/main/java/frc/robot/commands/IntakeRun.java @@ -1,43 +1,59 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; import frc.robot.subsystems.Intake; public class IntakeRun extends Command { private Intake s_Intake; + private DigitalInput LIM; + private JoystickButton overrideButton; + // private Boolean end; + private Boolean bHold; /** * Run intake untill note is obtained or manualy disabled * * @param s_Intake Intake object */ - public IntakeRun(Intake s_Intake) { + public IntakeRun(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton) { this.s_Intake = s_Intake; + this.LIM = LIM; + this.overrideButton = overrideButton; + // this.end = false; + this.bHold = true; + addRequirements(s_Intake); } @Override public void initialize() { // tell the intake to extend + System.out.println("Start IntakeRun"); s_Intake.setDeploy(true); + s_Intake.startIntake(); + this.bHold = true; } @Override public void execute() { - // spin the intake - s_Intake.runIntake(); + if (bHold) bHold = overrideButton.getAsBoolean(); } @Override public void end(boolean interrupted) { - // tell the intake to retratct - s_Intake.setDeploy(false); + System.out.println("End IntakeRun"); } @Override public boolean isFinished() { // this should trigger when a note is detected, or manualy triggerd - return(s_Intake.checkForNote() || false/*<--- insert manual overide check here*/); + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + if (!LIM.get()) System.out.println("LIM"); + if (overrideButton.getAsBoolean()) System.out.println("OVER"); + return !LIM.get() || (overrideButton.getAsBoolean() && !bHold); } } diff --git a/src/main/java/frc/robot/commands/LaunchASAP.java b/src/main/java/frc/robot/commands/LaunchASAP.java new file mode 100644 index 0000000..0df67a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/LaunchASAP.java @@ -0,0 +1,59 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.hal.simulation.ConstBufferCallback; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Launcher; + +public class LaunchASAP extends Command{ + private Intake s_Intake; + private Launcher s_Launcher; + private Timer timer; + private boolean launch; + + + public LaunchASAP(Intake s_Intake, Launcher s_Launcher) { + this.s_Intake = s_Intake; + this.s_Launcher = s_Launcher; + this.timer = new Timer(); + this.launch = false; + + addRequirements(s_Intake); + addRequirements(s_Launcher); + } + + @Override + public void initialize() { + timer = new Timer(); + timer.start(); + launch = false; + s_Launcher.startLaunch(); + System.out.println("Start LaunchASAP"); + } + + @Override + public void execute() { + + // System.out.println(timer.get()); + if(timer.get() >= Constants.Launcher.LaunchASAPTime && !launch) { + launch = true; + s_Intake.pushIntake(false); + System.out.println("Push"); + } + } + + @Override + public void end(boolean interrupted) { + s_Launcher.endLaunch(); + s_Intake.pushIntake(true); + System.out.println("End LaunchASAP"); + } + + @Override + public boolean isFinished() { + return (timer.get() >= (Constants.Launcher.LaunchASAPTime+Constants.Launcher.LaunchStopTime) && launch); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1f8044e..25dbcd8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -6,12 +6,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; + import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.revrobotics.CANSparkMax; +// import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.IdleMode; public class Intake extends SubsystemBase{ @@ -19,53 +22,88 @@ public class Intake extends SubsystemBase{ public VictorSPX m_intakeMotor2; public CANSparkMax m_barMotor; public RelativeEncoder barEncoder; - public SparkPIDController barPID; - public boolean deployed; - public AnalogInput ultrasonicSensor; + // public SparkPIDController barPID; + private boolean deployed; public double kP, kI, kD, kMaxOut, kMinOut, maxRPM, maxVel, maxAcc; - public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, int ultrasonicSensorID) { + public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID) { // SmartDashboard.putNumber("test",3); m_barMotor = new CANSparkMax(m_barMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); m_intakeMotor1 = new VictorSPX(m_intakeMotor1ID); m_intakeMotor2 = new VictorSPX(m_intakeMotor2ID); - ultrasonicSensor = new AnalogInput(ultrasonicSensorID); + + m_barMotor.restoreFactoryDefaults(); + m_barMotor.setIdleMode(IdleMode.kBrake); barEncoder = m_barMotor.getEncoder(); - barPID = m_barMotor.getPIDController(); + // barPID = m_barMotor.getPIDController(); - kP = Constants.Intake.barP; - kI = Constants.Intake.barI; - kD = Constants.Intake.barD; + // kP = Constants.Intake.barP; + // kI = Constants.Intake.barI; + // kD = Constants.Intake.barD; - kMaxOut = Constants.Intake.barMaxOut; - kMinOut = Constants.Intake.barMinOut; + // kMaxOut = Constants.Intake.barMaxOut; + // kMinOut = Constants.Intake.barMinOut; - maxRPM = Constants.Intake.barMaxRPM; - maxVel = Constants.Intake.barMaxVel; - maxAcc = Constants.Intake.barMaxAcc; + // maxRPM = Constants.Intake.barMaxRPM; + // maxVel = Constants.Intake.barMaxVel; + // maxAcc = Constants.Intake.barMaxAcc; // set PID coefficients - barPID.setP(kP); - barPID.setI(kI); - barPID.setD(kD); - barPID.setOutputRange(kMinOut, kMaxOut); + // barPID.setP(kP); + // barPID.setI(kI); + // barPID.setD(kD); + // barPID.setIZone(0); + // barPID.setFF(0); + // barPID.setOutputRange(kMinOut, kMaxOut); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardLim); + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, Constants.Intake.reversLim); + + SmartDashboard.putBoolean("Forward Soft Limit Enabled", + m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kForward)); + SmartDashboard.putBoolean("Reverse Soft Limit Enabled", + m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kReverse)); + SmartDashboard.putNumber("Forward Soft Limit", + m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kForward)); + SmartDashboard.putNumber("Reverse Soft Limit", + m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kReverse)); - int smartMotionSlot = 0; - barPID.setSmartMotionMaxVelocity(maxVel, smartMotionSlot); - barPID.setSmartMotionMinOutputVelocity(0, smartMotionSlot); - barPID.setSmartMotionMaxAccel(maxAcc, smartMotionSlot); - barPID.setSmartMotionAllowedClosedLoopError(0, smartMotionSlot); + // barPID.setReference(-0.3, CANSparkMax.ControlType.kDutyCycle); + } + + public void startIntake() { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); + // barPID.setReference(0.3, CANSparkMax.ControlType.kDutyCycle); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, false); + m_barMotor.set(Constants.Intake.acuateVel); + System.out.println("Start"); + } + + public void endIntake() { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + // barPID.setReference(-0.3, CANSparkMax.ControlType.kDutyCycle); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, false); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + m_barMotor.set(-Constants.Intake.acuateVel); + System.out.println("End"); } public void runIntake() { - m_intakeMotor1.set(VictorSPXControlMode.Velocity, Constants.Intake.intakeVel); - m_intakeMotor2.set(VictorSPXControlMode.Velocity, Constants.Intake.intakeVel); + // m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, SmartDashboard.getBoolean("Forward Soft Limit Enabled", true)); + // m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, SmartDashboard.getBoolean("Reverse Soft Limit Enabled", true)); + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float)SmartDashboard.getNumber("Forward Soft Limit", 15)); + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, (float)SmartDashboard.getNumber("Reverse Soft Limit", 0)); } public void breakIntake(){ @@ -74,31 +112,40 @@ public void breakIntake(){ } public boolean checkForNote() { - return (ultrasonicSensor.getValue() <= Constants.Intake.noteDetectionDistance); + return false; + } + + public void pushIntake(boolean stop) { + if(!stop) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } } public void toggleDeploy() { if(!deployed) { - // refrence this code for precice neo control through the spark max - // https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Smart%20Motion%20Example/src/main/java/frc/robot/Robot.java - - barPID.setReference(Constants.Intake.barOutPoint, CANSparkMax.ControlType.kSmartMotion); deployed = true; + startIntake(); } else { - barPID.setReference(Constants.Intake.barInPoint, CANSparkMax.ControlType.kSmartMotion); deployed = false; + endIntake(); } } public void setDeploy(boolean mode) { if(mode && !deployed) { - barPID.setReference(Constants.Intake.barOutPoint, CANSparkMax.ControlType.kSmartMotion); deployed = true; } else if(!mode && deployed) { - barPID.setReference(Constants.Intake.barInPoint, CANSparkMax.ControlType.kSmartMotion); deployed = false; } } + + public boolean getMode() { + return deployed; + } diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index 0efb1a6..90193b8 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -1,56 +1,94 @@ package frc.robot.subsystems; +import frc.robot.Constants; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase; + + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.networktables.NetworkTableInstance.NetworkMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; public class Launcher extends SubsystemBase { - public VictorSPX m_flyMotor1; - public VictorSPX m_flyMotor2; - public CANSparkMax m_screwMotor; - public RelativeEncoder screwEncoder; - public SparkPIDController screwPID; - public double kP, kI, kD, kMaxOut, kMinOut, maxRPM, maxVel, maxAcc; + public CANSparkMax m_launcherRight; + public SparkPIDController m_RPID; + public CANSparkMax m_launcherLeft; + public SparkPIDController m_LPID; + + // public VictorSPX m_launcherLeft; + // public VictorSPX m_launcherRight; + + public Intake s_Intake; + + public Launcher(int lMotorId, int rMotorId) { + // --- NEO CODE--- + m_launcherLeft = new CANSparkMax(lMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + m_launcherRight = new CANSparkMax(rMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + + m_launcherLeft.restoreFactoryDefaults(); + m_launcherRight.restoreFactoryDefaults(); - public Launcher(int m_screwMotorId, int m_flyMotor1ID, int m_flyMotor2ID) { + // m_LPID = m_launcherLeft.getPIDController(); + // m_RPID = m_launcherRight.getPIDController(); - m_screwMotor = new CANSparkMax(m_screwMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); - m_flyMotor1 = new VictorSPX(m_flyMotor1ID); - m_flyMotor2 = new VictorSPX(m_flyMotor2ID); + // m_LPID.setP(Constants.Launcher.P); + // m_LPID.setI(Constants.Launcher.I); + // m_LPID.setD(Constants.Launcher.D); + // m_LPID.setIZone(0); + // m_LPID.setFF(Constants.Launcher.FF); + // m_LPID.setOutputRange(Constants.Launcher.MinOut, Constants.Launcher.MaxOut); - screwEncoder = m_screwMotor.getEncoder(); - screwPID = m_screwMotor.getPIDController(); + // m_RPID.setP(Constants.Launcher.P); + // m_RPID.setI(Constants.Launcher.I); + // m_RPID.setD(Constants.Launcher.D); + // m_RPID.setIZone(0); + // m_RPID.setFF(Constants.Launcher.FF); + // m_RPID.setOutputRange(Constants.Launcher.MinOut, Constants.Launcher.MaxOut); - kP = Constants.Launcher.screwP; - kI = Constants.Launcher.screwI; - kD = Constants.Launcher.screwD; - - kMaxOut = Constants.Launcher.screwMaxOut; - kMinOut = Constants.Launcher.screwMinOut; + // m_LPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); + // m_RPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); + + // --- CIM/SPX CODE --- + // m_launcherLeft = new VictorSPX(lMotorId); + // m_launcherRight = new VictorSPX(rMotorId); + + // m_launcherLeft.setNeutralMode(NeutralMode.Coast); + // m_launcherRight.setNeutralMode(NeutralMode.Coast); + + // m_launcherLeft.setInverted(true); + + // m_launcherLeft.set(VictorSPXControlMode.Velocity,0); + // m_launcherRight.set(VictorSPXControlMode.Velocity,0); + } - maxRPM = Constants.Launcher.screwMaxRPM; - maxVel = Constants.Launcher.screwMaxVel; - maxAcc = Constants.Launcher.screwMaxAcc; - // set PID coefficients - screwPID.setP(kP); - screwPID.setI(kI); - screwPID.setD(kD); - screwPID.setOutputRange(kMinOut, kMaxOut); + public void startLaunch() { + // m_LPID.setReference(Constants.Launcher.LaunchP, CANSparkMax.ControlType.kDutyCycle); + // m_RPID.setReference(-Constants.Launcher.LaunchP, CANSparkMax.ControlType.kDutyCycle); + + System.out.println("start launch"); + m_launcherLeft.set(-Constants.Launcher.LaunchP); + m_launcherRight.set(Constants.Launcher.LaunchP); + // m_launcherLeft.set(VictorSPXControlMode.PercentOutput,Constants.Launcher.LaunchP); + // m_launcherRight.set(VictorSPXControlMode.PercentOutput,Constants.Launcher.LaunchP); + } + public void endLaunch() { + // m_LPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); + // m_RPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); + + m_launcherLeft.set(0); + m_launcherRight.set(0); - int smartMotionSlot = 0; - screwPID.setSmartMotionMaxVelocity(maxVel, smartMotionSlot); - screwPID.setSmartMotionMinOutputVelocity(0, smartMotionSlot); - screwPID.setSmartMotionMaxAccel(maxAcc, smartMotionSlot); - screwPID.setSmartMotionAllowedClosedLoopError(0, smartMotionSlot); + // m_launcherLeft.set(VictorSPXControlMode.PercentOutput,0); + // m_launcherRight.set(VictorSPXControlMode.PercentOutput,0); } + } From 0b8886ed04986f8ef1ea3f68c60598a950a82b59 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Wed, 28 Feb 2024 16:14:17 -0700 Subject: [PATCH 07/10] Work --- .pathplanner/settings.json | 7 +- .../deploy/pathplanner/autos/MidMain.auto | 75 +++++++++ src/main/deploy/pathplanner/autos/Pre.auto | 75 +++++++++ .../deploy/pathplanner/autos/TestAuto.auto | 4 +- .../deploy/pathplanner/paths/M-SHOOT.path | 52 +++++++ src/main/deploy/pathplanner/paths/M-TO-1.path | 52 +++++++ .../pathplanner/paths/New New Path.path | 52 +++++++ .../deploy/pathplanner/paths/New Path.path | 2 +- src/main/deploy/pathplanner/paths/Oval.path | 2 +- src/main/deploy/pathplanner/paths/Swaws.path | 2 +- src/main/deploy/pathplanner/paths/T-TO-1.path | 52 +++++++ src/main/deploy/pathplanner/paths/T-TO-2.path | 52 +++++++ src/main/deploy/pathplanner/paths/T-TO-3.path | 52 +++++++ .../deploy/pathplanner/paths/T-TO-M1.path | 79 ++++++++++ .../deploy/pathplanner/paths/T-TO-M2.path | 79 ++++++++++ .../deploy/pathplanner/paths/T-TO-M3.path | 95 +++++++++++ .../deploy/pathplanner/paths/TestPath.path | 18 +-- .../deploy/pathplanner/paths/TheBoxer.path | 2 +- src/main/java/frc/robot/Constants.java | 42 ++--- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 147 +++++++++++++++--- .../java/frc/robot/commands/IntakeAmp.java | 96 ++++++++++++ .../frc/robot/commands/IntakeDefault.java | 2 +- .../java/frc/robot/commands/IntakeFix.java | 84 ++++++++++ .../java/frc/robot/commands/IntakeRun.java | 35 ++++- .../java/frc/robot/commands/LaunchASAP.java | 12 +- .../java/frc/robot/commands/TeleopSwerve.java | 11 +- .../java/frc/robot/subsystems/Intake.java | 63 ++++++-- src/main/java/frc/robot/subsystems/LEDS.java | 92 +++++------ .../java/frc/robot/subsystems/Launcher.java | 48 ------ .../java/frc/robot/subsystems/Swerve.java | 16 +- 31 files changed, 1224 insertions(+), 177 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/MidMain.auto create mode 100644 src/main/deploy/pathplanner/autos/Pre.auto create mode 100644 src/main/deploy/pathplanner/paths/M-SHOOT.path create mode 100644 src/main/deploy/pathplanner/paths/M-TO-1.path create mode 100644 src/main/deploy/pathplanner/paths/New New Path.path create mode 100644 src/main/deploy/pathplanner/paths/T-TO-1.path create mode 100644 src/main/deploy/pathplanner/paths/T-TO-2.path create mode 100644 src/main/deploy/pathplanner/paths/T-TO-3.path create mode 100644 src/main/deploy/pathplanner/paths/T-TO-M1.path create mode 100644 src/main/deploy/pathplanner/paths/T-TO-M2.path create mode 100644 src/main/deploy/pathplanner/paths/T-TO-M3.path create mode 100644 src/main/java/frc/robot/commands/IntakeAmp.java create mode 100644 src/main/java/frc/robot/commands/IntakeFix.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab01..bf63abf 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -2,7 +2,12 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "TEST", + "MID", + "BTM", + "TOP" + ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/deploy/pathplanner/autos/MidMain.auto b/src/main/deploy/pathplanner/autos/MidMain.auto new file mode 100644 index 0000000..5653ef7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidMain.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3707054194735169, + "y": 5.5430292057165325 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeRun" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-1" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Pre.auto b/src/main/deploy/pathplanner/autos/Pre.auto new file mode 100644 index 0000000..d3239fd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Pre.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7728022663252532, + "y": 6.993104356579084 + }, + "rotation": 56.907309424713056 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "IntakeRun" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "T-TO-1" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "New New Path" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto index ed5313f..f7457bf 100644 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ b/src/main/deploy/pathplanner/autos/TestAuto.auto @@ -12,9 +12,9 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Swaws" + "name": "LaunchASAP" } } ] diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path new file mode 100644 index 0000000..ddd030d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3707054194735169, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.4306130406658415, + "y": 5.546357406893884 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6396709716496354, + "y": 5.5430292057165325 + }, + "prevControl": { + "x": 1.5801342990468157, + "y": 5.535587121641179 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path new file mode 100644 index 0000000..f209eca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8501657516135543, + "y": 5.556066607628711 + }, + "prevControl": null, + "nextControl": { + "x": 2.0489663771350335, + "y": 5.601499977928731 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.189296230444312, + "y": 5.556066607628711 + }, + "prevControl": { + "x": 2.0489663771350335, + "y": 5.5430292057165325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 6.159545400201877, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 0000000..2f39893 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.2009903848867527, + "y": 6.385008325572207 + }, + "prevControl": null, + "nextControl": { + "x": 2.2009903848867527, + "y": 5.921648206923535 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2009903848867527, + "y": 5.636582441256051 + }, + "prevControl": { + "x": 2.2009903848867527, + "y": 6.326933851561612 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 82cb879..fde62e2 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Oval.path b/src/main/deploy/pathplanner/paths/Oval.path index 3a0f608..6a9b60c 100644 --- a/src/main/deploy/pathplanner/paths/Oval.path +++ b/src/main/deploy/pathplanner/paths/Oval.path @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Swaws.path b/src/main/deploy/pathplanner/paths/Swaws.path index 99c1403..5127a38 100644 --- a/src/main/deploy/pathplanner/paths/Swaws.path +++ b/src/main/deploy/pathplanner/paths/Swaws.path @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-1.path b/src/main/deploy/pathplanner/paths/T-TO-1.path new file mode 100644 index 0000000..0803358 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4408703461281565, + "y": 6.805997885500045 + }, + "prevControl": null, + "nextControl": { + "x": 1.932024832710634, + "y": 6.9346335843668845 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.282849465983832, + "y": 6.946327738809324 + }, + "prevControl": { + "x": 1.7566125160740347, + "y": 6.934633584366883 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 5.662290558173469, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 16.50838708204789, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-2.path b/src/main/deploy/pathplanner/paths/T-TO-2.path new file mode 100644 index 0000000..2ff8c3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4759528094554764, + "y": 6.665668032190766 + }, + "prevControl": null, + "nextControl": { + "x": 2.2126845393291923, + "y": 6.2797609355902475 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5292958694025494, + "y": 6.1292382044223555 + }, + "prevControl": { + "x": 1.7566125160740347, + "y": 6.373314171129768 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.00797980144137, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 14.281095735970798, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-3.path b/src/main/deploy/pathplanner/paths/T-TO-3.path new file mode 100644 index 0000000..9128f15 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.0432690950851984, + "y": 6.852774503269805 + }, + "prevControl": null, + "nextControl": { + "x": 3.0078870414151084, + "y": 6.385008325572207 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.17799714878709, + "y": 4.113892912470511 + }, + "prevControl": { + "x": 1.1779971487870902, + "y": 4.113892912470511 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 2.045408488887245, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-M1.path b/src/main/deploy/pathplanner/paths/T-TO-M1.path new file mode 100644 index 0000000..db07c8c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-M1.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4525645005705963, + "y": 6.7241388044029655 + }, + "prevControl": null, + "nextControl": { + "x": 2.5619288814615206, + "y": 6.2341695361761404 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.917573402714373, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 2.5200511662284586, + "y": 6.304887564730085 + }, + "nextControl": { + "x": 3.2793314510542197, + "y": 6.269572202645167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7340637564158, + "y": 7.136910850398278 + }, + "prevControl": { + "x": 7.2625838945371575, + "y": 6.600718066301 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 27.050597007086274, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 25.559965171823848, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-M2.path b/src/main/deploy/pathplanner/paths/T-TO-M2.path new file mode 100644 index 0000000..b5915d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-M2.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3005404928188775, + "y": 6.712444649960525 + }, + "prevControl": null, + "nextControl": { + "x": 2.4099048737098014, + "y": 6.2224753817337 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.917573402714373, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 2.5200511662284586, + "y": 6.304887564730085 + }, + "nextControl": { + "x": 3.2793314510542197, + "y": 6.269572202645167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.623127318326708, + "y": 5.99981235998508 + }, + "prevControl": { + "x": 6.726391110439877, + "y": 6.156972313944628 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -14.62087398863165, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 26.872647952447924, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-M3.path b/src/main/deploy/pathplanner/paths/T-TO-M3.path new file mode 100644 index 0000000..78e0219 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-M3.path @@ -0,0 +1,95 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2537638750491173, + "y": 6.794303731057604 + }, + "prevControl": null, + "nextControl": { + "x": 2.363128255940041, + "y": 6.304334462830779 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.917573402714373, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 2.5200511662284586, + "y": 6.304887564730085 + }, + "nextControl": { + "x": 3.2793314510542197, + "y": 6.269572202645167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.162464216820325, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 5.260329639621656, + "y": 6.706974187170742 + }, + "nextControl": { + "x": 7.064598794018995, + "y": 5.865822129593065 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.641616724674891, + "y": 4.483681039434152 + }, + "prevControl": { + "x": 6.74488051678806, + "y": 4.6408409933937 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -25.08359400619089, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 26.003345844511543, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/TestPath.path index d38e095..13991ca 100644 --- a/src/main/deploy/pathplanner/paths/TestPath.path +++ b/src/main/deploy/pathplanner/paths/TestPath.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6513651260920754, - "y": 5.566417514601411 + "x": 1.7434960996048117, + "y": 2.14477113638912 }, "prevControl": null, "nextControl": { - "x": 2.7856981070087494, - "y": 1.3448277608805934 + "x": 2.8778290805214857, + "y": -2.0768186173316976 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.101140686611857, - "y": 5.85877137566241 + "x": 4.9976316168848545, + "y": 2.634740404615945 }, "prevControl": { - "x": 5.101140686611857, - "y": 6.95877137566241 + "x": 4.9976316168848545, + "y": 3.734740404615947 }, "nextControl": null, "isLocked": false, @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TheBoxer.path b/src/main/deploy/pathplanner/paths/TheBoxer.path index f92965a..1b7c25c 100644 --- a/src/main/deploy/pathplanner/paths/TheBoxer.path +++ b/src/main/deploy/pathplanner/paths/TheBoxer.path @@ -91,7 +91,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 35b363b..4c6867c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,8 +23,8 @@ public static final class Swerve { COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(24.5); //TODOx: This must be tuned to specific robot - public static final double wheelBase = Units.inchesToMeters(24.5); //TODOx: This must be tuned to specific robot + public static final double trackWidth = Units.inchesToMeters(16.75); //TODO: This must be tuned to specific robot + public static final double wheelBase = Units.inchesToMeters(16.75); //TODO: This must be tuned to specific robot public static final double wheelCircumference = chosenModule.wheelCircumference; /* Swerve Kinematics @@ -136,6 +136,7 @@ public static final class Intake { public static final int ActuatorCAN = 11; public static final int forwardLim = 60; + public static final int forwardAmpLim = 30; public static final int reversLim = 0; public static final double noteDetectionDistance = 30; @@ -153,8 +154,13 @@ public static final class Intake { public static final double barOutPoint = 1000; public static final double barInPoint = -1000; - public static final double intakeVel = 0.75; - public static final double acuateVel = 1; + public static final double intakeVel = 0.8; + public static final double intakeNudge = 0.2; + + public static final double acuateVel = 0.8; + public static final double acuateManual = 0.2; + + public static final double OverRun = 0.75; } public static final class Launcher { public static final int RightCAN = 13; @@ -200,27 +206,21 @@ public static final class Lock { } public static final class Colors { - public static final Color D = Color.kPurple; // Default Color - public static final Color I = Color.kRed; // Intake Color - public static final Color IO = Color.kDarkRed; // Intake Out Blink Color - public static final Color R = Color.kOrange; // Ready/Note Color - public static final Color RB = Color.kDarkOrange; // Ready Blink Color - public static final Color KP = Color.kYellow;// Lock Pending color - public static final Color K = Color.kGreen; // Lock color - public static final Color L = Color.kSkyBlue;// Launching color - - public static final Color C = Color.kYellow; // Climber Blink Color - public static final Color CE = Color.kRed; // Climber Extenidng other Blink Color - public static final Color CR = Color.kOrange; // Climber Ready other Blink Color - public static final Color CC = Color.kSkyBlue; // Climber Climbing other Blink Color - public static final Color CL = Color.kGreen; // Climber Locked other Blink Color - + public static final Color D = Color.kBlue; // Default Color + public static final Color IO = Color.kRed; // Intake Out Color + public static final Color IF = Color.kPurple; // Intake Fix Color + public static final Color IR = Color.kOrange; // Intake Retracting Color + public static final Color IA = Color.kPink; // Intake Amp Color + public static final Color N = Color.kYellow; // Note Color + public static final Color LS = Color.kGreen; // Launcher Spinup Color + public static final Color L = Color.kAqua; // Color Color + public static final Color DIS = Color.kPurple; //Disable Color } } public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot - public static final double kMaxSpeedMetersPerSecond = 2; - public static final double kMaxAccelerationMetersPerSecondSquared = 2; + public static final double kMaxSpeedMetersPerSecond = 1; + public static final double kMaxAccelerationMetersPerSecondSquared = 0.5; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a2c07ab..cef14e5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -85,6 +85,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7304763..0dc6ba0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,14 +1,28 @@ package frc.robot; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.networktables.DoubleTopic; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -16,6 +30,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.ArrayList; +import java.util.List; + import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -35,6 +52,7 @@ public class RobotContainer { private final Joystick driver = new Joystick(0); private final XboxController secondary = new XboxController(1); + private final PowerDistribution PDP = new PowerDistribution(); // /* XBOX CONTROLOR */ /* Drive Controls */ @@ -52,23 +70,29 @@ public class RobotContainer { private final int translationAxis = Joystick.AxisType.kY.value; private final int strafeAxis = Joystick.AxisType.kX.value; private final int rotationAxis = Joystick.AxisType.kTwist.value; + private final int throttleAxis = 2; /* Driver Buttons */ - private final JoystickButton zeroGyro = new JoystickButton(driver, Joystick.ButtonType.kTrigger.value); - private final JoystickButton robotCentric = new JoystickButton(driver, Joystick.ButtonType.kTop.value); + private final JoystickButton zeroGyro = new JoystickButton(driver, 2); + private final JoystickButton robotCentric = new JoystickButton(driver, 7); private final JoystickButton Test = new JoystickButton(driver, 3); + // private final NamedCommands NamedCommands = new NamedCommands(); + private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault(); /* Subsystems */ - private final LEDS theLEDs = new LEDS(9,60); + private final LEDS theLEDs = new LEDS(0,60); private final Band theBand = new Band(); private final Swerve s_Swerve = new Swerve(theLEDs); - public Intake intake = new Intake(Constants.Intake.ActuatorCAN, Constants.Intake.RollerCAN1, Constants.Intake.RollerCAN2); + public Intake intake = new Intake(Constants.Intake.ActuatorCAN, Constants.Intake.RollerCAN1, Constants.Intake.RollerCAN2,secondary); + public Launcher launcher = new Launcher(Constants.Launcher.LeftCAN,Constants.Launcher.RightCAN); + + public DigitalInput LIM = new DigitalInput(0); // private final Intake s_Intake = new Intake(#, #, #, #); @@ -78,6 +102,9 @@ public class RobotContainer { private final SendableChooser autoChooser; private final SendableChooser musiChooser; + + private Thread m_visionThread; + /** The container for the robot. Contains subsystems, OI devices, and commands. * this is the main class most things stem from. only thing above this is the robot.java that * connects this to the driverstation @@ -92,23 +119,77 @@ public RobotContainer() { () -> -driver.getRawAxis(translationAxis), () -> -driver.getRawAxis(strafeAxis), () -> -driver.getRawAxis(rotationAxis), + () -> -driver.getRawAxis(throttleAxis), () -> robotCentric.getAsBoolean(), theLEDs ) ); + + intake.setDefaultCommand(new IntakeDefault(intake,LIM)); + + NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); + NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); + // Configure the button bindings configureButtonBindings(); //Build the auto chooser - autoChooser = AutoBuilder.buildAutoChooser(); - // autoChooser = AutoBuilder.buildAutoChooser("My Default Auto"); + // autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser = AutoBuilder.buildAutoChooser("MidMain"); SmartDashboard.putData("Auto Chooser", autoChooser); musiChooser = theBand.Buildchoser(); SmartDashboard.putData("Music Choser",musiChooser); - theLEDs.SetFull(255, 60, 0); + secondary.getPOV(); + + // theLEDs.SetFull(255, 60, 0); + theLEDs.setMode("D"); + + + m_visionThread = + new Thread( + () -> { + // Get the UsbCamera from CameraServer + UsbCamera camera = CameraServer.startAutomaticCapture(); + // Set the resolution + camera.setResolution(640/2, 480/2); + + // Get a CvSink. This will capture Mats from the camera + CvSink cvSink = CameraServer.getVideo(); + // Setup a CvSource. This will send images back to the Dashboard + CvSource outputStream = CameraServer.putVideo("Note", 640/2, 480/2); + + // Mats are very memory expensive. Lets reuse this Mat. + Mat mat = new Mat(); + + // This cannot be 'true'. The program will never exit if it is. This + // lets the robot stop this thread when restarting robot code or + // deploying. + while (!Thread.interrupted()) { + // Tell the CvSink to grab a frame from the camera and put it + // in the source mat. If there is an error notify the output. + if (cvSink.grabFrame(mat) == 0) { + // Send the output the error. + outputStream.notifyError(cvSink.getError()); + // skip the rest of the current iteration + continue; + } + // Imgproc.rectangle( + // mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5); + // List list = new ArrayList(); + // list.add( new MatOfPoint ( + // new Point(208, 71), new Point(421, 161), + // new Point(226, 232), new Point(332, 52), + // new Point(363, 250))); + // Imgproc.polylines(mat, list, false, new Scalar(255,255,255), 2); + // // Give the output stream a new image to display + outputStream.putFrame(mat); + } + }); + m_visionThread.setDaemon(true); + m_visionThread.start(); } /** @@ -120,27 +201,46 @@ public RobotContainer() { private void configureButtonBindings() { /** Driver Buttons */ zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); + new JoystickButton(secondary, 4).onTrue(new LaunchASAP(intake,launcher,theLEDs)); + new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); + new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),theLEDs)); + new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); } public void updateInfo() { - SmartDashboard.putBoolean("HasNote", false /*s_Intake.checkForNote()*/); - SmartDashboard.putBoolean("IntakeDeployed", false/*s_Intake.deployed*/); - SmartDashboard.putNumber("LauncherAngle", 45/*s_Launcher.getAngle()*/); + SmartDashboard.putBoolean("IntakeDeployed", intake.getMode()); + SmartDashboard.putBoolean("Note", LIM.get()); - // {"D","IO","IL","N","R","K","KP","L","CE","CR","C","CL"}; + // {"D","IO","IR","L","LS"}; SmartDashboard.putBoolean("D", theLEDs.getMode() == "D"); SmartDashboard.putBoolean("IO", theLEDs.getMode() == "IO"); - SmartDashboard.putBoolean("IL", theLEDs.getMode() == "IL"); + SmartDashboard.putBoolean("IR", theLEDs.getMode() == "IR"); + SmartDashboard.putBoolean("IF", theLEDs.getMode() == "IF"); + SmartDashboard.putBoolean("IA", theLEDs.getMode() == "IA"); SmartDashboard.putBoolean("N", theLEDs.getMode() == "N"); - SmartDashboard.putBoolean("R", theLEDs.getMode() == "R"); - SmartDashboard.putBoolean("K", theLEDs.getMode() == "K"); - SmartDashboard.putBoolean("KP", theLEDs.getMode() == "KP"); + SmartDashboard.putBoolean("LS", theLEDs.getMode() == "LS"); SmartDashboard.putBoolean("L", theLEDs.getMode() == "L"); - SmartDashboard.putBoolean("CE", theLEDs.getMode() == "CE"); - SmartDashboard.putBoolean("CR", theLEDs.getMode() == "CR"); - SmartDashboard.putBoolean("C", theLEDs.getMode() == "C"); - SmartDashboard.putBoolean("CL", theLEDs.getMode() == "CL"); + + SmartDashboard.putData("intake", intake); + SmartDashboard.putData("Launcher", launcher); + SmartDashboard.putData("Gyro",s_Swerve.gyro); + SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); + SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); + SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); + + SmartDashboard.putData(PDP); + System.out.println("Radio"); + System.out.println(PDP.getCurrent(15)); + System.out.println("RIO"); + System.out.println(PDP.getCurrent(20)); + System.out.println("Total"); + System.out.println(PDP.getTotalCurrent()); + + intake.updateData(); + + double throtleVal = (driver.getRawAxis(throttleAxis)+1)/2; + SmartDashboard.putNumber("Throttle", throtleVal); } @@ -163,12 +263,19 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } + public void teleopInit() { + theLEDs.SetFull(0, 0, 255); + theLEDs.setMode("D"); + theLEDs.updateMode(); + } + public void teleopPeriodic() { // theLEDs.rainbow(); + theLEDs.updateMode(); } public void autonomousPeriodic() { - theLEDs.rainbow(); + // theLEDs.rainbow(); } public void testInit() { diff --git a/src/main/java/frc/robot/commands/IntakeAmp.java b/src/main/java/frc/robot/commands/IntakeAmp.java new file mode 100644 index 0000000..7595510 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeAmp.java @@ -0,0 +1,96 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import com.revrobotics.CANSparkMax; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; + +public class IntakeAmp extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + private JoystickButton overrideButton; + private JoystickButton PushButton; + // private Boolean end; + private Boolean bHold; + private Boolean end; + private Timer timer; + private LEDS theLEDs; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeAmp(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, JoystickButton PushButton, LEDS theLEDs) { + this.s_Intake = s_Intake; + this.LIM = LIM; + this.overrideButton = overrideButton; + this.PushButton = PushButton; + this.theLEDs = theLEDs; + // this.end = false; + this.bHold = true; + this.end = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + s_Intake.m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardAmpLim); + System.out.println("Start IntakeAmp"); + s_Intake.setDeploy(true); + this.bHold = true; + this.end = false; + timer = new Timer(); + theLEDs.setMode("IA"); + // timer.start(); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + if (PushButton.getAsBoolean() && !end) { + timer.start(); + end = true; + System.out.println("End1"); + s_Intake.pushIntake(false); + // theLEDs.setMode("IR"); + // System.out.println("set IR"); + + } + System.out.println(timer.get()); + System.out.println(timer.get() >= Constants.Intake.OverRun); + } + + @Override + public void end(boolean interrupted) { + System.out.println("End IntakeAmp"); + s_Intake.m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardLim); + s_Intake.pushIntake(true); + if (end) { + theLEDs.setMode("N"); + System.out.println("set N"); + } else { + theLEDs.setMode("D"); + System.out.println("set D"); + } + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + if (!LIM.get()) System.out.println("LIM"); + if (overrideButton.getAsBoolean()) System.out.println("OVER"); + return timer.get() >= Constants.Intake.OverRun || (overrideButton.getAsBoolean() && !bHold); + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeDefault.java b/src/main/java/frc/robot/commands/IntakeDefault.java index a00a04b..f14ad42 100644 --- a/src/main/java/frc/robot/commands/IntakeDefault.java +++ b/src/main/java/frc/robot/commands/IntakeDefault.java @@ -37,7 +37,7 @@ public void initialize() { @Override public void execute() { - + s_Intake.nudge(); } @Override diff --git a/src/main/java/frc/robot/commands/IntakeFix.java b/src/main/java/frc/robot/commands/IntakeFix.java new file mode 100644 index 0000000..5330933 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeFix.java @@ -0,0 +1,84 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.LEDS; + +public class IntakeFix extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + private JoystickButton overrideButton; + // private Boolean end; + private Boolean bHold; + private Boolean end; + private Timer timer; + private LEDS theLEDs; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeFix(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, LEDS theLEDs) { + this.s_Intake = s_Intake; + this.LIM = LIM; + this.overrideButton = overrideButton; + this.theLEDs = theLEDs; + // this.end = false; + this.bHold = true; + this.end = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + System.out.println("Start IntakeFix"); + s_Intake.setDeploy(true); + this.bHold = true; + this.end = false; + timer = new Timer(); + theLEDs.setMode("IF"); + System.out.println("set IF"); + // timer.start(); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + s_Intake.nudge(); + s_Intake.manualAcuation(); + System.out.println(timer.get()); + System.out.println(timer.get() >= Constants.Intake.OverRun); + } + + @Override + public void end(boolean interrupted) { + System.out.println("End IntakeFix"); + if (!LIM.get()) { + theLEDs.setMode("N"); + System.out.println("set N"); + } else { + theLEDs.setMode("D"); + System.out.println("set D"); + } + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + if (!LIM.get()) System.out.println("LIM"); + if (overrideButton.getAsBoolean()) System.out.println("OVER"); + return timer.get() >= Constants.Intake.OverRun || (overrideButton.getAsBoolean() && !bHold); + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeRun.java b/src/main/java/frc/robot/commands/IntakeRun.java index 013e723..de10858 100644 --- a/src/main/java/frc/robot/commands/IntakeRun.java +++ b/src/main/java/frc/robot/commands/IntakeRun.java @@ -1,10 +1,14 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import java.util.function.BooleanSupplier; + +import frc.robot.Constants; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; public class IntakeRun extends Command { @@ -13,18 +17,23 @@ public class IntakeRun extends Command { private JoystickButton overrideButton; // private Boolean end; private Boolean bHold; + private Boolean end; + private Timer timer; + private LEDS theLEDs; /** * Run intake untill note is obtained or manualy disabled * * @param s_Intake Intake object */ - public IntakeRun(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton) { + public IntakeRun(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, LEDS theLEDs) { this.s_Intake = s_Intake; this.LIM = LIM; this.overrideButton = overrideButton; + this.theLEDs = theLEDs; // this.end = false; this.bHold = true; + this.end = false; addRequirements(s_Intake); } @@ -35,16 +44,38 @@ public void initialize() { s_Intake.setDeploy(true); s_Intake.startIntake(); this.bHold = true; + this.end = false; + timer = new Timer(); + theLEDs.setMode("IO"); + // timer.start(); } @Override public void execute() { if (bHold) bHold = overrideButton.getAsBoolean(); + if (!LIM.get() && !end) { + timer.start(); + s_Intake.endIntake(); + end = true; + System.out.println("End1"); + theLEDs.setMode("IR"); + System.out.println("set IR"); + + } + System.out.println(timer.get()); + System.out.println(timer.get() >= Constants.Intake.OverRun); } @Override public void end(boolean interrupted) { System.out.println("End IntakeRun"); + if (end) { + theLEDs.setMode("N"); + System.out.println("set N"); + } else { + theLEDs.setMode("D"); + System.out.println("set D"); + } } @Override @@ -53,7 +84,7 @@ public boolean isFinished() { // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); if (!LIM.get()) System.out.println("LIM"); if (overrideButton.getAsBoolean()) System.out.println("OVER"); - return !LIM.get() || (overrideButton.getAsBoolean() && !bHold); + return timer.get() >= Constants.Intake.OverRun || (overrideButton.getAsBoolean() && !bHold); } } diff --git a/src/main/java/frc/robot/commands/LaunchASAP.java b/src/main/java/frc/robot/commands/LaunchASAP.java index 0df67a7..7f32038 100644 --- a/src/main/java/frc/robot/commands/LaunchASAP.java +++ b/src/main/java/frc/robot/commands/LaunchASAP.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; import frc.robot.subsystems.Launcher; public class LaunchASAP extends Command{ @@ -13,13 +14,15 @@ public class LaunchASAP extends Command{ private Launcher s_Launcher; private Timer timer; private boolean launch; + private LEDS theLEDs; - public LaunchASAP(Intake s_Intake, Launcher s_Launcher) { + public LaunchASAP(Intake s_Intake, Launcher s_Launcher, LEDS theLEDs) { this.s_Intake = s_Intake; this.s_Launcher = s_Launcher; this.timer = new Timer(); this.launch = false; + this.theLEDs = theLEDs; addRequirements(s_Intake); addRequirements(s_Launcher); @@ -32,6 +35,9 @@ public void initialize() { launch = false; s_Launcher.startLaunch(); System.out.println("Start LaunchASAP"); + theLEDs.setMode("LS"); + System.out.println("set LS"); + } @Override @@ -40,6 +46,8 @@ public void execute() { // System.out.println(timer.get()); if(timer.get() >= Constants.Launcher.LaunchASAPTime && !launch) { launch = true; + theLEDs.setMode("L"); + System.out.println("set L"); s_Intake.pushIntake(false); System.out.println("Push"); } @@ -50,6 +58,8 @@ public void end(boolean interrupted) { s_Launcher.endLaunch(); s_Intake.pushIntake(true); System.out.println("End LaunchASAP"); + theLEDs.setMode("D"); + System.out.println("set D"); } @Override diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 7bd0fc3..0eb7e54 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -17,6 +18,7 @@ public class TeleopSwerve extends Command { private DoubleSupplier translationSup; private DoubleSupplier strafeSup; private DoubleSupplier rotationSup; + private DoubleSupplier throtleSupplier; private BooleanSupplier robotCentricSup; private LEDS led; @@ -29,7 +31,7 @@ public class TeleopSwerve extends Command { * @param rotationSup Rotation suplier, typicaly a controlor axis * @param robotCentricSup wether to drive relative to the robot. */ - public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, LEDS led) { + public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, DoubleSupplier throtleSupplier, BooleanSupplier robotCentricSup, LEDS led) { this.s_Swerve = s_Swerve; addRequirements(s_Swerve); @@ -37,6 +39,7 @@ public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli this.strafeSup = strafeSup; this.rotationSup = rotationSup; this.robotCentricSup = robotCentricSup; + this.throtleSupplier = throtleSupplier; this.led = led; } @@ -46,11 +49,13 @@ public void execute() { double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + double throtleVal = 1-((throtleSupplier.getAsDouble()+1)/2); + // SmartDashboard.putNumber("Throttle", throtleVal); /* Drive */ s_Swerve.drive( - new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed), - rotationVal * Constants.Swerve.maxAngularVelocity, + new Translation2d(translationVal*throtleVal, strafeVal*throtleVal).times(Constants.Swerve.maxSpeed), + (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, !robotCentricSup.getAsBoolean(), true ); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 25dbcd8..51b20c9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,10 +2,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import java.util.function.IntSupplier; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; @@ -25,10 +27,12 @@ public class Intake extends SubsystemBase{ // public SparkPIDController barPID; private boolean deployed; + private XboxController secondaryController; + public double kP, kI, kD, kMaxOut, kMinOut, maxRPM, maxVel, maxAcc; - public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID) { + public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, XboxController secondaryControllerIn) { // SmartDashboard.putNumber("test",3); m_barMotor = new CANSparkMax(m_barMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); m_intakeMotor1 = new VictorSPX(m_intakeMotor1ID); @@ -37,6 +41,8 @@ public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID) { m_barMotor.restoreFactoryDefaults(); m_barMotor.setIdleMode(IdleMode.kBrake); + secondaryController = secondaryControllerIn; + barEncoder = m_barMotor.getEncoder(); // barPID = m_barMotor.getPIDController(); @@ -65,15 +71,6 @@ public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID) { m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardLim); m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, Constants.Intake.reversLim); - - SmartDashboard.putBoolean("Forward Soft Limit Enabled", - m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kForward)); - SmartDashboard.putBoolean("Reverse Soft Limit Enabled", - m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kReverse)); - SmartDashboard.putNumber("Forward Soft Limit", - m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kForward)); - SmartDashboard.putNumber("Reverse Soft Limit", - m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kReverse)); // barPID.setReference(-0.3, CANSparkMax.ControlType.kDutyCycle); } @@ -118,7 +115,7 @@ public boolean checkForNote() { public void pushIntake(boolean stop) { if(!stop) { m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel); - m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel*0.5); } else { m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); @@ -135,6 +132,39 @@ public void toggleDeploy() { } } + public void nudge() { + if (secondaryController.getPOV() != -1) { + System.out.println(secondaryController.getPOV()); + } + if (secondaryController.getPOV() == 0) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeNudge); + } else if (secondaryController.getPOV() == 180){ + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + } + + public void manualAcuation() { + if (secondaryController.getPOV() != -1) { + System.out.println(secondaryController.getPOV()); + } + if (secondaryController.getPOV() == 90) { + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, false); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + m_barMotor.set(-Constants.Intake.acuateManual); + } else if (secondaryController.getPOV() == 270){ + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, false); + m_barMotor.set(Constants.Intake.acuateManual); + } else { + m_barMotor.set(0); + } + } + public void setDeploy(boolean mode) { if(mode && !deployed) { deployed = true; @@ -146,6 +176,17 @@ public void setDeploy(boolean mode) { public boolean getMode() { return deployed; } + + public void updateData() { + SmartDashboard.putBoolean("Forward Soft Limit Enabled", + m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kForward)); + SmartDashboard.putBoolean("Reverse Soft Limit Enabled", + m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kReverse)); + SmartDashboard.putNumber("Forward Soft Limit", + m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kForward)); + SmartDashboard.putNumber("Reverse Soft Limit", + m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kReverse)); + } diff --git a/src/main/java/frc/robot/subsystems/LEDS.java b/src/main/java/frc/robot/subsystems/LEDS.java index 2a723cc..a68592c 100644 --- a/src/main/java/frc/robot/subsystems/LEDS.java +++ b/src/main/java/frc/robot/subsystems/LEDS.java @@ -7,10 +7,11 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -public class LEDS { +public class LEDS{ private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; @@ -98,72 +99,65 @@ public void SetFullHSV(int H, int S, int V) { public void updateMode() { if (mode != lastMode) { + lastMode = mode; + /* + * LED modes + * + * D BLUE, Default Drive Mode + * IO RED, Intake out, running intake + * IR ORANGE Intake Stowing + * IF PURPLE Intake Fixing + * IA PINK Intake Amp + * N YELLOW Note Ready + * LS GREEN Launcher Spin Up + * L AQUA Launching + * + */ switch (mode) { case "D": SetFull(Constants.Mode.Colors.D); + System.out.println("D"); + break; case "IO": - SetFull(Constants.Mode.Colors.I); - case "IL": - SetFull(Constants.Mode.Colors.I); + SetFull(Constants.Mode.Colors.IO); + System.out.println("IO"); + break; + case "IR": + SetFull(Constants.Mode.Colors.IR); + System.out.println("IR"); + break; + case "IF": + SetFull(Constants.Mode.Colors.IF); + System.out.println("IF"); + break; + case "IA": + SetFull(Constants.Mode.Colors.IA); + System.out.println("IA"); + break; case "N": - SetFull(Constants.Mode.Colors.R); - case "R": - SetFull(Constants.Mode.Colors.R); - case "K": - SetFull(Constants.Mode.Colors.K); - case "KP": - SetFull(Constants.Mode.Colors.KP); + SetFull(Constants.Mode.Colors.N); + System.out.println("N"); + break; + case "LS": + SetFull(Constants.Mode.Colors.LS); + System.out.println("LS"); + break; case "L": SetFull(Constants.Mode.Colors.L); - case "CE": - SetFull(Constants.Mode.Colors.C); - case "CR": - SetFull(Constants.Mode.Colors.C); - case "C": - SetFull(Constants.Mode.Colors.C); - case "CL": - SetFull(Constants.Mode.Colors.C); - default: + System.out.println("L"); break; - } - } - - if(timer.get() >= nextBlink) { - blink = !blink; - switch (mode) { - case "IO": - if(blink) SetFull(Constants.Mode.Colors.I); - else SetFull(Constants.Mode.Colors.IO); - case "R": - if(blink) SetFull(Constants.Mode.Colors.I); - else SetFull(Constants.Mode.Colors.IO); - case "CE": - if(blink) SetFull(Constants.Mode.Colors.C); - else SetFull(Constants.Mode.Colors.CE); - case "CR": - if(blink) SetFull(Constants.Mode.Colors.C); - else SetFull(Constants.Mode.Colors.CR); - case "C": - if(blink) SetFull(Constants.Mode.Colors.C); - else SetFull(Constants.Mode.Colors.CC); - case "CL": - if(blink) SetFull(Constants.Mode.Colors.C); - else SetFull(Constants.Mode.Colors.CL); default: break; } - } - - - } public void setMode(String modeID) { - String[] vModes = {"D","IO","IL","N","R","K","KP","L","CE","CR","C","CL"}; + String[] vModes = {"D","IO","IR","IF","IA","N","LS","L"}; if (Arrays.asList(vModes).contains(modeID)) { mode = modeID; } + this.updateMode(); } public String getMode() { diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index 90193b8..f997c13 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -20,74 +20,26 @@ public class Launcher extends SubsystemBase { public CANSparkMax m_launcherLeft; public SparkPIDController m_LPID; - // public VictorSPX m_launcherLeft; - // public VictorSPX m_launcherRight; - public Intake s_Intake; public Launcher(int lMotorId, int rMotorId) { - // --- NEO CODE--- m_launcherLeft = new CANSparkMax(lMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); m_launcherRight = new CANSparkMax(rMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); m_launcherLeft.restoreFactoryDefaults(); m_launcherRight.restoreFactoryDefaults(); - - // m_LPID = m_launcherLeft.getPIDController(); - // m_RPID = m_launcherRight.getPIDController(); - - // m_LPID.setP(Constants.Launcher.P); - // m_LPID.setI(Constants.Launcher.I); - // m_LPID.setD(Constants.Launcher.D); - // m_LPID.setIZone(0); - // m_LPID.setFF(Constants.Launcher.FF); - // m_LPID.setOutputRange(Constants.Launcher.MinOut, Constants.Launcher.MaxOut); - - // m_RPID.setP(Constants.Launcher.P); - // m_RPID.setI(Constants.Launcher.I); - // m_RPID.setD(Constants.Launcher.D); - // m_RPID.setIZone(0); - // m_RPID.setFF(Constants.Launcher.FF); - // m_RPID.setOutputRange(Constants.Launcher.MinOut, Constants.Launcher.MaxOut); - - // m_LPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); - // m_RPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); - - // --- CIM/SPX CODE --- - // m_launcherLeft = new VictorSPX(lMotorId); - // m_launcherRight = new VictorSPX(rMotorId); - - // m_launcherLeft.setNeutralMode(NeutralMode.Coast); - // m_launcherRight.setNeutralMode(NeutralMode.Coast); - - // m_launcherLeft.setInverted(true); - - // m_launcherLeft.set(VictorSPXControlMode.Velocity,0); - // m_launcherRight.set(VictorSPXControlMode.Velocity,0); } public void startLaunch() { - // m_LPID.setReference(Constants.Launcher.LaunchP, CANSparkMax.ControlType.kDutyCycle); - // m_RPID.setReference(-Constants.Launcher.LaunchP, CANSparkMax.ControlType.kDutyCycle); - System.out.println("start launch"); m_launcherLeft.set(-Constants.Launcher.LaunchP); m_launcherRight.set(Constants.Launcher.LaunchP); - - // m_launcherLeft.set(VictorSPXControlMode.PercentOutput,Constants.Launcher.LaunchP); - // m_launcherRight.set(VictorSPXControlMode.PercentOutput,Constants.Launcher.LaunchP); } public void endLaunch() { - // m_LPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); - // m_RPID.setReference(0, CANSparkMax.ControlType.kDutyCycle); - m_launcherLeft.set(0); m_launcherRight.set(0); - - // m_launcherLeft.set(VictorSPXControlMode.PercentOutput,0); - // m_launcherRight.set(VictorSPXControlMode.PercentOutput,0); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 1026e07..0908e2b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -56,10 +57,10 @@ public Swerve(LEDS iled) { this::getChasisHeading, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + new PIDConstants(5.0, 1.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 1.0, 0.0), // Rotation PID constants 3.5, // Max module speed, in m/s - 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + 0.3, // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options here ), () -> { @@ -165,7 +166,11 @@ public Rotation2d getHeading(){ } public void setHeading(Rotation2d heading){ - swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), new Pose2d(getPose().getTranslation(), heading)); + swerveOdometry.resetPosition( + getGyroYaw(), + getModulePositions(), + new Pose2d(getPose().getTranslation(), heading) + ); } public void zeroHeading(){ @@ -184,7 +189,8 @@ public void resetModulesToAbsolute(){ public ChassisSpeeds getChasisHeading(){ Pose2d now = getPose(); - return ChassisSpeeds.fromFieldRelativeSpeeds( + // return swerveOdometry. + return ChassisSpeeds.fromRobotRelativeSpeeds( now.getX(), now.getY(), now.getRotation().getRadians(), From b2b102d16b9c2f7e4ec5562d5ad340f71483f6ff Mon Sep 17 00:00:00 2001 From: Paden O'Neal Date: Thu, 29 Feb 2024 23:05:51 -0700 Subject: [PATCH 08/10] I edited the RobotContainer file to allow for switchover if driver controls fail for some reason. :( --- src/main/java/frc/robot/RobotContainer.java | 48 ++++++++++++++++++--- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0dc6ba0..9377bc2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,6 +52,9 @@ public class RobotContainer { private final Joystick driver = new Joystick(0); private final XboxController secondary = new XboxController(1); + // FALLOVER JUST IN CASE MAIN CONTROLLER FAILS! PLZ CHECK MY SYNTAX SINCE MY COMPUTER DOES NOT HAVE ALL OF THE NESSESARY THINGS INSTALLED :D + private boolean main_failed = false; + private final PowerDistribution PDP = new PowerDistribution(); // /* XBOX CONTROLOR */ @@ -109,6 +112,34 @@ public class RobotContainer { * this is the main class most things stem from. only thing above this is the robot.java that * connects this to the driverstation */ + private double getTranslationAxisMaybe() { + if (main_failed) { + return secondary.getRawAxis(sLy); + } + return -driver.getRawAxis(translationAxis); + } + + private double getStrafeAxisMaybe() { + if (main_failed) { + return secondary.getRawAxis(sLX); + } + return -driver.getRawAxis(strafeAxis); + } + + private double getRotationAxisMaybe() { + if (main_failed) { + return secondary.getRawAxis(sRX); + } + return -driver.getRawAxis(rotationAxis); + } + + private double getThrottleAxisMaybe() { + if (main_failed) { + return 1.00; + } + return -driver.getRawAxis(throttleAxis); + } + public RobotContainer() { /**sets the Teleop command, uses TeleopSwever class to connect the swerve drive and controls * runs the Swerve.Drive() function periodicly @@ -116,10 +147,10 @@ public RobotContainer() { s_Swerve.setDefaultCommand( new TeleopSwerve( s_Swerve, - () -> -driver.getRawAxis(translationAxis), - () -> -driver.getRawAxis(strafeAxis), - () -> -driver.getRawAxis(rotationAxis), - () -> -driver.getRawAxis(throttleAxis), + () -> getTranslationAxisMaybe(), + () -> getStrafeAxisMaybe(), + () -> getRotationAxisMaybe(), + () -> getThrottleAxisMaybe(), () -> robotCentric.getAsBoolean(), theLEDs ) @@ -205,7 +236,14 @@ private void configureButtonBindings() { new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),theLEDs)); new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); - + + // NEW STUFF I ADDED! PLZ CHECK BECAUSE I KNOW I DID NOT DO THIS RIGHT! + //I DONT WANT TO SHOVE THIS IN A RANDOM COMMAND FOR FEAR THAT IT WILL NO LONGER BE ABLE TO ACCESS THE main_failed VARIABLE + if (secondary.getLeftBumperPressed() && main_failed) { + main_failed = false; + } else { + main_failed = true; + } } public void updateInfo() { From ff9cf051214408907e4286c5ddd3ec6d080b5d22 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Sat, 2 Mar 2024 00:19:28 -0700 Subject: [PATCH 09/10] Work --- .pathplanner/settings.json | 8 +- .../deploy/pathplanner/autos/AnadaAuto.auto | 25 ------ src/main/deploy/pathplanner/autos/Btm4.auto | 79 +++++++++++++++++ .../deploy/pathplanner/autos/BtmMain.auto | 67 ++++++++++++++ src/main/deploy/pathplanner/autos/Btmalt.auto | 61 +++++++++++++ src/main/deploy/pathplanner/autos/Mid4.auto | 85 ++++++++++++++++++ .../deploy/pathplanner/autos/MidMain.auto | 56 +++++------- src/main/deploy/pathplanner/autos/Pre.auto | 75 ---------------- .../deploy/pathplanner/autos/TestAuto.auto | 25 ------ .../deploy/pathplanner/autos/TopMain.auto | 55 ++++++++++++ .../deploy/pathplanner/autos/bigboyauto.auto | 37 -------- .../autos/{New Auto.auto => test.auto} | 12 +-- .../deploy/pathplanner/paths/BTM-SHOOT.path | 52 +++++++++++ .../deploy/pathplanner/paths/BTM-TAXI.path | 64 ++++++++++++++ .../deploy/pathplanner/paths/BTM-TO-2.path | 63 +++++++++++++ .../deploy/pathplanner/paths/BTM-TO-3.path | 70 +++++++++++++++ .../deploy/pathplanner/paths/BTM-TO-M4.path | 88 +++++++++++++++++++ .../deploy/pathplanner/paths/BTM-TO-M5.path | 82 +++++++++++++++++ .../deploy/pathplanner/paths/M-SHOOT.path | 10 +-- src/main/deploy/pathplanner/paths/M-TO-1.path | 40 ++++++--- src/main/deploy/pathplanner/paths/M-TO-2.path | 70 +++++++++++++++ src/main/deploy/pathplanner/paths/M-TO-3.path | 70 +++++++++++++++ .../deploy/pathplanner/paths/MB-TO-SHOOT.path | 64 ++++++++++++++ .../pathplanner/paths/SmartLaunchMid.path | 70 +++++++++++++++ .../paths/{New New Path.path => T-SHOOT.path} | 22 ++--- src/main/deploy/pathplanner/paths/T-TO-1.path | 38 +++++--- src/main/deploy/pathplanner/paths/T-TO-2.path | 38 +++++--- src/main/deploy/pathplanner/paths/T-TO-3.path | 38 +++++--- .../deploy/pathplanner/paths/T-TO-M1.path | 38 +++++--- .../deploy/pathplanner/paths/T-TO-M2.path | 38 +++++--- .../deploy/pathplanner/paths/T-TO-M3.path | 18 ++-- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 35 +++++--- .../frc/robot/commands/IntakeDefault.java | 2 +- .../java/frc/robot/commands/IntakeFix.java | 28 ++++-- .../java/frc/robot/commands/IntakeRun.java | 2 + .../java/frc/robot/commands/LaunchASAP.java | 1 + .../frc/robot/commands/LaunchControled.java | 74 ++++++++++++++++ .../java/frc/robot/commands/SmartShoot.java | 45 ++++++++++ .../java/frc/robot/subsystems/Intake.java | 22 +++++ .../java/frc/robot/subsystems/Launcher.java | 4 + .../java/frc/robot/subsystems/Swerve.java | 11 +-- 42 files changed, 1465 insertions(+), 319 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/AnadaAuto.auto create mode 100644 src/main/deploy/pathplanner/autos/Btm4.auto create mode 100644 src/main/deploy/pathplanner/autos/BtmMain.auto create mode 100644 src/main/deploy/pathplanner/autos/Btmalt.auto create mode 100644 src/main/deploy/pathplanner/autos/Mid4.auto delete mode 100644 src/main/deploy/pathplanner/autos/Pre.auto delete mode 100644 src/main/deploy/pathplanner/autos/TestAuto.auto create mode 100644 src/main/deploy/pathplanner/autos/TopMain.auto delete mode 100644 src/main/deploy/pathplanner/autos/bigboyauto.auto rename src/main/deploy/pathplanner/autos/{New Auto.auto => test.auto} (65%) create mode 100644 src/main/deploy/pathplanner/paths/BTM-SHOOT.path create mode 100644 src/main/deploy/pathplanner/paths/BTM-TAXI.path create mode 100644 src/main/deploy/pathplanner/paths/BTM-TO-2.path create mode 100644 src/main/deploy/pathplanner/paths/BTM-TO-3.path create mode 100644 src/main/deploy/pathplanner/paths/BTM-TO-M4.path create mode 100644 src/main/deploy/pathplanner/paths/BTM-TO-M5.path create mode 100644 src/main/deploy/pathplanner/paths/M-TO-2.path create mode 100644 src/main/deploy/pathplanner/paths/M-TO-3.path create mode 100644 src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path create mode 100644 src/main/deploy/pathplanner/paths/SmartLaunchMid.path rename src/main/deploy/pathplanner/paths/{New New Path.path => T-SHOOT.path} (68%) create mode 100644 src/main/java/frc/robot/commands/LaunchControled.java create mode 100644 src/main/java/frc/robot/commands/SmartShoot.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index bf63abf..58c4307 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.558, + "robotLength": 0.558, "holonomicMode": true, "pathFolders": [ "TEST", @@ -8,7 +8,9 @@ "BTM", "TOP" ], - "autoFolders": [], + "autoFolders": [ + "TEST" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/src/main/deploy/pathplanner/autos/AnadaAuto.auto b/src/main/deploy/pathplanner/autos/AnadaAuto.auto deleted file mode 100644 index 40dd28b..0000000 --- a/src/main/deploy/pathplanner/autos/AnadaAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 2 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Swaws" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Btm4.auto b/src/main/deploy/pathplanner/autos/Btm4.auto new file mode 100644 index 0000000..c00f446 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Btm4.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.78, + "y": 4.65 + }, + "rotation": -61.189206257026925 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M5" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M4" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BtmMain.auto b/src/main/deploy/pathplanner/autos/BtmMain.auto new file mode 100644 index 0000000..2f261e7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BtmMain.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.78, + "y": 4.65 + }, + "rotation": -61.189206257026925 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M5" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TAXI" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Btmalt.auto b/src/main/deploy/pathplanner/autos/Btmalt.auto new file mode 100644 index 0000000..302dad8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Btmalt.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7275269251393202, + "y": 4.630885159206216 + }, + "rotation": -60.10109816138542 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BTM-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M4" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M5" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid4.auto b/src/main/deploy/pathplanner/autos/Mid4.auto new file mode 100644 index 0000000..e188d68 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid4.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.171904793952038, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-1" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MidMain.auto b/src/main/deploy/pathplanner/autos/MidMain.auto index 5653ef7..f637a4a 100644 --- a/src/main/deploy/pathplanner/autos/MidMain.auto +++ b/src/main/deploy/pathplanner/autos/MidMain.auto @@ -1,12 +1,6 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 1.3707054194735169, - "y": 5.5430292057165325 - }, - "rotation": 0 - }, + "startingPose": null, "command": { "type": "sequential", "data": { @@ -24,35 +18,27 @@ } }, { - "type": "parallel", + "type": "path", + "data": { + "pathName": "M-TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeRun" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "path", - "data": { - "pathName": "M-TO-1" - } - } - ] - } - } - ] + "pathName": "M-TO-3" } }, { diff --git a/src/main/deploy/pathplanner/autos/Pre.auto b/src/main/deploy/pathplanner/autos/Pre.auto deleted file mode 100644 index d3239fd..0000000 --- a/src/main/deploy/pathplanner/autos/Pre.auto +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7728022663252532, - "y": 6.993104356579084 - }, - "rotation": 56.907309424713056 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "LaunchASAP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "IntakeRun" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "T-TO-1" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "New New Path" - } - }, - { - "type": "named", - "data": { - "name": "LaunchASAP" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto deleted file mode 100644 index f7457bf..0000000 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8912450873334795, - "y": 3.508246332731982 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "LaunchASAP" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopMain.auto b/src/main/deploy/pathplanner/autos/TopMain.auto new file mode 100644 index 0000000..55266dd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopMain.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "rotation": 56.907309424713056 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.0 + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "T-SHOOT" + } + }, + { + "type": "path", + "data": { + "pathName": "T-TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "T-SHOOT" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/bigboyauto.auto b/src/main/deploy/pathplanner/autos/bigboyauto.auto deleted file mode 100644 index e44c808..0000000 --- a/src/main/deploy/pathplanner/autos/bigboyauto.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.522729427225236, - "y": 5.531335051274091 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Oval" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "TheBoxer" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/test.auto similarity index 65% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/test.auto index e143544..9a003af 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.6513651260920754, - "y": 5.566417514601411 + "x": 1.183598948394478, + "y": 5.5430292057165325 }, "rotation": 0 }, @@ -14,18 +14,18 @@ { "type": "path", "data": { - "pathName": "TestPath" + "pathName": "M-SHOOT" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "TheBoxer" + "name": "LaunchASAP" } } ] } }, - "folder": null, + "folder": "TEST", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path new file mode 100644 index 0000000..6d42fb5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4993411183403562, + "y": 4.034483282641779 + }, + "prevControl": null, + "nextControl": { + "x": 1.2537638750491178, + "y": 4.104648209296418 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": { + "x": 1.1952931028369176, + "y": 4.139730672623739 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BTM SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -58.39249775375112, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TAXI.path b/src/main/deploy/pathplanner/paths/BTM-TAXI.path new file mode 100644 index 0000000..6ac425c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TAXI.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.943718987153074, + "y": 2.6311847495489866 + }, + "prevControl": null, + "nextControl": { + "x": 2.9437189871530745, + "y": 2.6311847495489866 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.890346316305168, + "y": 0.7835083476434771 + }, + "prevControl": { + "x": 5.890346316305168, + "y": 0.7835083476434771 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-2.path b/src/main/deploy/pathplanner/paths/BTM-TO-2.path new file mode 100644 index 0000000..642b0a9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-2.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.9029392417759186, + "y": 4.256672217048139 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 2.4699559370628714, + "y": 5.601499977928731 + }, + "prevControl": { + "x": 1.4699559370628714, + "y": 5.601499977928731 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-3.path b/src/main/deploy/pathplanner/paths/BTM-TO-3.path new file mode 100644 index 0000000..72d8c43 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.1602106395095981, + "y": 4.256672217048139 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 2.4699559370628714, + "y": 4.128036518181299 + }, + "prevControl": { + "x": 1.6981417438618356, + "y": 4.139730672623739 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path new file mode 100644 index 0000000..2ba3da8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.9029392417759181, + "y": 4.256672217048139 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 7.942820216124763, + "y": 2.4440782784699477 + }, + "prevControl": { + "x": 5.416882856557737, + "y": 0.2338830888487989 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 7.329212122874697, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.8, + "constraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path new file mode 100644 index 0000000..5c38c08 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path @@ -0,0 +1,82 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.3940937283583967, + "y": 2.1049477996391905 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 7.884349443912563, + "y": 0.7835083476434771 + }, + "prevControl": { + "x": 5.440271165442615, + "y": 0.573013567679558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.85, + "constraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path index ddd030d..4c9740b 100644 --- a/src/main/deploy/pathplanner/paths/M-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.3707054194735169, + "x": 1.183598948394478, "y": 5.5430292057165325 }, "prevControl": null, "nextControl": { - "x": 1.4306130406658415, + "x": 1.2435065695868026, "y": 5.546357406893884 }, "isLocked": false, @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 1.6396709716496354, + "x": 1.5928943538798759, "y": 5.5430292057165325 }, "prevControl": { - "x": 1.5801342990468157, + "x": 1.5333576812770562, "y": 5.535587121641179 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "MID SHOOT" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path index f209eca..bb9a10b 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-1.path +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.8501657516135543, - "y": 5.556066607628711 + "x": 1.5928943538798759, + "y": 5.5430292057165325 }, "prevControl": null, "nextControl": { - "x": 2.0489663771350335, - "y": 5.601499977928731 + "x": 1.5812001994374358, + "y": 6.303149244475128 }, "isLocked": false, - "linkedName": null + "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.189296230444312, - "y": 5.556066607628711 + "x": 2.5752033270448305, + "y": 6.841080348827365 }, "prevControl": { - "x": 2.0489663771350335, - "y": 5.5430292057165325 + "x": 1.5578118905525564, + "y": 6.794303731057606 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -39,7 +57,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 6.159545400201877, + "rotation": 22.47943439710317, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/M-TO-2.path b/src/main/deploy/pathplanner/paths/M-TO-2.path new file mode 100644 index 0000000..0004efc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-TO-2.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.791694979401355, + "y": 5.5884625760165525 + }, + "isLocked": false, + "linkedName": "MID SHOOT" + }, + { + "anchor": { + "x": 2.5284267092750707, + "y": 5.556066607628711 + }, + "prevControl": { + "x": 2.388096855965792, + "y": 5.5430292057165325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 6.159545400201877, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-TO-3.path b/src/main/deploy/pathplanner/paths/M-TO-3.path new file mode 100644 index 0000000..75e82f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-TO-3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.3005404928188773, + "y": 4.420390379242298 + }, + "isLocked": false, + "linkedName": "MID SHOOT" + }, + { + "anchor": { + "x": 2.61028579037215, + "y": 4.326837143702777 + }, + "prevControl": { + "x": 2.493344245947751, + "y": 4.736132549188175 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -44.999999999999964, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path new file mode 100644 index 0000000..7000475 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.182700177366167, + "y": 1.567016695286953 + }, + "prevControl": null, + "nextControl": { + "x": 5.919731497582651, + "y": 1.4734634597474332 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": { + "x": 1.0315749406427586, + "y": 2.432384124027509 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BTM SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.75, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.264512298079936, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": 1.5481576989780002, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path new file mode 100644 index 0000000..803b271 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.160210639509598, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 1.5812001994374358, + "y": 5.55472336015897 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": { + "x": 1.3005404928188773, + "y": 5.5430292057165325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MID SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/T-SHOOT.path similarity index 68% rename from src/main/deploy/pathplanner/paths/New New Path.path rename to src/main/deploy/pathplanner/paths/T-SHOOT.path index 2f39893..a6efb21 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/T-SHOOT.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.2009903848867527, - "y": 6.385008325572207 + "x": 1.6396709716496354, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 2.2009903848867527, - "y": 5.921648206923535 + "x": 1.008186631757879, + "y": 6.607197259978566 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.2009903848867527, - "y": 5.636582441256051 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": { - "x": 2.2009903848867527, - "y": 6.326933851561612 + "x": 1.2420697206066778, + "y": 6.747527113287845 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" } ], "rotationTargets": [], @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 58.51656810879132, "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TOP", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-1.path b/src/main/deploy/pathplanner/paths/T-TO-1.path index 0803358..7fbf13c 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.4408703461281565, - "y": 6.805997885500045 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 1.932024832710634, - "y": 6.9346335843668845 + "x": 1.3707054194735169, + "y": 6.817692039942485 }, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" }, { "anchor": { - "x": 2.282849465983832, - "y": 6.946327738809324 + "x": 2.5518150181599504, + "y": 7.004798511021523 }, "prevControl": { - "x": 1.7566125160740347, - "y": 6.934633584366883 + "x": 2.025578068250153, + "y": 6.993104356579082 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/T-TO-2.path b/src/main/deploy/pathplanner/paths/T-TO-2.path index 2ff8c3a..960ca4a 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.4759528094554764, - "y": 6.665668032190766 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 2.2126845393291923, - "y": 6.2797609355902475 + "x": 1.6162826627647553, + "y": 6.303149244475128 }, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" }, { "anchor": { - "x": 2.5292958694025494, - "y": 6.1292382044223555 + "x": 2.69214487146923, + "y": 5.84707722121997 }, "prevControl": { - "x": 1.7566125160740347, - "y": 6.373314171129768 + "x": 1.9194615181407153, + "y": 6.091153187927382 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/T-TO-3.path b/src/main/deploy/pathplanner/paths/T-TO-3.path index 9128f15..8463bc3 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0432690950851984, - "y": 6.852774503269805 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 3.0078870414151084, - "y": 6.385008325572207 + "x": 2.844168879220949, + "y": 6.221290163378049 }, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" }, { "anchor": { - "x": 2.17799714878709, - "y": 4.113892912470511 + "x": 2.5167325548326307, + "y": 4.081259900411539 }, "prevControl": { - "x": 1.1779971487870902, - "y": 4.113892912470511 + "x": 1.1251281761822782, + "y": 4.057871591526659 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M1.path b/src/main/deploy/pathplanner/paths/T-TO-M1.path index db07c8c..6001ca6 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.4525645005705963, - "y": 6.7241388044029655 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 2.5619288814615206, - "y": 6.2341695361761404 + "x": 1.9889153137819637, + "y": 6.199087072848821 }, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" }, { "anchor": { @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.7340637564158, - "y": 7.136910850398278 + "x": 7.9545143705672015, + "y": 7.226987445427883 }, "prevControl": { - "x": 7.2625838945371575, - "y": 6.600718066301 + "x": 7.483034508688559, + "y": 6.690794661330605 }, "nextControl": null, "isLocked": false, @@ -57,7 +57,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 1.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M2.path b/src/main/deploy/pathplanner/paths/T-TO-M2.path index b5915d9..5dfa5e0 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.3005404928188775, - "y": 6.712444649960525 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 2.4099048737098014, - "y": 6.2224753817337 + "x": 1.9889153137819633, + "y": 6.199087072848821 }, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" }, { "anchor": { @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.623127318326708, - "y": 5.99981235998508 + "x": 8.001290988336962, + "y": 5.88215968454729 }, "prevControl": { - "x": 6.726391110439877, - "y": 6.156972313944628 + "x": 7.104554780450131, + "y": 6.0393196385068375 }, "nextControl": null, "isLocked": false, @@ -57,7 +57,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M3.path b/src/main/deploy/pathplanner/paths/T-TO-M3.path index 78e0219..dd71b42 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M3.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.2537638750491173, - "y": 6.794303731057604 + "x": 0.8795509328910394, + "y": 6.689056341075646 }, "prevControl": null, "nextControl": { - "x": 2.363128255940041, - "y": 6.304334462830779 + "x": 1.9889153137819628, + "y": 6.199087072848821 }, "isLocked": false, - "linkedName": null + "linkedName": "TOP SHOOT" }, { "anchor": { @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 7.641616724674891, - "y": 4.483681039434152 + "x": 8.012985142779401, + "y": 4.280060525933018 }, "prevControl": { - "x": 6.74488051678806, - "y": 4.6408409933937 + "x": 7.11624893489257, + "y": 4.437220479892566 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4c6867c..df70e97 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -169,7 +169,7 @@ public static final class Launcher { public static final double LaunchV = 100; public static final double LaunchP = 1; - public static final double LaunchASAPTime = 2; + public static final double LaunchASAPTime = 1; public static final double LaunchStopTime = 1; public static final double P = 0.08; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9377bc2..ce7dfa6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathPlannerPath; import com.revrobotics.CANSparkMax; import edu.wpi.first.cameraserver.CameraServer; @@ -159,6 +160,7 @@ public RobotContainer() { intake.setDefaultCommand(new IntakeDefault(intake,LIM)); NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); + NamedCommands.registerCommand("EndIntake", new IntakeDefault(intake, LIM)); NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); @@ -167,7 +169,7 @@ public RobotContainer() { //Build the auto chooser // autoChooser = AutoBuilder.buildAutoChooser(); - autoChooser = AutoBuilder.buildAutoChooser("MidMain"); + autoChooser = AutoBuilder.buildAutoChooser("Btm4"); SmartDashboard.putData("Auto Chooser", autoChooser); musiChooser = theBand.Buildchoser(); @@ -232,10 +234,12 @@ public RobotContainer() { private void configureButtonBindings() { /** Driver Buttons */ zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); - new JoystickButton(secondary, 4).onTrue(new LaunchASAP(intake,launcher,theLEDs)); + // new JoystickButton(secondary, 4).onTrue(new LaunchASAP(intake,launcher,theLEDs)); + new JoystickButton(secondary, 4).onTrue(new LaunchControled(intake,launcher,theLEDs,new JoystickButton(secondary, 4),new JoystickButton(secondary, 6))); new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); - new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),theLEDs)); + new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),new JoystickButton(secondary, 6),theLEDs)); new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); +<<<<<<< Updated upstream // NEW STUFF I ADDED! PLZ CHECK BECAUSE I KNOW I DID NOT DO THIS RIGHT! //I DONT WANT TO SHOVE THIS IN A RANDOM COMMAND FOR FEAR THAT IT WILL NO LONGER BE ABLE TO ACCESS THE main_failed VARIABLE @@ -244,11 +248,15 @@ private void configureButtonBindings() { } else { main_failed = true; } +======= + new JoystickButton(secondary, 8).onTrue(smartLaunch()); +>>>>>>> Stashed changes } public void updateInfo() { SmartDashboard.putBoolean("IntakeDeployed", intake.getMode()); SmartDashboard.putBoolean("Note", LIM.get()); + SmartDashboard.putBoolean("Launcher Spin", launcher.spin); // {"D","IO","IR","L","LS"}; SmartDashboard.putBoolean("D", theLEDs.getMode() == "D"); @@ -267,13 +275,12 @@ public void updateInfo() { SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); - SmartDashboard.putData(PDP); - System.out.println("Radio"); - System.out.println(PDP.getCurrent(15)); - System.out.println("RIO"); - System.out.println(PDP.getCurrent(20)); - System.out.println("Total"); - System.out.println(PDP.getTotalCurrent()); + // System.out.println("Radio"); + // System.out.println(PDP.getCurrent(15)); + // System.out.println("RIO"); + // System.out.println(PDP.getCurrent(20)); + // System.out.println("Total"); + // System.out.println(PDP.getTotalCurrent()); intake.updateData(); @@ -301,6 +308,14 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } + public Command smartLaunch() { + // Load the path you want to follow using its name in the GUI + PathPlannerPath path = PathPlannerPath.fromPathFile("Example Path"); + + // Create a path following command using AutoBuilder. This will also trigger event markers. + return AutoBuilder.followPath(path); + } + public void teleopInit() { theLEDs.SetFull(0, 0, 255); theLEDs.setMode("D"); diff --git a/src/main/java/frc/robot/commands/IntakeDefault.java b/src/main/java/frc/robot/commands/IntakeDefault.java index f14ad42..a870301 100644 --- a/src/main/java/frc/robot/commands/IntakeDefault.java +++ b/src/main/java/frc/robot/commands/IntakeDefault.java @@ -42,7 +42,7 @@ public void execute() { @Override public void end(boolean interrupted) { - System.out.println("Start IntakeDefault"); + System.out.println("End IntakeDefault"); } @Override diff --git a/src/main/java/frc/robot/commands/IntakeFix.java b/src/main/java/frc/robot/commands/IntakeFix.java index 5330933..e411dbc 100644 --- a/src/main/java/frc/robot/commands/IntakeFix.java +++ b/src/main/java/frc/robot/commands/IntakeFix.java @@ -1,5 +1,6 @@ package frc.robot.commands; +import edu.wpi.first.networktables.PubSub; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -8,7 +9,6 @@ import frc.robot.Constants; import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Launcher; import frc.robot.subsystems.LEDS; public class IntakeFix extends Command { @@ -16,9 +16,11 @@ public class IntakeFix extends Command { private Intake s_Intake; private DigitalInput LIM; private JoystickButton overrideButton; + private JoystickButton pushButton; // private Boolean end; private Boolean bHold; private Boolean end; + private Boolean push; private Timer timer; private LEDS theLEDs; @@ -27,13 +29,15 @@ public class IntakeFix extends Command { * * @param s_Intake Intake object */ - public IntakeFix(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, LEDS theLEDs) { + public IntakeFix(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, JoystickButton pushButton, LEDS theLEDs) { this.s_Intake = s_Intake; this.LIM = LIM; this.overrideButton = overrideButton; this.theLEDs = theLEDs; + this.pushButton = pushButton; // this.end = false; this.bHold = true; + this.push = false; this.end = false; addRequirements(s_Intake); } @@ -43,27 +47,39 @@ public void initialize() { // tell the intake to extend System.out.println("Start IntakeFix"); s_Intake.setDeploy(true); + // s_Intake.startIntake(); this.bHold = true; this.end = false; + this.push = false; timer = new Timer(); theLEDs.setMode("IF"); - System.out.println("set IF"); // timer.start(); } @Override public void execute() { if (bHold) bHold = overrideButton.getAsBoolean(); + if (!LIM.get() && !end) { + timer.start(); + // s_Intake.endIntake(); + end = true; + System.out.println("End1"); + theLEDs.setMode("IR"); + System.out.println("set IR"); + s_Intake.lightPull(false); + } + s_Intake.nudge(); - s_Intake.manualAcuation(); + System.out.println(timer.get()); System.out.println(timer.get() >= Constants.Intake.OverRun); } @Override public void end(boolean interrupted) { - System.out.println("End IntakeFix"); - if (!LIM.get()) { + System.out.println("End IntakeRun"); + s_Intake.lightPull(true); + if (end) { theLEDs.setMode("N"); System.out.println("set N"); } else { diff --git a/src/main/java/frc/robot/commands/IntakeRun.java b/src/main/java/frc/robot/commands/IntakeRun.java index de10858..da481b4 100644 --- a/src/main/java/frc/robot/commands/IntakeRun.java +++ b/src/main/java/frc/robot/commands/IntakeRun.java @@ -60,6 +60,7 @@ public void execute() { System.out.println("End1"); theLEDs.setMode("IR"); System.out.println("set IR"); + s_Intake.lightPull(false); } System.out.println(timer.get()); @@ -69,6 +70,7 @@ public void execute() { @Override public void end(boolean interrupted) { System.out.println("End IntakeRun"); + s_Intake.lightPull(true); if (end) { theLEDs.setMode("N"); System.out.println("set N"); diff --git a/src/main/java/frc/robot/commands/LaunchASAP.java b/src/main/java/frc/robot/commands/LaunchASAP.java index 7f32038..1cdddda 100644 --- a/src/main/java/frc/robot/commands/LaunchASAP.java +++ b/src/main/java/frc/robot/commands/LaunchASAP.java @@ -34,6 +34,7 @@ public void initialize() { timer.start(); launch = false; s_Launcher.startLaunch(); + s_Intake.lightPull(false); System.out.println("Start LaunchASAP"); theLEDs.setMode("LS"); System.out.println("set LS"); diff --git a/src/main/java/frc/robot/commands/LaunchControled.java b/src/main/java/frc/robot/commands/LaunchControled.java new file mode 100644 index 0000000..e7c19e5 --- /dev/null +++ b/src/main/java/frc/robot/commands/LaunchControled.java @@ -0,0 +1,74 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.hal.simulation.ConstBufferCallback; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; + +public class LaunchControled extends Command{ + private Intake s_Intake; + private Launcher s_Launcher; + private Timer timer; + private boolean launch; + private LEDS theLEDs; + private JoystickButton overrideButton; + private JoystickButton launchButton; + + + public LaunchControled(Intake s_Intake, Launcher s_Launcher, LEDS theLEDs, JoystickButton overrideButton, JoystickButton launchButton) { + this.s_Intake = s_Intake; + this.s_Launcher = s_Launcher; + this.timer = new Timer(); + this.launch = false; + this.theLEDs = theLEDs; + this.overrideButton = overrideButton; + this.launchButton = launchButton; + + addRequirements(s_Intake); + addRequirements(s_Launcher); + } + + @Override + public void initialize() { + timer = new Timer(); + // timer.start(); + launch = false; + s_Intake.lightPull(false); + s_Launcher.startLaunch(); + System.out.println("Start LaunchASAP"); + theLEDs.setMode("LS"); + System.out.println("set LS"); + + } + + @Override + public void execute() { + + if (launchButton.getAsBoolean()) { + timer.start(); + launch = true; + theLEDs.setMode("L"); + System.out.println("set L"); + s_Intake.pushIntake(false); + System.out.println("Push"); + } + } + + @Override + public void end(boolean interrupted) { + s_Launcher.endLaunch(); + s_Intake.pushIntake(true); + System.out.println("End LaunchControled"); + theLEDs.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + return (timer.get() >= (Constants.Launcher.LaunchStopTime) && launch); + } +} diff --git a/src/main/java/frc/robot/commands/SmartShoot.java b/src/main/java/frc/robot/commands/SmartShoot.java new file mode 100644 index 0000000..ef16016 --- /dev/null +++ b/src/main/java/frc/robot/commands/SmartShoot.java @@ -0,0 +1,45 @@ +package frc.robot.commands; + +import edu.wpi.first.networktables.PubSub; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; + +public class SmartShoot extends Command { + + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public SmartShoot() { + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + + @Override + public void end(boolean interrupted) { + + } + + @Override + public boolean isFinished() { + return false; + } + +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 51b20c9..8833ac2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -132,6 +132,28 @@ public void toggleDeploy() { } } + public void lightPull(boolean stop) { + if(!stop) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + + } + + public void lightPush(boolean stop) { + if(!stop) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + + } + public void nudge() { if (secondaryController.getPOV() != -1) { System.out.println(secondaryController.getPOV()); diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index f997c13..ec4e725 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -19,6 +19,7 @@ public class Launcher extends SubsystemBase { public SparkPIDController m_RPID; public CANSparkMax m_launcherLeft; public SparkPIDController m_LPID; + public boolean spin; public Intake s_Intake; @@ -28,6 +29,7 @@ public Launcher(int lMotorId, int rMotorId) { m_launcherLeft.restoreFactoryDefaults(); m_launcherRight.restoreFactoryDefaults(); + spin = false; } @@ -35,11 +37,13 @@ public void startLaunch() { System.out.println("start launch"); m_launcherLeft.set(-Constants.Launcher.LaunchP); m_launcherRight.set(Constants.Launcher.LaunchP); + spin = true; } public void endLaunch() { m_launcherLeft.set(0); m_launcherRight.set(0); + spin = false; } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0908e2b..7997519 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -59,7 +59,7 @@ public Swerve(LEDS iled) { new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class new PIDConstants(5.0, 1.0, 0.0), // Translation PID constants new PIDConstants(5.0, 1.0, 0.0), // Rotation PID constants - 3.5, // Max module speed, in m/s + 4.5, // Max module speed, in m/s 0.3, // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options here ), @@ -188,14 +188,7 @@ public void resetModulesToAbsolute(){ } public ChassisSpeeds getChasisHeading(){ - Pose2d now = getPose(); - // return swerveOdometry. - return ChassisSpeeds.fromRobotRelativeSpeeds( - now.getX(), - now.getY(), - now.getRotation().getRadians(), - this.getHeading() - ); + return Constants.Swerve.swerveKinematics.toChassisSpeeds(getModuleStates()); } @Override From 460cefdd295a7f64848b106529834ef693f88508 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Sat, 2 Mar 2024 00:25:07 -0700 Subject: [PATCH 10/10] remove main_failed --- src/main/java/frc/robot/RobotContainer.java | 50 ++------------------- 1 file changed, 4 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce7dfa6..c792693 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,9 +53,6 @@ public class RobotContainer { private final Joystick driver = new Joystick(0); private final XboxController secondary = new XboxController(1); - // FALLOVER JUST IN CASE MAIN CONTROLLER FAILS! PLZ CHECK MY SYNTAX SINCE MY COMPUTER DOES NOT HAVE ALL OF THE NESSESARY THINGS INSTALLED :D - private boolean main_failed = false; - private final PowerDistribution PDP = new PowerDistribution(); // /* XBOX CONTROLOR */ @@ -113,34 +110,6 @@ public class RobotContainer { * this is the main class most things stem from. only thing above this is the robot.java that * connects this to the driverstation */ - private double getTranslationAxisMaybe() { - if (main_failed) { - return secondary.getRawAxis(sLy); - } - return -driver.getRawAxis(translationAxis); - } - - private double getStrafeAxisMaybe() { - if (main_failed) { - return secondary.getRawAxis(sLX); - } - return -driver.getRawAxis(strafeAxis); - } - - private double getRotationAxisMaybe() { - if (main_failed) { - return secondary.getRawAxis(sRX); - } - return -driver.getRawAxis(rotationAxis); - } - - private double getThrottleAxisMaybe() { - if (main_failed) { - return 1.00; - } - return -driver.getRawAxis(throttleAxis); - } - public RobotContainer() { /**sets the Teleop command, uses TeleopSwever class to connect the swerve drive and controls * runs the Swerve.Drive() function periodicly @@ -148,10 +117,10 @@ public RobotContainer() { s_Swerve.setDefaultCommand( new TeleopSwerve( s_Swerve, - () -> getTranslationAxisMaybe(), - () -> getStrafeAxisMaybe(), - () -> getRotationAxisMaybe(), - () -> getThrottleAxisMaybe(), + () -> -driver.getRawAxis(translationAxis), + () -> -driver.getRawAxis(strafeAxis), + () -> -driver.getRawAxis(rotationAxis), + () -> -driver.getRawAxis(throttleAxis), () -> robotCentric.getAsBoolean(), theLEDs ) @@ -239,18 +208,7 @@ private void configureButtonBindings() { new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),new JoystickButton(secondary, 6),theLEDs)); new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); -<<<<<<< Updated upstream - - // NEW STUFF I ADDED! PLZ CHECK BECAUSE I KNOW I DID NOT DO THIS RIGHT! - //I DONT WANT TO SHOVE THIS IN A RANDOM COMMAND FOR FEAR THAT IT WILL NO LONGER BE ABLE TO ACCESS THE main_failed VARIABLE - if (secondary.getLeftBumperPressed() && main_failed) { - main_failed = false; - } else { - main_failed = true; - } -======= new JoystickButton(secondary, 8).onTrue(smartLaunch()); ->>>>>>> Stashed changes } public void updateInfo() {