From 0c222734f77ba5d6263eb5e55af203958ba0eaf3 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 9 Feb 2024 20:07:00 -0600 Subject: [PATCH 1/9] feat: intake by setting desired states. Using a DutyCycle encoder for the absolute popition. --- src/main/java/frc/robot/RobotContainer.java | 19 ++++- .../robot/subsystems/ShooterSubsystem.java | 28 +++++++ .../java/frc/robot/subsystems/intake.java | 21 ++++++ vendordeps/REVLib.json | 74 +++++++++++++++++++ 4 files changed, 140 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/intake.java create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..8cfa519 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,17 +4,32 @@ package frc.robot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.ShooterSubsystem; public class RobotContainer { + public final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + private XboxController controller = new XboxController(0); public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + new JoystickButton(controller, Button.kB.value) + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(0.75), m_ShooterSubsystem)) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem)); + new JoystickButton(controller, Button.kA.value) + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-0.75), m_ShooterSubsystem)) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem)); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..cdba89a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +public class ShooterSubsystem extends SubsystemBase { + /** Creates a new ShooterSubsystem. */ + CANSparkFlex m_bottom = new CANSparkFlex(20, MotorType.kBrushless); + CANSparkFlex m_top = new CANSparkFlex(35, MotorType.kBrushless); + public ShooterSubsystem() { + + + } + public void spin(double speed){ + m_bottom.set(speed); + m_top.set(speed); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Speed", m_bottom.); + } +} \ No newline at end of file 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..694d3b5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake.java @@ -0,0 +1,21 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class intake extends SubsystemBase { + /** Creates a new intake. */ + public intake() {} + + void load(int speed){ + //TODO: Implement load/unload through speed (which is a trigger) + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} 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 479bf3cb0f195ab80852653947dc4549d31e21a3 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 9 Feb 2024 20:13:14 -0600 Subject: [PATCH 2/9] feat: DutyCylce encoder and setting position via desired states. --- .../robot/subsystems/ShooterSubsystem.java | 6 + .../java/frc/robot/subsystems/intake.java | 50 ++- vendordeps/Phoenix6.json | 339 ++++++++++++++++++ 3 files changed, 392 insertions(+), 3 deletions(-) create mode 100644 vendordeps/Phoenix6.json diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index cdba89a..8b34488 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -20,6 +20,12 @@ public void spin(double speed){ m_bottom.set(speed); m_top.set(speed); } + + + + + + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/intake.java b/src/main/java/frc/robot/subsystems/intake.java index 694d3b5..c0bd4b4 100644 --- a/src/main/java/frc/robot/subsystems/intake.java +++ b/src/main/java/frc/robot/subsystems/intake.java @@ -4,15 +4,59 @@ package frc.robot.subsystems; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkMax; +import com.revrobotics.EncoderType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DigitalSource; +import edu.wpi.first.wpilibj.DutyCycle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.CounterBase.EncodingType; +import edu.wpi.first.math.controller.PIDController; + import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class intake extends SubsystemBase { +public class Intake extends SubsystemBase { + //TODO: add constants + CANSparkFlex intakeMotor = new CANSparkFlex(0, MotorType.kBrushless); + CANSparkFlex armMotor = new CANSparkFlex(0, MotorType.kBrushless); + + + PIDController intakeVeloPID = new PIDController(0,0,0); + PIDController armPID = new PIDController(0,0,0); + DutyCycleEncoder armEncoder = new DutyCycleEncoder(0); + + + + + /** Creates a new intake. */ - public intake() {} + public Intake() { + + } - void load(int speed){ + void load(double speed){ + intakeMotor.set(speed); //TODO: Implement load/unload through speed (which is a trigger) } + /** + *@param angle in radians + *@everyone join vc we are playing gartic phone + */ + + public void tiltToAngle(double angle) { + double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); + armMotor.set(motorPower); + } + @Override public void periodic() { diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..2b7d172 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.2.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.2.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.2.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.2.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.2.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.2.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.2.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.2.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.2.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.2.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.2.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.2.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.2.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file From e99195ff815cc78ca73caf2251b601fcb0746647 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 12 Feb 2024 16:47:30 -0800 Subject: [PATCH 3/9] feat: finished driver controls for the intake --- src/main/java/frc/robot/IntakeConstants.java | 6 ++++++ src/main/java/frc/robot/RobotContainer.java | 18 ++++++++++++++++++ .../{intake.java => IntakeSubsystem.java} | 18 +++++++++++++----- 3 files changed, 37 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/IntakeConstants.java rename src/main/java/frc/robot/subsystems/{intake.java => IntakeSubsystem.java} (86%) diff --git a/src/main/java/frc/robot/IntakeConstants.java b/src/main/java/frc/robot/IntakeConstants.java new file mode 100644 index 0000000..8fc71b7 --- /dev/null +++ b/src/main/java/frc/robot/IntakeConstants.java @@ -0,0 +1,6 @@ +package frc.robot; + +public class IntakeConstants { + public static final double IntakeDroppedAngle = 9.0; + public static final double IntakeRaisedAngle = 9.0; +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8cfa519..cab2f9f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,14 +10,32 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class RobotContainer { public final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + public final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + private boolean IntakeDropped = false; + private boolean lastAButton = false; + private XboxController controller = new XboxController(0); public RobotContainer() { configureBindings(); + m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem)); + } + + public void periodic(){ + if(controller.getAButton()){ + lastAButton = true; + if (!lastAButton) + IntakeDropped = !IntakeDropped; + } + else { + lastAButton = false; + } } private void configureBindings() { diff --git a/src/main/java/frc/robot/subsystems/intake.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java similarity index 86% rename from src/main/java/frc/robot/subsystems/intake.java rename to src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c0bd4b4..e91f9f2 100644 --- a/src/main/java/frc/robot/subsystems/intake.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Intake extends SubsystemBase { +public class IntakeSubsystem extends SubsystemBase { //TODO: add constants CANSparkFlex intakeMotor = new CANSparkFlex(0, MotorType.kBrushless); CANSparkFlex armMotor = new CANSparkFlex(0, MotorType.kBrushless); @@ -39,15 +39,22 @@ public class Intake extends SubsystemBase { /** Creates a new intake. */ - public Intake() { + public IntakeSubsystem() { } - - void load(double speed){ + /** + * + * @param speed motor power to apply to intake + *@param angle in radians + * + */ + public void load(double speed, double angle){ intakeMotor.set(speed); - //TODO: Implement load/unload through speed (which is a trigger) + tiltToAngle(angle); + } /** + * *@param angle in radians *@everyone join vc we are playing gartic phone */ @@ -56,6 +63,7 @@ public void tiltToAngle(double angle) { double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); armMotor.set(motorPower); } + @Override From e290e263a5ba902312005cc192b306acd01c0e55 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 16 Feb 2024 17:23:41 -0800 Subject: [PATCH 4/9] chore: sent stuff to constants --- src/main/java/frc/robot/Constants.java | 33 +++++++++++++++++++ src/main/java/frc/robot/IntakeConstants.java | 6 ---- src/main/java/frc/robot/RobotContainer.java | 20 ++++++++--- .../frc/robot/subsystems/IntakeSubsystem.java | 30 +++++++---------- .../robot/subsystems/ShooterSubsystem.java | 1 + 5 files changed, 61 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc/robot/Constants.java delete mode 100644 src/main/java/frc/robot/IntakeConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..e54b19f --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +public final class Constants { + + +//Intake PID and Encoder Constants + public static class IntakeConstants { + public static final double kIntakeDroppedAngle = 9.0; + public static final double kIntakeRaisedAngle = 9.0; + public static final int kIntakeMotorID = 0; + public static final int kArmMotorID = 0; + public static final double kIntakeP = 0; + public static final double kIntakeI = 0; + public static final double kIntakeD = 0; + public static final double kArmP = 0; + public static final double kArmI = 0; + public static final double kArmD = 0; + public static final int kArmEncoderCh = 0; + } + + //Shooter subsystem speed constants + public static class ShooterConstants { + public static final double kSpinSpeedTrue = 0.75; + public static final double kSpinSpeedFalse = 0; + + + } +} + diff --git a/src/main/java/frc/robot/IntakeConstants.java b/src/main/java/frc/robot/IntakeConstants.java deleted file mode 100644 index 8fc71b7..0000000 --- a/src/main/java/frc/robot/IntakeConstants.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot; - -public class IntakeConstants { - public static final double IntakeDroppedAngle = 9.0; - public static final double IntakeRaisedAngle = 9.0; -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18ca027..44b12f6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,10 +10,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class RobotContainer { + //The robots subsystems are defined here public final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); public final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); private boolean IntakeDropped = false; @@ -22,9 +25,13 @@ public class RobotContainer { private XboxController controller = new XboxController(0); public RobotContainer() { configureBindings(); - m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem)); + m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.kIntakeDroppedAngle : IntakeConstants.kIntakeRaisedAngle),m_IntakeSubsystem)); } + + /** + * checks for intake button on controller every tick + */ public void periodic(){ if(controller.getAButton()){ lastAButton = true; @@ -36,13 +43,16 @@ public void periodic(){ } } + /** + * Use this method to define your button->command mappings. + */ private void configureBindings() { new JoystickButton(controller, Button.kB.value) - .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(0.75), m_ShooterSubsystem)) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem)); + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedTrue), m_ShooterSubsystem)) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedFalse), m_ShooterSubsystem)); new JoystickButton(controller, Button.kA.value) - .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-0.75), m_ShooterSubsystem)) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem)); + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-ShooterConstants.kSpinSpeedTrue), m_ShooterSubsystem)) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedFalse), m_ShooterSubsystem)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 6408f88..794a666 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -11,21 +11,17 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; +//creates new motors and pid controllers lmao public class IntakeSubsystem extends SubsystemBase { - //TODO: add constants - CANSparkFlex intakeMotor = new CANSparkFlex(0, MotorType.kBrushless); - CANSparkFlex armMotor = new CANSparkFlex(0, MotorType.kBrushless); + CANSparkFlex intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); + CANSparkFlex armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - - PIDController intakeVeloPID = new PIDController(0,0,0); - PIDController armPID = new PIDController(0,0,0); - DutyCycleEncoder armEncoder = new DutyCycleEncoder(0); + PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP,IntakeConstants.kIntakeI,IntakeConstants.kIntakeD); + PIDController armPID = new PIDController(IntakeConstants.kArmP,IntakeConstants.kArmI,IntakeConstants.kArmD); + DutyCycleEncoder armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); - - - - /** Creates a new intake. */ public IntakeSubsystem() { @@ -41,19 +37,17 @@ public void load(double speed, double angle){ tiltToAngle(angle); } - /** - * - *@param angle in radians - *@everyone join vc we are playing gartic phone - */ + /** + * + * @param angle motor to apply to intake + * + */ public void tiltToAngle(double angle) { double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); armMotor.set(motorPower); } - - @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8ff662b..1ea808c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + public class ShooterSubsystem extends SubsystemBase { /** Creates a new ShooterSubsystem. */ CANSparkFlex m_bottom = new CANSparkFlex(20, MotorType.kBrushless); From b35629d9ef68302e03acd724f53168ade173ea4f Mon Sep 17 00:00:00 2001 From: Programming Date: Mon, 19 Feb 2024 17:55:40 -0800 Subject: [PATCH 5/9] fix: merged well enough this time --- src/main/java/frc/robot/Constants.java | 92 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 22 +++-- .../robot/subsystems/ShooterSubsystem.java | 32 +++++++ 3 files changed, 137 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e54b19f..3b2a1da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,9 +4,77 @@ package frc.robot; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N3; + public final class Constants { +public static final class DriveConstants { + // TODO: set motor and encoder constants + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 38; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 19; + public static final int kRearLeftTurningEncoderPort = 20; + public static final int kFrontRightTurningEncoderPort = 18; + public static final int kRearRightTurningEncoderPort = 17; + + public static final double kFrontLeftTurningEncoderOffset = 0; + public static final double kRearLeftTurningEncoderOffset = 0; + public static final double kFrontRightTurningEncoderOffset = 0; + public static final double kRearRightTurningEncoderOffset = 0; + + // TODO: Test motor orientations before driving on an actual robot + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kRearLeftDriveMotorReversed = false; + public static final boolean kFrontRightDriveMotorReversed = true; + public static final boolean kRearRightDriveMotorReversed = true; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth = 0.57785; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.57785; + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters = 0.1; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration + // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration + + // TODO: Tune this PID before running on a robot on the ground + public static final double kPModuleTurningController = -0.3; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxSpeedMetersPerSecond = 4.4196; + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; + // ^^ Calculated using the method taken from the old SDS github example + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime = 0.2; + // TODO: Tune this PID before running on a robot on the ground + public static final double kPHeadingCorrectionController = 5; + } //Intake PID and Encoder Constants public static class IntakeConstants { public static final double kIntakeDroppedAngle = 9.0; @@ -20,14 +88,38 @@ public static class IntakeConstants { public static final double kArmI = 0; public static final double kArmD = 0; public static final int kArmEncoderCh = 0; + public static final double IntakeDroppedAngle = 0; + public static final double IntakeRaisedAngle = 0; } //Shooter subsystem speed constants public static class ShooterConstants { public static final double kSpinSpeedTrue = 0.75; public static final double kSpinSpeedFalse = 0; + public static int kBottomShooterMotorPort; + public static int kTopShooterMotorPort; } + + public static final class VisionConstants { + // TODO: Update cam pose relative to center of bot + public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final double[] kLimelightCamPose = { + kCamPose.getX(), + kCamPose.getY(), + kCamPose.getZ(), + kCamPose.getRotation().getX(), + kCamPose.getRotation().getY(), + kCamPose.getRotation().getZ() }; + + // TODO: Experiment with different std devs in the pose estimator + public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + + // Field size in meters + public static final double kFieldWidth = 8.21055; + public static final double kFieldLength = 16.54175; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2a8fd9..72c7663 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.Constants.IntakeConstants; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -29,30 +30,33 @@ public class RobotContainer { private boolean lastAButton = false; private XboxController controller = new XboxController(0); + public RobotContainer() { // Configure the trigger bindings configureBindings(); - m_intakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem)); + m_intakeSubsystem.setDefaultCommand(new InstantCommand( + () -> m_intakeSubsystem.load(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis(), + IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle), + m_intakeSubsystem)); } - public void periodic(){ - if(controller.getAButton()){ + public void periodic() { + if (controller.getAButton()) { lastAButton = true; if (!lastAButton) IntakeDropped = !IntakeDropped; - } - else { + } else { lastAButton = false; } } private void configureBindings() { new JoystickButton(controller, Button.kB.value) - .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(0.75), m_ShooterSubsystem)) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem)); + .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); new JoystickButton(controller, Button.kA.value) - .onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-0.75), m_ShooterSubsystem)) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem)); + .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); } /** diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e69de29..a47de12 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; + +public class ShooterSubsystem extends SubsystemBase { + /** Creates a new ShooterSubsystem. */ + CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless); + CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless); + + public ShooterSubsystem() { + + } + + public void spin(double speed) { + m_bottom.set(speed); + m_top.set(speed); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Speed", m_bottom.); + } +} \ No newline at end of file From fd6d2877d45445d569995b19b246e4c21d2bcd24 Mon Sep 17 00:00:00 2001 From: Programming Date: Mon, 19 Feb 2024 19:20:18 -0800 Subject: [PATCH 6/9] fix/refactor: fixed intake and arm logic --- src/main/java/frc/robot/Constants.java | 173 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 21 ++- .../frc/robot/subsystems/IntakeSubsystem.java | 35 ++-- 3 files changed, 118 insertions(+), 111 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b2a1da..4fdc273 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,70 +14,72 @@ public final class Constants { -public static final class DriveConstants { - // TODO: set motor and encoder constants - public static final int kFrontLeftDriveMotorPort = 32; - public static final int kRearLeftDriveMotorPort = 29; - public static final int kFrontRightDriveMotorPort = 38; - public static final int kRearRightDriveMotorPort = 34; - - public static final int kFrontLeftTurningMotorPort = 28; - public static final int kRearLeftTurningMotorPort = 22; - public static final int kFrontRightTurningMotorPort = 37; - public static final int kRearRightTurningMotorPort = 26; - - public static final int kFrontLeftTurningEncoderPort = 19; - public static final int kRearLeftTurningEncoderPort = 20; - public static final int kFrontRightTurningEncoderPort = 18; - public static final int kRearRightTurningEncoderPort = 17; - - public static final double kFrontLeftTurningEncoderOffset = 0; - public static final double kRearLeftTurningEncoderOffset = 0; - public static final double kFrontRightTurningEncoderOffset = 0; - public static final double kRearRightTurningEncoderOffset = 0; - - // TODO: Test motor orientations before driving on an actual robot - public static final boolean kFrontLeftDriveMotorReversed = false; - public static final boolean kRearLeftDriveMotorReversed = false; - public static final boolean kFrontRightDriveMotorReversed = true; - public static final boolean kRearRightDriveMotorReversed = true; - - /** Distance between centers of right and left wheels on robot (in meters). */ - public static final double kTrackWidth = 0.57785; - - /** Distance between front and back wheels on robot (in meters). */ - public static final double kWheelBase = 0.57785; - - /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ - public static final double kWheelDiameterMeters = 0.1; - - /** Gear ratio between the motor and the wheel. */ - public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration - // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration - - // TODO: Tune this PID before running on a robot on the ground - public static final double kPModuleTurningController = -0.3; - - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); - - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxSpeedMetersPerSecond = 4.4196; - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; - // ^^ Calculated using the method taken from the old SDS github example - - /** Heading Correction */ - public static final double kHeadingCorrectionTurningStopTime = 0.2; - // TODO: Tune this PID before running on a robot on the ground - public static final double kPHeadingCorrectionController = 5; - } -//Intake PID and Encoder Constants + public static final class DriveConstants { + // TODO: set motor and encoder constants + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 38; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 19; + public static final int kRearLeftTurningEncoderPort = 20; + public static final int kFrontRightTurningEncoderPort = 18; + public static final int kRearRightTurningEncoderPort = 17; + + public static final double kFrontLeftTurningEncoderOffset = 0; + public static final double kRearLeftTurningEncoderOffset = 0; + public static final double kFrontRightTurningEncoderOffset = 0; + public static final double kRearRightTurningEncoderOffset = 0; + + // TODO: Test motor orientations before driving on an actual robot + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kRearLeftDriveMotorReversed = false; + public static final boolean kFrontRightDriveMotorReversed = true; + public static final boolean kRearRightDriveMotorReversed = true; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth = 0.57785; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.57785; + + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters = 0.1; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration + // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 + // configuration + + // TODO: Tune this PID before running on a robot on the ground + public static final double kPModuleTurningController = -0.3; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxSpeedMetersPerSecond = 4.4196; + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; + // ^^ Calculated using the method taken from the old SDS github example + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime = 0.2; + // TODO: Tune this PID before running on a robot on the ground + public static final double kPHeadingCorrectionController = 5; + } + + // Intake PID and Encoder Constants public static class IntakeConstants { - public static final double kIntakeDroppedAngle = 9.0; + public static final double kIntakeLoweredAngle = 9.0; public static final double kIntakeRaisedAngle = 9.0; public static final int kIntakeMotorID = 0; public static final int kArmMotorID = 0; @@ -88,38 +90,35 @@ public static class IntakeConstants { public static final double kArmI = 0; public static final double kArmD = 0; public static final int kArmEncoderCh = 0; - public static final double IntakeDroppedAngle = 0; - public static final double IntakeRaisedAngle = 0; + public static double kIntakeSpeed; } - //Shooter subsystem speed constants + // Shooter subsystem speed constants public static class ShooterConstants { public static final double kSpinSpeedTrue = 0.75; public static final double kSpinSpeedFalse = 0; public static int kBottomShooterMotorPort; public static int kTopShooterMotorPort; + } + public static final class VisionConstants { + // TODO: Update cam pose relative to center of bot + public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final double[] kLimelightCamPose = { + kCamPose.getX(), + kCamPose.getY(), + kCamPose.getZ(), + kCamPose.getRotation().getX(), + kCamPose.getRotation().getY(), + kCamPose.getRotation().getZ() }; + + // TODO: Experiment with different std devs in the pose estimator + public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + + // Field size in meters + public static final double kFieldWidth = 8.21055; + public static final double kFieldLength = 16.54175; } - - public static final class VisionConstants { - // TODO: Update cam pose relative to center of bot - public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final double[] kLimelightCamPose = { - kCamPose.getX(), - kCamPose.getY(), - kCamPose.getZ(), - kCamPose.getRotation().getX(), - kCamPose.getRotation().getY(), - kCamPose.getRotation().getZ() }; - - // TODO: Experiment with different std devs in the pose estimator - public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); - public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); - - // Field size in meters - public static final double kFieldWidth = 8.21055; - public static final double kFieldLength = 16.54175; - } } - diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 72c7663..3fe6a58 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,19 +29,18 @@ public class RobotContainer { private boolean IntakeDropped = false; private boolean lastAButton = false; - private XboxController controller = new XboxController(0); + private XboxController m_controller = new XboxController(0); public RobotContainer() { // Configure the trigger bindings configureBindings(); - m_intakeSubsystem.setDefaultCommand(new InstantCommand( - () -> m_intakeSubsystem.load(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis(), - IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle), - m_intakeSubsystem)); + m_intakeSubsystem.setDefaultCommand(m_controller.getLeftTriggerAxis() > 0.5 + ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem) + : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem)); } public void periodic() { - if (controller.getAButton()) { + if (m_controller.getAButton()) { lastAButton = true; if (!lastAButton) IntakeDropped = !IntakeDropped; @@ -51,10 +50,16 @@ public void periodic() { } private void configureBindings() { - new JoystickButton(controller, Button.kB.value) + new JoystickButton(m_controller, Button.kY.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), m_intakeSubsystem)) + .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + new JoystickButton(m_controller, Button.kX.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), m_intakeSubsystem)) + .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + new JoystickButton(m_controller, Button.kB.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(controller, Button.kA.value) + new JoystickButton(m_controller, Button.kA.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 794a666..ba7c052 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DutyCycleEncoder; - +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; @@ -17,25 +17,25 @@ public class IntakeSubsystem extends SubsystemBase { CANSparkFlex intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); CANSparkFlex armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - - PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP,IntakeConstants.kIntakeI,IntakeConstants.kIntakeD); - PIDController armPID = new PIDController(IntakeConstants.kArmP,IntakeConstants.kArmI,IntakeConstants.kArmD); + + PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, + IntakeConstants.kIntakeD); + PIDController armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); DutyCycleEncoder armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); - + /** Creates a new intake. */ public IntakeSubsystem() { - + } - /** - * - * @param speed motor power to apply to intake - *@param angle in radians - * - */ - public void load(double speed, double angle){ - intakeMotor.set(speed); - tiltToAngle(angle); + // Starts intaking the disk + public void intakeDisk() { + intakeMotor.set(IntakeConstants.kIntakeSpeed); + } + + // + public void stopIntaking() { + intakeMotor.set(0); } /** @@ -47,7 +47,10 @@ public void tiltToAngle(double angle) { double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); armMotor.set(motorPower); } - + public void stopRotating(){ + + } + @Override public void periodic() { // This method will be called once per scheduler run From 0965a64b523f888b2ded803145a5d14ced0c7ed6 Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Tue, 20 Feb 2024 16:13:00 -0800 Subject: [PATCH 7/9] chore: minor bugfixes and cleanup --- src/main/java/frc/robot/RobotContainer.java | 3 -- .../frc/robot/subsystems/IntakeSubsystem.java | 32 +++++++++++-------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3fe6a58..74426c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,14 +4,11 @@ package frc.robot; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.IntakeConstants; import frc.robot.subsystems.IntakeSubsystem; diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ba7c052..9d55b9c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -9,19 +9,20 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.IntakeConstants; -//creates new motors and pid controllers lmao + public class IntakeSubsystem extends SubsystemBase { - CANSparkFlex intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); - CANSparkFlex armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); + CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); + CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, + PIDController m_intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, IntakeConstants.kIntakeD); - PIDController armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); - DutyCycleEncoder armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); + PIDController m_armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); + + DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); /** Creates a new intake. */ public IntakeSubsystem() { @@ -30,29 +31,32 @@ public IntakeSubsystem() { // Starts intaking the disk public void intakeDisk() { - intakeMotor.set(IntakeConstants.kIntakeSpeed); + m_intakeMotor.set(IntakeConstants.kIntakeSpeed); } - // + //Stops rotating the intake public void stopIntaking() { - intakeMotor.set(0); + m_intakeMotor.set(0); } /** - * + * Rotates the arm to a given angle * @param angle motor to apply to intake * */ public void tiltToAngle(double angle) { - double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); - armMotor.set(motorPower); + m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition(), angle)); } + + //Stops rotating the arm public void stopRotating(){ - + m_armMotor.set(0); } @Override public void periodic() { + SmartDashboard.putBoolean("Intake",m_intakeMotor.get()>0); + // This method will be called once per scheduler run } } From f81d171a29b270fe23fbcd62785c37d89c14199f Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Tue, 20 Feb 2024 16:21:53 -0800 Subject: [PATCH 8/9] chore: fixed the merge --- src/main/java/frc/robot/Constants.java | 21 +++++++ src/main/java/frc/robot/RobotContainer.java | 63 ++++++++++++++++++--- 2 files changed, 75 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4fdc273..a963b8d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,7 +12,28 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the + * constants are needed, to reduce verbosity. + */ public final class Constants { + /** + * Input/Output constants + */ + public static final class IOConstants { + public static final int kDriverControllerPort = 0; + + public static final double kControllerDeadband = 0.2; + public static final double kSlowModeScalar = 0.8; + } public static final class DriveConstants { // TODO: set motor and encoder constants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 74426c7..535c73b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,18 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.IOConstants; +import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -21,23 +26,57 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { + + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); + private boolean IntakeDropped = false; private boolean lastAButton = false; - private XboxController m_controller = new XboxController(0); + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); public RobotContainer() { // Configure the trigger bindings configureBindings(); - m_intakeSubsystem.setDefaultCommand(m_controller.getLeftTriggerAxis() > 0.5 + m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5 ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem) : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem)); + + m_robotDrive.setDefaultCommand( + new RunCommand( + () -> m_robotDrive.drive( + MathUtil.applyDeadband( + -m_driverController.getLeftY(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + * 0.8, + MathUtil.applyDeadband( + -m_driverController.getLeftX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + * 0.8, + MathUtil.applyDeadband( + m_driverController.getRightX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxAngularSpeedRadiansPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + / 2, + !m_driverController.getRightBumper()), + m_robotDrive)); + } public void periodic() { - if (m_controller.getAButton()) { + if (m_driverController.getAButton()) { lastAButton = true; if (!lastAButton) IntakeDropped = !IntakeDropped; @@ -46,17 +85,23 @@ public void periodic() { } } + /** + * Use this method to define your button->command mappings. + */ private void configureBindings() { - new JoystickButton(m_controller, Button.kY.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), m_intakeSubsystem)) + // TODO: Move shoot commands to operator controller + new JoystickButton(m_driverController, Button.kY.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), + m_intakeSubsystem)) .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); - new JoystickButton(m_controller, Button.kX.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), m_intakeSubsystem)) + new JoystickButton(m_driverController, Button.kX.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), + m_intakeSubsystem)) .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); - new JoystickButton(m_controller, Button.kB.value) + new JoystickButton(m_driverController, Button.kB.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_controller, Button.kA.value) + new JoystickButton(m_driverController, Button.kA.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); } From 4e18f680c4bf7b286c852a1c2d591970add20f33 Mon Sep 17 00:00:00 2001 From: Programming SR <85456157+ProgrammingSR@users.noreply.github.com> Date: Tue, 20 Feb 2024 17:49:15 -0800 Subject: [PATCH 9/9] Revert "Intake" (#13) --- src/main/java/frc/robot/Constants.java | 215 ++++++++---------- src/main/java/frc/robot/RobotContainer.java | 38 +--- .../frc/robot/subsystems/IntakeSubsystem.java | 62 ----- .../robot/subsystems/ShooterSubsystem.java | 2 +- vendordeps/Phoenix6.json | 48 ++-- 5 files changed, 131 insertions(+), 234 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a963b8d..aa4bfd6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,121 +25,102 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - /** - * Input/Output constants - */ - public static final class IOConstants { - public static final int kDriverControllerPort = 0; - - public static final double kControllerDeadband = 0.2; - public static final double kSlowModeScalar = 0.8; - } - - public static final class DriveConstants { - // TODO: set motor and encoder constants - public static final int kFrontLeftDriveMotorPort = 32; - public static final int kRearLeftDriveMotorPort = 29; - public static final int kFrontRightDriveMotorPort = 38; - public static final int kRearRightDriveMotorPort = 34; - - public static final int kFrontLeftTurningMotorPort = 28; - public static final int kRearLeftTurningMotorPort = 22; - public static final int kFrontRightTurningMotorPort = 37; - public static final int kRearRightTurningMotorPort = 26; - - public static final int kFrontLeftTurningEncoderPort = 19; - public static final int kRearLeftTurningEncoderPort = 20; - public static final int kFrontRightTurningEncoderPort = 18; - public static final int kRearRightTurningEncoderPort = 17; - - public static final double kFrontLeftTurningEncoderOffset = 0; - public static final double kRearLeftTurningEncoderOffset = 0; - public static final double kFrontRightTurningEncoderOffset = 0; - public static final double kRearRightTurningEncoderOffset = 0; - - // TODO: Test motor orientations before driving on an actual robot - public static final boolean kFrontLeftDriveMotorReversed = false; - public static final boolean kRearLeftDriveMotorReversed = false; - public static final boolean kFrontRightDriveMotorReversed = true; - public static final boolean kRearRightDriveMotorReversed = true; - - /** Distance between centers of right and left wheels on robot (in meters). */ - public static final double kTrackWidth = 0.57785; - - /** Distance between front and back wheels on robot (in meters). */ - public static final double kWheelBase = 0.57785; - - /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ - public static final double kWheelDiameterMeters = 0.1; - - /** Gear ratio between the motor and the wheel. */ - public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration - // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 - // configuration - - // TODO: Tune this PID before running on a robot on the ground - public static final double kPModuleTurningController = -0.3; - - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); - - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxSpeedMetersPerSecond = 4.4196; - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; - // ^^ Calculated using the method taken from the old SDS github example - - /** Heading Correction */ - public static final double kHeadingCorrectionTurningStopTime = 0.2; - // TODO: Tune this PID before running on a robot on the ground - public static final double kPHeadingCorrectionController = 5; - } - - // Intake PID and Encoder Constants - public static class IntakeConstants { - public static final double kIntakeLoweredAngle = 9.0; - public static final double kIntakeRaisedAngle = 9.0; - public static final int kIntakeMotorID = 0; - public static final int kArmMotorID = 0; - public static final double kIntakeP = 0; - public static final double kIntakeI = 0; - public static final double kIntakeD = 0; - public static final double kArmP = 0; - public static final double kArmI = 0; - public static final double kArmD = 0; - public static final int kArmEncoderCh = 0; - public static double kIntakeSpeed; - } - - // Shooter subsystem speed constants - public static class ShooterConstants { - public static final double kSpinSpeedTrue = 0.75; - public static final double kSpinSpeedFalse = 0; - public static int kBottomShooterMotorPort; - public static int kTopShooterMotorPort; - - } - - public static final class VisionConstants { - // TODO: Update cam pose relative to center of bot - public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final double[] kLimelightCamPose = { - kCamPose.getX(), - kCamPose.getY(), - kCamPose.getZ(), - kCamPose.getRotation().getX(), - kCamPose.getRotation().getY(), - kCamPose.getRotation().getZ() }; - - // TODO: Experiment with different std devs in the pose estimator - public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); - public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); - - // Field size in meters - public static final double kFieldWidth = 8.21055; - public static final double kFieldLength = 16.54175; - } + + /** + * Input/Output constants + */ + public static final class IOConstants { + public static final int kDriverControllerPort = 0; + + public static final double kControllerDeadband = 0.2; + public static final double kSlowModeScalar = 0.8; + } + + public static final class DriveConstants { + // TODO: set motor and encoder constants + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 38; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 19; + public static final int kRearLeftTurningEncoderPort = 20; + public static final int kFrontRightTurningEncoderPort = 18; + public static final int kRearRightTurningEncoderPort = 17; + + public static final double kFrontLeftTurningEncoderOffset = 0; + public static final double kRearLeftTurningEncoderOffset = 0; + public static final double kFrontRightTurningEncoderOffset = 0; + public static final double kRearRightTurningEncoderOffset = 0; + + // TODO: Test motor orientations before driving on an actual robot + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kRearLeftDriveMotorReversed = false; + public static final boolean kFrontRightDriveMotorReversed = true; + public static final boolean kRearRightDriveMotorReversed = true; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth = 0.57785; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.57785; + + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters = 0.1; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration + // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration + + // TODO: Tune this PID before running on a robot on the ground + public static final double kPModuleTurningController = -0.3; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxSpeedMetersPerSecond = 4.4196; + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; + // ^^ Calculated using the method taken from the old SDS github example + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime = 0.2; + // TODO: Tune this PID before running on a robot on the ground + public static final double kPHeadingCorrectionController = 5; + } + + public static final class ShooterConstants { + public static final int kTopShooterMotorPort = 35; + public static final int kBottomShooterMotorPort = 20; + } + + public static final class VisionConstants { + // TODO: Update cam pose relative to center of bot + public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final double[] kLimelightCamPose = { + kCamPose.getX(), + kCamPose.getY(), + kCamPose.getZ(), + kCamPose.getRotation().getX(), + kCamPose.getRotation().getY(), + kCamPose.getRotation().getZ() }; + + // TODO: Experiment with different std devs in the pose estimator + public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + + // Field size in meters + public static final double kFieldWidth = 8.21055; + public static final double kFieldLength = 16.54175; + } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 535c73b..c19c224 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,11 +12,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; /* @@ -26,22 +24,18 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - + // The robot's subsystems and commands are defined here private final DriveSubsystem m_robotDrive = new DriveSubsystem(); public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); - public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - - private boolean IntakeDropped = false; - private boolean lastAButton = false; private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { // Configure the trigger bindings configureBindings(); - m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5 - ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem) - : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem)); m_robotDrive.setDefaultCommand( new RunCommand( @@ -72,32 +66,16 @@ public RobotContainer() { / 2, !m_driverController.getRightBumper()), m_robotDrive)); - - } - - public void periodic() { - if (m_driverController.getAButton()) { - lastAButton = true; - if (!lastAButton) - IntakeDropped = !IntakeDropped; - } else { - lastAButton = false; - } } /** * Use this method to define your button->command mappings. */ private void configureBindings() { + new JoystickButton(m_driverController, Button.kStart.value) + .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); + // TODO: Move shoot commands to operator controller - new JoystickButton(m_driverController, Button.kY.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), - m_intakeSubsystem)) - .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); - new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), - m_intakeSubsystem)) - .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); new JoystickButton(m_driverController, Button.kB.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); @@ -114,4 +92,4 @@ private void configureBindings() { public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 9d55b9c..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.IntakeConstants; - - -public class IntakeSubsystem extends SubsystemBase { - CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); - CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - - PIDController m_intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, - IntakeConstants.kIntakeD); - PIDController m_armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); - - DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); - - /** Creates a new intake. */ - public IntakeSubsystem() { - - } - - // Starts intaking the disk - public void intakeDisk() { - m_intakeMotor.set(IntakeConstants.kIntakeSpeed); - } - - //Stops rotating the intake - public void stopIntaking() { - m_intakeMotor.set(0); - } - - /** - * Rotates the arm to a given angle - * @param angle motor to apply to intake - * - */ - public void tiltToAngle(double angle) { - m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition(), angle)); - } - - //Stops rotating the arm - public void stopRotating(){ - m_armMotor.set(0); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Intake",m_intakeMotor.get()>0); - - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a47de12..afdbd19 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -29,4 +29,4 @@ public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("Speed", m_bottom.); } -} \ No newline at end of file +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d172..69a4079 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,