From 11f9b0cba86f82f040275490c04de001217f9e06 Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgrammer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Sat, 24 Feb 2024 16:08:02 -0800 Subject: [PATCH 1/8] Update Dump Control, sushi speeds --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/DumpControl.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e8062c..6fde1d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -77,7 +77,7 @@ public static class MechanismConstants { public static final boolean INTAKE_MOTOR_SUSHI_INVERTED = true; public static final boolean INTAKE_MOTOR_FRONT_INVERTED = false; public static final double INTAKE_MOTOR_SPEED_FRONT = 0.8; - public static final double INTAKE_MOTOR_SPEED_SUSHI = 0.8; + public static final double INTAKE_MOTOR_SPEED_SUSHI = 1; // Conveyor Motors public static final int CONVEYOR_MOTOR_1 = 1; diff --git a/src/main/java/frc/robot/commands/DumpControl.java b/src/main/java/frc/robot/commands/DumpControl.java index e801fb8..2bd1d0f 100644 --- a/src/main/java/frc/robot/commands/DumpControl.java +++ b/src/main/java/frc/robot/commands/DumpControl.java @@ -31,7 +31,7 @@ public void initialize() {} public void execute() { // TODO: Change this to the correct button if (DumpJoystick.getRawButton(1)) { - DumpSubsystem.openThenClose(); + DumpSubsystem.open(); // TODO: Change this to the correct button } else if (DumpJoystick.getRawButton(2)) { DumpSubsystem.close(); From b0296443422a2cf7935df53992329b10fc99e7b7 Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgrammer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Sun, 25 Feb 2024 16:50:19 -0800 Subject: [PATCH 2/8] Update Constants.java --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e8062c..9e75355 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -76,14 +76,14 @@ public static class MechanismConstants { public static final int INTAKE_MOTOR_FRONT = 52; public static final boolean INTAKE_MOTOR_SUSHI_INVERTED = true; public static final boolean INTAKE_MOTOR_FRONT_INVERTED = false; - public static final double INTAKE_MOTOR_SPEED_FRONT = 0.8; + public static final double INTAKE_MOTOR_SPEED_FRONT = 0.2; public static final double INTAKE_MOTOR_SPEED_SUSHI = 0.8; // Conveyor Motors public static final int CONVEYOR_MOTOR_1 = 1; public static final int CONVEYOR_MOTOR_2 = 2; public static final boolean CONVEYOR_MOTOR_1_INVERTED = true; - public static final boolean CONVEYOR_MOTOR_2_INVERTED = true; - public static final double CONVEYOR_MOTOR_SPEED = 0.30; + public static final boolean CONVEYOR_MOTOR_2_INVERTED = false; + public static final double CONVEYOR_MOTOR_SPEED = 0.40; } } From f2873ee993a3628a5bea6d5749edabac46b8998e Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgrammer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Sun, 25 Feb 2024 19:50:41 -0800 Subject: [PATCH 3/8] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e75355..35ebf46 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -84,6 +84,6 @@ public static class MechanismConstants { public static final int CONVEYOR_MOTOR_2 = 2; public static final boolean CONVEYOR_MOTOR_1_INVERTED = true; public static final boolean CONVEYOR_MOTOR_2_INVERTED = false; - public static final double CONVEYOR_MOTOR_SPEED = 0.40; + public static final double CONVEYOR_MOTOR_SPEED = 0.38; } } From 71f19f5a21c7cb85fbadeca0408d28c241db3588 Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgrammer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Wed, 28 Feb 2024 19:22:01 -0800 Subject: [PATCH 4/8] make swerve not chatter make swerve not chatter --- src/main/deploy/swerve/controllerproperties.json | 4 ++-- src/main/deploy/swerve/modules/backleft.json | 4 ++-- src/main/deploy/swerve/modules/backright.json | 4 ++-- src/main/deploy/swerve/modules/frontleft.json | 4 ++-- src/main/deploy/swerve/modules/frontright.json | 4 ++-- src/main/deploy/swerve/modules/physicalproperties.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json index ab93140..4abb93a 100644 --- a/src/main/deploy/swerve/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { - "angleJoystickRadiusDeadband": 0.25, + "angleJoystickRadiusDeadband": 0.05, "heading": { "p": 0.4, "i": 0, - "d": 0.01 + "d": 0 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index a22a195..187a1ca 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -1,7 +1,7 @@ { "location": { - "front": -13.5, - "left": 13.5 + "front": -11, + "left": 11 }, "absoluteEncoderOffset": 276.15234375, "drive": { diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 65decfd..b93f795 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -1,7 +1,7 @@ { "location": { - "front": -13.5, - "left": -13.5 + "front": -11, + "left": -11 }, "absoluteEncoderOffset": 299.267578125, "drive": { diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 123e6a1..b1b7e96 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -1,7 +1,7 @@ { "location": { - "front": 13.5, - "left": 13.5 + "front": 11, + "left": 11 }, "absoluteEncoderOffset": 357.978515625, "drive": { diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 16efc78..1b99eab 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -1,7 +1,7 @@ { "location": { - "front": 13.5, - "left": -13.5 + "front": 11, + "left": -11 }, "absoluteEncoderOffset": 52.91015625, "drive": { diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index dfb4be5..b2d3972 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -11,6 +11,6 @@ }, "rampRate": { "drive": 0.25, - "angle": 0.25 + "angle": 0 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index efc0c2b..fbdb906 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,7 +61,7 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - AbsoluteDriveAdv closedAbsoluteDriveAdv = + final AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv( drivebase, () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), @@ -94,7 +94,7 @@ public RobotContainer() { drivebase.driveCommand( () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRawAxis(2)); + () -> driverXbox.getRightX() * 0.5); Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand( @@ -102,7 +102,7 @@ public RobotContainer() { () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); - drivebase.setDefaultCommand(closedAbsoluteDriveAdv); + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } /** From c34ade460fbca566f727645f46d4292cb20903ac Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Thu, 29 Feb 2024 23:10:39 -0600 Subject: [PATCH 5/8] Update swerve controls Updated swerve controls to use field oriented with directed angle. --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbdb906..efd4b48 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,7 +102,7 @@ public RobotContainer() { () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + drivebase.setDefaultCommand(driveFieldOrientedDirectAngle); } /** From 82c0640d41e4ecc9c98ecee6ca2e471e835eecf8 Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgrammer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Sun, 3 Mar 2024 19:12:39 -0600 Subject: [PATCH 6/8] Revert IMU back to normal operations --- src/main/deploy/swerve/swervedrive.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index dce981d..da97274 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", From 32b7e6d6465970c4acfb0ed2e5db7b35668918b5 Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgramer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Tue, 5 Mar 2024 17:03:28 -0600 Subject: [PATCH 7/8] Add flywheels --- src/main/java/frc/robot/Constants.java | 7 +++ src/main/java/frc/robot/RobotContainer.java | 4 ++ .../java/frc/robot/commands/FlyWCommand.java | 49 +++++++++++++++++++ .../java/frc/robot/subsystems/FlyWheel.java | 47 ++++++++++++++++++ 4 files changed, 107 insertions(+) create mode 100644 src/main/java/frc/robot/commands/FlyWCommand.java create mode 100644 src/main/java/frc/robot/subsystems/FlyWheel.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 35ebf46..75869bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -85,5 +85,12 @@ public static class MechanismConstants { public static final boolean CONVEYOR_MOTOR_1_INVERTED = true; public static final boolean CONVEYOR_MOTOR_2_INVERTED = false; public static final double CONVEYOR_MOTOR_SPEED = 0.38; + + // Flywheel Motors + public static final int FLYWHEEL_MOTOR_1 = 3; + public static final int FLYWHEEL_MOTOR_2 = 4; + public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; + public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; + public static final double FLYWHEEL_MOTOR_SPEED = 0.8; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index efd4b48..63d50f6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,10 +20,12 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.ConveyorCommand; import frc.robot.commands.DumpControl; +import frc.robot.commands.FlyWCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.Conveyor; import frc.robot.subsystems.Dump; +import frc.robot.subsystems.FlyWheel; import frc.robot.subsystems.Intake; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -50,6 +52,7 @@ public class RobotContainer { private final Intake intake = new Intake(); private final Dump dump = new Dump(); private final Conveyor conveyor = new Conveyor(); + private final FlyWheel flyWheel = new FlyWheel(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -57,6 +60,7 @@ public RobotContainer() { intake.setDefaultCommand(new IntakeCommand(intake)); dump.setDefaultCommand(new DumpControl(dump)); conveyor.setDefaultCommand(new ConveyorCommand(conveyor)); + flyWheel.setDefaultCommand(new FlyWCommand(flyWheel)); // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/commands/FlyWCommand.java b/src/main/java/frc/robot/commands/FlyWCommand.java new file mode 100644 index 0000000..7dde141 --- /dev/null +++ b/src/main/java/frc/robot/commands/FlyWCommand.java @@ -0,0 +1,49 @@ +// 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.commands; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.FlyWheel; + +public class FlyWCommand extends Command { + private final FlyWheel m_FlyWSubsystem; + // controller + // TODO: Change this to the correct controller + private final Joystick ConveyorJoystick = new Joystick(1); + + public FlyWCommand(FlyWheel subsystem) { + m_FlyWSubsystem = subsystem; + addRequirements(m_FlyWSubsystem); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + // TODO: Change this to the correct button + if (ConveyorJoystick.getRawButton(3) == true) { + m_FlyWSubsystem.enableflywheel(); + // TODO: Change this to the correct button + } else if (ConveyorJoystick.getRawButton(4) == true) { + m_FlyWSubsystem.reverseflywheel(); + System.out.println("Conveyor Moving in Reverse"); + } else { + m_FlyWSubsystem.disableflywheel(); + } + } + + @Override + public void end(boolean interrupted) { + m_FlyWSubsystem.disableflywheel(); + m_FlyWSubsystem.enableflywheel(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java new file mode 100644 index 0000000..2ee2530 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -0,0 +1,47 @@ +// 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.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.MechanismConstants; + +public class FlyWheel extends SubsystemBase { + + private final CANSparkMax flywheelMotor1 = + new CANSparkMax(MechanismConstants.FLYWHEEL_MOTOR_1, CANSparkLowLevel.MotorType.kBrushless); + private final CANSparkMax flywheelMotor2 = + new CANSparkMax(MechanismConstants.FLYWHEEL_MOTOR_2, CANSparkLowLevel.MotorType.kBrushless); + + /** Creates a new flywheel. */ + public FlyWheel() { + flywheelMotor1.setInverted(MechanismConstants.FLYWHEEL_MOTOR_1_INVERTED); + flywheelMotor2.setInverted(MechanismConstants.FLYWHEEL_MOTOR_2_INVERTED); + } + + /** Sets the flywheel motors to 100% power. */ + public void enableflywheel() { + flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED); + flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED); + } + + /** Sets the flywheel motors to 0% power. */ + public void disableflywheel() { + flywheelMotor1.set(0); + flywheelMotor2.set(0); + } + + /** Sets the flywheel motors to -100% power. (Reverse direction) */ + public void reverseflywheel() { + flywheelMotor1.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED); + flywheelMotor2.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 624c997981376a265ce6fea8348657213cccd33d Mon Sep 17 00:00:00 2001 From: OakvilleDynamicsProgramer <159196208+OakvilleDynamicsProgrammer@users.noreply.github.com> Date: Tue, 5 Mar 2024 19:04:06 -0600 Subject: [PATCH 8/8] Update flywheel speeds --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/commands/FlyWCommand.java | 9 +++++---- src/main/java/frc/robot/subsystems/FlyWheel.java | 12 +++++++++--- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 75869bb..c9984f1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -91,6 +91,7 @@ public static class MechanismConstants { public static final int FLYWHEEL_MOTOR_2 = 4; public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; - public static final double FLYWHEEL_MOTOR_SPEED = 0.8; + public static final double FLYWHEEL_MOTOR_SPEED = 1.0; + public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; } } diff --git a/src/main/java/frc/robot/commands/FlyWCommand.java b/src/main/java/frc/robot/commands/FlyWCommand.java index 7dde141..a2e1d0e 100644 --- a/src/main/java/frc/robot/commands/FlyWCommand.java +++ b/src/main/java/frc/robot/commands/FlyWCommand.java @@ -25,12 +25,14 @@ public void initialize() {} @Override public void execute() { // TODO: Change this to the correct button - if (ConveyorJoystick.getRawButton(3) == true) { - m_FlyWSubsystem.enableflywheel(); + if (ConveyorJoystick.getRawButton(7) == true) { + m_FlyWSubsystem.enableflywheelfast(); // TODO: Change this to the correct button - } else if (ConveyorJoystick.getRawButton(4) == true) { + } else if (ConveyorJoystick.getRawButton(8) == true) { m_FlyWSubsystem.reverseflywheel(); System.out.println("Conveyor Moving in Reverse"); + } else if (ConveyorJoystick.getRawButton(9) == true) { + m_FlyWSubsystem.enableflywheelslow(); } else { m_FlyWSubsystem.disableflywheel(); } @@ -39,7 +41,6 @@ public void execute() { @Override public void end(boolean interrupted) { m_FlyWSubsystem.disableflywheel(); - m_FlyWSubsystem.enableflywheel(); } @Override diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 2ee2530..df05dae 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -23,11 +23,17 @@ public FlyWheel() { } /** Sets the flywheel motors to 100% power. */ - public void enableflywheel() { + public void enableflywheelfast() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED); } + /** Sets the flywheel motors to slow speed */ + public void enableflywheelslow() { + flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW); + flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW); + } + /** Sets the flywheel motors to 0% power. */ public void disableflywheel() { flywheelMotor1.set(0); @@ -36,8 +42,8 @@ public void disableflywheel() { /** Sets the flywheel motors to -100% power. (Reverse direction) */ public void reverseflywheel() { - flywheelMotor1.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED); - flywheelMotor2.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED); + flywheelMotor1.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW); + flywheelMotor2.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW); } @Override