From a65ae8cf126a5959122618f1a317b2d8f4c64d21 Mon Sep 17 00:00:00 2001 From: Computer 0 <48131223+TheGamer1002@users.noreply.github.com> Date: Tue, 28 Mar 2023 08:05:44 -0400 Subject: [PATCH] wip --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 56 +++++++++-------- .../java/frc/robot/commands/AutoCommand.java | 4 +- .../java/frc/robot/commands/DriveCommand.java | 2 +- .../robot/commands/InvertDriveCommand.java | 56 +++++++++++++++++ .../frc/robot/commands/LineupCommand.java | 40 ------------ .../frc/robot/commands/MoveArmYCommand.java | 2 +- ...eClawCommand.java => MoveClawCommand.java} | 13 ++-- .../frc/robot/commands/OpenClawCommand.java | 38 ------------ src/main/java/frc/robot/subsystems/Claw.java | 7 ++- .../java/frc/robot/subsystems/LineupPID.java | 62 ------------------- src/main/java/frc/robot/subsystems/Tank.java | 8 +-- 12 files changed, 107 insertions(+), 183 deletions(-) create mode 100644 src/main/java/frc/robot/commands/InvertDriveCommand.java delete mode 100644 src/main/java/frc/robot/commands/LineupCommand.java rename src/main/java/frc/robot/commands/{CloseClawCommand.java => MoveClawCommand.java} (65%) delete mode 100644 src/main/java/frc/robot/commands/OpenClawCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/LineupPID.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c7d84d3..7290622 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,7 +31,7 @@ public static final class CanConstants { public static final int kLeftRearMotorPort = 2; public static final int kRightFrontMotorPort = 4; public static final int kRightRearMotorPort = 5; - public static final double kRampRate = 0; + public static final double kRampRate = 0.1; public static double maxSpeed = 1; public static final double maxSpeedIncrement = 0.05; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index faa1d0a..59852fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,27 +21,28 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { + public static boolean inverted = false; + public static final Joystick m_armController = new Joystick(0); public static final XboxController m_armController2 = new XboxController(1); public static final XboxController m_driverController = new XboxController(2); // INSTANTIATES ALL SUBSYSTEMS private final Arm m_Arm = new Arm(); - private final Claw m_claw = new Claw(); + private final Claw m_Claw = new Claw(); private final Tank m_Tank = new Tank(); private final PID m_PID = new PID(); private final PDP m_PDP = new PDP(); - private final LineupPID m_LineupPID = new LineupPID(); + private final Vision m_Vision = new Vision(); // INSTANTIATES ALL COMMANDS - private final OpenClawCommand m_OpenClawCommand = new OpenClawCommand(m_claw); - private final CloseClawCommand m_CloseClawCommand = new CloseClawCommand(m_claw); - + private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw); private final MoveArmYCommand m_MoveArmYCommand = new MoveArmYCommand(m_Arm); private final DriveCommand m_DriveCommand = new DriveCommand(m_Tank); private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_PID, m_Tank); - private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_claw, m_Arm); + private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_Claw, m_Arm); + private final InvertDriveCommand m_InvertDriveCommand = new InvertDriveCommand(m_Tank, this); private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand = new PlaceConeSecondLevelCommand(m_Tank, m_Arm); private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand = @@ -49,7 +50,7 @@ public class RobotContainer { private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand = new DecreaseMaxSpeedCommand(m_Tank); private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank); - private final LineupCommand m_LineupCommand = new LineupCommand(m_LineupPID); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -63,22 +64,20 @@ public RobotContainer() { tab.add(m_Arm); tab.add(m_AutoCommand); tab.add(m_BalanceCommand); - tab.add(m_CloseClawCommand); tab.add(m_DecreaseMaxSpeedCommand); tab.add(m_DriveCommand); tab.add(m_FineDriveCommand); tab.add(m_IncreaseMaxSpeedCommand); tab.add(m_MoveArmYCommand); tab.add(m_PDP); - tab.add(m_OpenClawCommand); - tab.add(m_LineupPID); - tab.add(m_LineupCommand); + + tab.add(m_PID); tab.add(m_PlaceConeSecondLevelCommand); tab.add(m_Tank); - tab.add(m_claw); + tab.add(m_Claw); Logger.configureLoggingAndConfig(this, false); } @@ -95,13 +94,13 @@ private void configureButtonBindings() { .whileTrue(m_IncreaseMaxSpeedCommand); new JoystickButton(m_driverController, XboxController.Button.kBack.value) .whileTrue(m_DecreaseMaxSpeedCommand); - new JoystickButton(m_driverController, XboxController.Button.kA.value) - .whileTrue(m_LineupCommand); + new JoystickButton(m_driverController, XboxController.Button.kX.value) + .toggleOnTrue(m_InvertDriveCommand); - new JoystickButton(m_armController2, XboxController.Button.kX.value) - .whileTrue(m_CloseClawCommand); - new JoystickButton(m_armController2, XboxController.Button.kY.value) - .whileTrue(m_OpenClawCommand); + // new JoystickButton(m_armController2, XboxController.Button.kX.value) + // .whileTrue(m_CloseClawCommand); + // new JoystickButton(m_armController2, XboxController.Button.kY.value) + // .whileTrue(m_OpenClawCommand); new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value) .whileTrue(m_FineDriveCommand); new JoystickButton(m_armController2, XboxController.Button.kB.value) @@ -183,17 +182,17 @@ public static double getControllerRightStickX() { return axis; } - public static double getJoystickArmY() { - double axis = m_armController.getRawAxis(1); - if (Math.abs(axis) < 0.15) { + public static double getControllerRightTrigger() { + double axis = m_armController2.getRightTriggerAxis(); + if (Math.abs(axis) < 0.01) { axis = 0; } return axis; } - public static double getJoystickArmTwist() { - double axis = m_armController.getRawAxis(2); - if (Math.abs(axis) < 0.15) { + public static double getControllerLeftTrigger() { + double axis = m_armController2.getLeftTriggerAxis(); + if (Math.abs(axis) < 0.01) { axis = 0; } return axis; @@ -205,10 +204,13 @@ public Command getAutonomousCommand() { } public void initTeleop() { - // Set the default tank command to DriveCommand - m_Tank.setDefaultCommand(m_DriveCommand); - + if(inverted){ + m_Tank.setDefaultCommand(m_InvertDriveCommand); + } else{ + m_Tank.setDefaultCommand(m_DriveCommand); + } m_Arm.setDefaultCommand(m_MoveArmYCommand); + m_Claw.setDefaultCommand(m_MoveClawCommand); } public void initTest() { diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java index a74b202..683d1e1 100644 --- a/src/main/java/frc/robot/commands/AutoCommand.java +++ b/src/main/java/frc/robot/commands/AutoCommand.java @@ -9,9 +9,9 @@ public class AutoCommand extends SequentialCommandGroup { public AutoCommand(PID m_pid, Tank m_driveBase, Claw m_claw, Arm m_arm) { - addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm)); + addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm), - // new BalanceCommand(m_pid, m_driveBase); + new BalanceCommand(m_pid, m_driveBase)); // new PlaceCubeFirstLevelCommand(m_driveBase, m_claw, m_arm) // new PlaceConeSecondLevelCommand(m_driveBase, m_arm) } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 15e59ee..4fd60e8 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -29,7 +29,7 @@ public void execute() { // double check getMaxSpeed(), might be wrong Tank.arcadeDrive( RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed, - RobotContainer.getDriverControllerRightStickXAdjusted() * Constants.CanConstants.maxSpeed); + RobotContainer.getDriverControllerRightStickXAdjusted() * 0.75); SmartDashboard.putNumber("maxSpeed", Constants.CanConstants.maxSpeed); } diff --git a/src/main/java/frc/robot/commands/InvertDriveCommand.java b/src/main/java/frc/robot/commands/InvertDriveCommand.java new file mode 100644 index 0000000..2f076b6 --- /dev/null +++ b/src/main/java/frc/robot/commands/InvertDriveCommand.java @@ -0,0 +1,56 @@ +// 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.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Tank; + +public class InvertDriveCommand extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Tank m_drivebase; + private final RobotContainer robot; + + public InvertDriveCommand(Tank subsystem, RobotContainer robot) { + m_drivebase = subsystem; + this.robot = robot; + + addRequirements(m_drivebase); + } + + @Override + public void initialize() { + RobotContainer.inverted = true; + robot.initTeleop(); + } + + @Override + public void execute() { + // double check getMaxSpeed(), might be wrong + m_drivebase.arcadeDrive( + -RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed, + RobotContainer.getDriverControllerRightStickXAdjusted() * 0.75); + } + + @Override + public void end(boolean interrupted) { + m_drivebase.arcadeDrive(0, 0); + RobotContainer.inverted = false; + robot.initTeleop(); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public boolean runsWhenDisabled() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/LineupCommand.java b/src/main/java/frc/robot/commands/LineupCommand.java deleted file mode 100644 index 7d83749..0000000 --- a/src/main/java/frc/robot/commands/LineupCommand.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.LineupPID; -import frc.robot.subsystems.Tank; - -public class LineupCommand extends CommandBase { - private LineupPID m_lineupPID; - private double m_output; - - public LineupCommand(LineupPID lineupPID) { - m_lineupPID = lineupPID; - addRequirements(m_lineupPID); - } - - @Override - public void initialize() { - m_lineupPID.setSetpoint(); - } - - @Override - public void execute() { - m_output = m_lineupPID.m_output; - Tank.arcadeDrive(0, m_output); - } - - @Override - public void end(boolean interrupted) { - m_lineupPID.disable(); - Tank.arcadeDrive(0, 0); - } - - @Override - public boolean isFinished() { - // Return true when the command should end. - - // if the PID controller is at the setpoint, return true - return m_lineupPID.atSetpoint(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/MoveArmYCommand.java b/src/main/java/frc/robot/commands/MoveArmYCommand.java index b1d6b7f..cf5c3a4 100644 --- a/src/main/java/frc/robot/commands/MoveArmYCommand.java +++ b/src/main/java/frc/robot/commands/MoveArmYCommand.java @@ -22,7 +22,7 @@ public void initialize() {} @Override public void execute() { - m_subsystem.moveArm(RobotContainer.getControllerLeftStickY() * 1.5); + m_subsystem.moveArm(RobotContainer.getControllerLeftStickY()); } @Override diff --git a/src/main/java/frc/robot/commands/CloseClawCommand.java b/src/main/java/frc/robot/commands/MoveClawCommand.java similarity index 65% rename from src/main/java/frc/robot/commands/CloseClawCommand.java rename to src/main/java/frc/robot/commands/MoveClawCommand.java index a5ad783..b2a1797 100644 --- a/src/main/java/frc/robot/commands/CloseClawCommand.java +++ b/src/main/java/frc/robot/commands/MoveClawCommand.java @@ -5,13 +5,14 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; import frc.robot.subsystems.Claw; -public class CloseClawCommand extends CommandBase { +public class MoveClawCommand extends CommandBase { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final Claw m_subsystem; - public CloseClawCommand(Claw subsystem) { + public MoveClawCommand(Claw subsystem) { m_subsystem = subsystem; addRequirements(subsystem); } @@ -21,9 +22,11 @@ public void initialize() {} @Override public void execute() { - // check if the power draw for the motor is over 2 amps -- if so, end the command - // if (m_subsystem.get) - m_subsystem.closeClaw(); + if(Math.abs(RobotContainer.getControllerLeftTrigger()) < 0.2){ + m_subsystem.clawMotor.set(RobotContainer.getControllerRightTrigger() * 0.7); + } else{ + m_subsystem.clawMotor.set(-RobotContainer.getControllerLeftTrigger() * 0.7); + } } @Override diff --git a/src/main/java/frc/robot/commands/OpenClawCommand.java b/src/main/java/frc/robot/commands/OpenClawCommand.java deleted file mode 100644 index 05f177d..0000000 --- a/src/main/java/frc/robot/commands/OpenClawCommand.java +++ /dev/null @@ -1,38 +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.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Claw; - -public class OpenClawCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final Claw m_subsystem; - - public OpenClawCommand(Claw subsystem) { - m_subsystem = subsystem; - addRequirements(subsystem); - } - - @Override - public void initialize() { - // no initialization needed - } - - @Override - public void execute() { - m_subsystem.openClaw(); - } - - @Override - public void end(boolean interrupted) { - m_subsystem.stopClaw(); - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 1326f2f..6c7d2e9 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -7,11 +7,14 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; import frc.robot.Interfaces.*; import java.util.Map; @@ -27,7 +30,7 @@ public class Claw extends SubsystemBase { public Claw() { clawMotor = new CANSparkMax(8, MotorType.kBrushless); clawMotor.setIdleMode(IdleMode.kBrake); - clawMotor.setSmartCurrentLimit(10); + clawMotor.setSmartCurrentLimit(15); clawMotor.burnFlash(); addChild("Claw Motor", clawMotor); @@ -51,7 +54,7 @@ public Claw() { @Override public void periodic() { - + SmartDashboard.putNumber("rightTrigger", RobotContainer.getControllerRightTrigger()); } public void closeClaw() { diff --git a/src/main/java/frc/robot/subsystems/LineupPID.java b/src/main/java/frc/robot/subsystems/LineupPID.java deleted file mode 100644 index 13b2200..0000000 --- a/src/main/java/frc/robot/subsystems/LineupPID.java +++ /dev/null @@ -1,62 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; -import frc.robot.subsystems.Vision; -import frc.robot.subsystems.Tank; - -public class LineupPID extends PIDSubsystem { - private static double kP = 0.0; - private static double kI = 0.0; - private static double kD = 0.0; - private PIDController m_controller; - public double m_output; - public double m_setpoint; - - public LineupPID() { - super(new PIDController(kP, kI, kD)); - addChild("Lineup Controller", getController()); - // add that sendable to the shuffleboard - Shuffleboard.getTab("Subsystems").add(getController()); - m_controller = getController(); - - // create a new NetworkTable table for all the PID stuff - NetworkTable table = NetworkTableInstance.getDefault().getTable("lineupPID"); - - // add the PID values to the table - table.getEntry("kP").setDouble(kP); - table.getEntry("kI").setDouble(kI); - table.getEntry("kD").setDouble(kD); - - } - - @Override - public void useOutput(double output, double setpoint) { - // Use the output here - m_output = output; - m_setpoint = setpoint; - } - - @Override - public double getMeasurement() { - // Return the distance from the best retroreflective tape pole (determined by the Limelight itself, configured in the Limelight's web interface) to the center of the robot - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0); - - - - } - - // Set the PID controller's setpoint to the distance from the best retroreflective tape pole (determined by the Limelight itself, configured in the Limelight's web interface) to the center of the robot - public void setSetpoint() { - m_controller.setSetpoint(getMeasurement()); - } - - // check if the PID controller is at the setpoint; if it is, return true - public boolean atSetpoint() { - return m_controller.atSetpoint(); - } - -} diff --git a/src/main/java/frc/robot/subsystems/Tank.java b/src/main/java/frc/robot/subsystems/Tank.java index ba5183c..8ae0b84 100644 --- a/src/main/java/frc/robot/subsystems/Tank.java +++ b/src/main/java/frc/robot/subsystems/Tank.java @@ -150,10 +150,10 @@ public Tank() { maxSpeed = Constants.CanConstants.maxSpeed; // set the current limits for all four motors - leftFront.setSmartCurrentLimit(40, 60); - leftRear.setSmartCurrentLimit(40, 60); - rightFront.setSmartCurrentLimit(40, 60); - rightRear.setSmartCurrentLimit(40, 60); + leftFront.setSmartCurrentLimit(40, 50); + leftRear.setSmartCurrentLimit(40, 50); + rightFront.setSmartCurrentLimit(40, 50); + rightRear.setSmartCurrentLimit(40, 50); tankLayout = Shuffleboard.getTab("Config") .getLayout("Tank", BuiltInLayouts.kList)