From f5b490ec07a7ce8ce3d5bfaa193f57d8b93a3b80 Mon Sep 17 00:00:00 2001 From: Capyblapy <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 15 Oct 2024 18:13:00 -0500 Subject: [PATCH 1/9] Refactored Autonoumus Drive Code Merged all of the timed drive forward code into one command with a time variable. --- .../src/main/java/frc/robot/Constants.java | 2 +- .../main/java/frc/robot/RobotContainer.java | 13 ++--- .../robot/commands/AutonomousDrive4Sec.java | 56 ------------------- .../robot/commands/AutonomousDrive6Sec.java | 56 ------------------- ...ive2Sec.java => AutonomousDriveTimed.java} | 14 ++++- .../robot/commands/AutonomousPassLine.java | 56 ------------------- 6 files changed, 17 insertions(+), 180 deletions(-) delete mode 100644 CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive4Sec.java delete mode 100644 CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive6Sec.java rename CompetitionSoftware/src/main/java/frc/robot/commands/{AutonomousDrive2Sec.java => AutonomousDriveTimed.java} (73%) delete mode 100644 CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousPassLine.java diff --git a/CompetitionSoftware/src/main/java/frc/robot/Constants.java b/CompetitionSoftware/src/main/java/frc/robot/Constants.java index 8f82446..b578e63 100644 --- a/CompetitionSoftware/src/main/java/frc/robot/Constants.java +++ b/CompetitionSoftware/src/main/java/frc/robot/Constants.java @@ -24,7 +24,7 @@ public static class RobotConstants { } public static class AutoConstants { - public static final long kPassLineDuration_mS = 2000; + public static final int kPassLineDuration_mS = 2000; public static final double kPassLineSpeed = 0.40; } diff --git a/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java b/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java index 4b2607e..18730b3 100644 --- a/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java +++ b/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,7 @@ package frc.robot; import frc.robot.Constants.*; -import frc.robot.commands.AutonomousDrive2Sec; -import frc.robot.commands.AutonomousDrive4Sec; -import frc.robot.commands.AutonomousDrive6Sec; -import frc.robot.commands.AutonomousPassLine; +import frc.robot.commands.AutonomousDriveTimed; import frc.robot.commands.ClimbCommand; import frc.robot.commands.NoteHandlingSpeedCommand; import frc.robot.commands.RotateCommand; @@ -122,10 +119,10 @@ public void reportStatus() { private void configureDriverStationControls() { // Drop-down chooser for auto program. - m_autoChooser.setDefaultOption("Pass Auto Line", new AutonomousPassLine(m_driveSubsystem)); - m_autoChooser.addOption("Drive forward 2 Seconds", new AutonomousDrive2Sec(m_driveSubsystem)); - m_autoChooser.addOption("Drive forward 4 Seconds", new AutonomousDrive4Sec(m_driveSubsystem)); - m_autoChooser.addOption("Drive forward 6 Seconds", new AutonomousDrive6Sec(m_driveSubsystem)); + m_autoChooser.setDefaultOption("Pass Auto Line", new AutonomousDriveTimed(m_driveSubsystem, AutoConstants.kPassLineDuration_mS)); + m_autoChooser.addOption("Drive forward 2 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 2000)); + m_autoChooser.addOption("Drive forward 4 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 4000)); + m_autoChooser.addOption("Drive forward 6 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 6000)); SmartDashboard.putData(m_autoChooser); } diff --git a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive4Sec.java b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive4Sec.java deleted file mode 100644 index 05c0f80..0000000 --- a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive4Sec.java +++ /dev/null @@ -1,56 +0,0 @@ -// This is the default auto command that drives the robot across the auto line. - -package frc.robot.commands; - -import frc.robot.Constants.*; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DriveSubsystem; - -/** An example command that uses an example subsystem. */ -public class AutonomousDrive4Sec extends Command { - private final DriveSubsystem m_subsystem; - private double m_leftSpeed = 0.0; - private double m_rightSpeed = 0.0; - private long m_startTime; - - /** - * Creates a new AutonomousDrive4Sec. - * - * @param subsystem The subsystem used by this command. - */ - public AutonomousDrive4Sec(DriveSubsystem subsystem) - { - m_subsystem = subsystem; - - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_startTime = System.currentTimeMillis(); - m_subsystem.setTankSpeeds(0.0, 0.0); - - m_leftSpeed = AutoConstants.kPassLineSpeed; - m_rightSpeed = AutoConstants.kPassLineSpeed; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_subsystem.setTankSpeeds(m_leftSpeed, m_rightSpeed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_subsystem.setTankSpeeds(0.0, 0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (System.currentTimeMillis() - m_startTime) >= 4000; - } -} diff --git a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive6Sec.java b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive6Sec.java deleted file mode 100644 index c76ab39..0000000 --- a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive6Sec.java +++ /dev/null @@ -1,56 +0,0 @@ -// This is the default auto command that drives the robot across the auto line. - -package frc.robot.commands; - -import frc.robot.Constants.*; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DriveSubsystem; - -/** An example command that uses an example subsystem. */ -public class AutonomousDrive6Sec extends Command { - private final DriveSubsystem m_subsystem; - private double m_leftSpeed = 0.0; - private double m_rightSpeed = 0.0; - private long m_startTime; - - /** - * Creates a new AutonomousDrive6Sec. - * - * @param subsystem The subsystem used by this command. - */ - public AutonomousDrive6Sec(DriveSubsystem subsystem) - { - m_subsystem = subsystem; - - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_startTime = System.currentTimeMillis(); - m_subsystem.setTankSpeeds(0.0, 0.0); - - m_leftSpeed = AutoConstants.kPassLineSpeed; - m_rightSpeed = AutoConstants.kPassLineSpeed; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_subsystem.setTankSpeeds(m_leftSpeed, m_rightSpeed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_subsystem.setTankSpeeds(0.0, 0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (System.currentTimeMillis() - m_startTime) >= 6000; - } -} diff --git a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive2Sec.java b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDriveTimed.java similarity index 73% rename from CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive2Sec.java rename to CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDriveTimed.java index 9490ed9..0c45b61 100644 --- a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive2Sec.java +++ b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDriveTimed.java @@ -3,25 +3,33 @@ package frc.robot.commands; import frc.robot.Constants.*; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; /** An example command that uses an example subsystem. */ -public class AutonomousDrive2Sec extends Command { +public class AutonomousDriveTimed extends Command { private final DriveSubsystem m_subsystem; private double m_leftSpeed = 0.0; private double m_rightSpeed = 0.0; private long m_startTime; + private int m_executeTime; /** * Creates a new AutonomousDrive2Sec. * * @param subsystem The subsystem used by this command. + * @param executeTime wants time in positive miliseconds */ - public AutonomousDrive2Sec(DriveSubsystem subsystem) + public AutonomousDriveTimed(DriveSubsystem subsystem, int executeTime) { m_subsystem = subsystem; + if (executeTime < 0) { + DriverStation.reportError("executeTime in AutonomusDrive Constructor is negative", null); + } + m_executeTime = executeTime; + // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } @@ -51,6 +59,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return (System.currentTimeMillis() - m_startTime) >= 2000; + return (System.currentTimeMillis() - m_startTime) >= m_executeTime; } } diff --git a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousPassLine.java b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousPassLine.java deleted file mode 100644 index dd994a8..0000000 --- a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousPassLine.java +++ /dev/null @@ -1,56 +0,0 @@ -// This is the default auto command that drives the robot across the auto line. - -package frc.robot.commands; - -import frc.robot.Constants.*; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DriveSubsystem; - -/** An example command that uses an example subsystem. */ -public class AutonomousPassLine extends Command { - private final DriveSubsystem m_subsystem; - private double m_leftSpeed = 0.0; - private double m_rightSpeed = 0.0; - private long m_startTime; - - /** - * Creates a new AutonomousPassLine. - * - * @param subsystem The subsystem used by this command. - */ - public AutonomousPassLine(DriveSubsystem subsystem) - { - m_subsystem = subsystem; - - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_startTime = System.currentTimeMillis(); - m_subsystem.setTankSpeeds(0.0, 0.0); - - m_leftSpeed = AutoConstants.kPassLineSpeed; - m_rightSpeed = AutoConstants.kPassLineSpeed; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_subsystem.setTankSpeeds(m_leftSpeed, m_rightSpeed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_subsystem.setTankSpeeds(0.0, 0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (System.currentTimeMillis() - m_startTime) >= AutoConstants.kPassLineDuration_mS; - } -} From 1cda80e6a23aa9c10040392ddad5891eba5a74a2 Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Sat, 19 Oct 2024 12:37:51 -0500 Subject: [PATCH 2/9] Created a TimedCommand Decorator --- .../java/frc/robot/commands/TimedCommand.java | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 CompetitionSoftware/src/main/java/frc/robot/commands/TimedCommand.java diff --git a/CompetitionSoftware/src/main/java/frc/robot/commands/TimedCommand.java b/CompetitionSoftware/src/main/java/frc/robot/commands/TimedCommand.java new file mode 100644 index 0000000..c6b8a33 --- /dev/null +++ b/CompetitionSoftware/src/main/java/frc/robot/commands/TimedCommand.java @@ -0,0 +1,62 @@ +// This should end up in common when that is set up - Owen G. + +package frc.robot.commands; +import java.util.Set; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** An example command that uses an example subsystem. */ +public class TimedCommand extends Command { + private final Command m_command; + private long m_startTime; + private int m_executeTime; + + /** + * Runs a command for a timed ammount. + * + * @param command The command being executed. + * @param executeTime executeTime wants the run time in positive miliseconds. + */ + public TimedCommand(Command command, int executeTime) + { + m_command = command; + + if (executeTime < 0) { + DriverStation.reportError("executeTime in TimedCommand Constructor is negative", null); + } + m_executeTime = executeTime; + + // Gets the required subsystem dependencies and then adds them as a requirement. + Set requirements = m_command.getRequirements(); + for (Subsystem requirement : requirements) { + addRequirements(requirement); + } + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_startTime = System.currentTimeMillis(); + m_command.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_command.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_command.end(interrupted); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + boolean timedOut = (System.currentTimeMillis() - m_startTime) >= m_executeTime; + return timedOut || m_command.isFinished(); + } +} From 962782df901ef7963f28cff2ab50f5accb63e513 Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Sat, 19 Oct 2024 12:51:23 -0500 Subject: [PATCH 3/9] Added test auto option w/ new timedCommand. --- .../main/java/frc/robot/RobotContainer.java | 6 +++ .../frc/robot/commands/AutonomousDrive.java | 54 +++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive.java diff --git a/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java b/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java index 18730b3..a2b1374 100644 --- a/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java +++ b/CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,14 @@ package frc.robot; import frc.robot.Constants.*; +import frc.robot.commands.AutonomousDrive; import frc.robot.commands.AutonomousDriveTimed; import frc.robot.commands.ClimbCommand; import frc.robot.commands.NoteHandlingSpeedCommand; import frc.robot.commands.RotateCommand; import frc.robot.commands.SetGearCommand; import frc.robot.commands.SetModeCommand; +import frc.robot.commands.TimedCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.GooseRotationSubsystem; import frc.robot.subsystems.NoteHandlingSubsystem; @@ -123,6 +125,10 @@ private void configureDriverStationControls() m_autoChooser.addOption("Drive forward 2 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 2000)); m_autoChooser.addOption("Drive forward 4 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 4000)); m_autoChooser.addOption("Drive forward 6 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 6000)); + + AutonomousDrive autoDrive = new AutonomousDrive(m_driveSubsystem); + m_autoChooser.addOption("Drive Forward 2 Seconds !! NOT TESTED !!", new TimedCommand(autoDrive, 2000)); + SmartDashboard.putData(m_autoChooser); } diff --git a/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive.java b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive.java new file mode 100644 index 0000000..c4d5ea8 --- /dev/null +++ b/CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive.java @@ -0,0 +1,54 @@ +// This is the default auto command that drives the robot across the auto line. + +package frc.robot.commands; + +import frc.robot.Constants.*; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DriveSubsystem; + +/** An example command that uses an example subsystem. */ +public class AutonomousDrive extends Command { + private final DriveSubsystem m_subsystem; + private double m_leftSpeed = 0.0; + private double m_rightSpeed = 0.0; + + /** + * Creates a new AutonomousDrive2Sec. + * + * @param subsystem The subsystem used by this command. + */ + public AutonomousDrive(DriveSubsystem subsystem) + { + m_subsystem = subsystem; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_subsystem.setTankSpeeds(0.0, 0.0); + + m_leftSpeed = AutoConstants.kPassLineSpeed; + m_rightSpeed = AutoConstants.kPassLineSpeed; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_subsystem.setTankSpeeds(m_leftSpeed, m_rightSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_subsystem.setTankSpeeds(0.0, 0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; // <-- VERY SKETCHY !! + } +} From 5737c9371bb9f09928c8b4b5543ab3440b6fe1be Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 29 Oct 2024 18:34:44 -0500 Subject: [PATCH 4/9] Converted BasicDriveSingleTalon to Command Based, THIS IS UNTESTED. --- .../src/main/java/frc/robot/Constants.java | 30 ++++++ .../src/main/java/frc/robot/Robot.java | 83 ++++++++++++----- .../main/java/frc/robot/RobotContainer.java | 52 +++++++++++ .../robot/commands/ArcadeDriveCommand.java | 55 +++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 92 +++++++++++++++++++ .../robot/subsystems/ExampleSubsystem.java | 47 ++++++++++ 6 files changed, 338 insertions(+), 21 deletions(-) create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ArcadeDriveCommand.java create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/DriveSubsystem.java create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ExampleSubsystem.java diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..f2870aa --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java @@ -0,0 +1,30 @@ +// 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; + +/** + * 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 { + public static class DrivetrainConstants { + public static final int kLeftMotorCANID = 10; + public static final int kRightMotorCANID = 20; + } + + public static class DriverConstants { + public static final int kDriveJoystick = 0; + + public static final double kTurnDivider = 2.0; + } + + public static class OperatorConstants { + public static final int kOperatorController = 2; + } +} \ No newline at end of file diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java index 36aef35..a6f152a 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java @@ -4,48 +4,89 @@ package frc.robot; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj.smartdashboard.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; /** * This is a simple program supporting a basic drivetrain powered by a single Kraken (TalonFX) * on each side. The drivetrain is controlled via tank drive using two joysticks. */ public class Robot extends TimedRobot { - private DifferentialDrive m_myRobot; - private Joystick m_leftStick; + private Command m_autonomousCommand; - private final TalonFX m_leftMotor = new TalonFX(10); - private final TalonFX m_rightMotor = new TalonFX(20); + private RobotContainer m_robotContainer; + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ @Override public void robotInit() { - // We need to invert one side of the drivetrain so that positive voltages - // result in both sides moving forward. Depending on how your robot's - // gearbox is constructed, you might have to invert the left side instead. - m_rightMotor.setInverted(true); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } - m_myRobot = new DifferentialDrive(m_leftMotor, m_rightMotor); - m_leftMotor.setPosition(0.0); - m_rightMotor.setPosition(0.0); + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() {} - m_leftStick = new Joystick(0); + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } } + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + @Override public void teleopInit() { - m_leftMotor.setPosition(0.0); - m_rightMotor.setPosition(0.0); + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + + // Get the driver's choice of control system - arcade or tank drive. + m_robotContainer.setDriveType(); + + //TODO Do all motors need to be turned off here? } + /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - m_myRobot.arcadeDrive(-m_leftStick.getY(), -(m_leftStick.getTwist()/2.0), true); + + } - SmartDashboard.putNumber("LeftPosition", m_leftMotor.getPosition().getValue()); - SmartDashboard.putNumber("RightPosition", m_rightMotor.getPosition().getValue()); + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} } diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..f5ce86e --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,52 @@ +// 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; +import edu.wpi.first.wpilibj.Joystick; +//import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.*; +import frc.robot.subsystems.DriveSubsystem; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and trigger mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems are defined here + private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + + // Joysticks + //private final XboxController m_operatorController = new XboxController(OperatorConstants.kOperatorController); + private final Joystick m_driverJoystick = new Joystick(DriverConstants.kDriveJoystick); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + // Configure the trigger bindings + configureBindings(); + } + + // The function that connects the buttons to commands / subsystems. + private void configureBindings() + { + + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return null; + } + + // Set drive command to arcade + public void setDriveType() + { + m_driveSubsystem.initDefaultCommand(m_driverJoystick); + } +} diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ArcadeDriveCommand.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ArcadeDriveCommand.java new file mode 100644 index 0000000..84bbe30 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ArcadeDriveCommand.java @@ -0,0 +1,55 @@ +// This is one of the default commands for drive subsystem and allows control +// using a single joystick configured for arcade drive (Y axis controls speed, +// yaw/twist axis controls turn rate). + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriverConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** An example command that uses an example subsystem. */ +public class ArcadeDriveCommand extends Command { + private final DriveSubsystem m_subsystem; + private Joystick m_Joystick; + + /** + * Creates a new DriveCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ArcadeDriveCommand(DriveSubsystem subsystem, Joystick driveJoystick) + { + m_subsystem = subsystem; + m_Joystick = driveJoystick; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() + { + // Note: We negate both axis values so that pushing the joystick forwards + // (which makes the readin more negative) increases the speed and twisting clockwise + // turns the robot clockwise. + m_subsystem.setArcadeSpeeds(-m_Joystick.getY(), -(m_Joystick.getTwist() / DriverConstants.kTurnDivider)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() + { + return false; + } +} \ No newline at end of file diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..3546570 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,92 @@ +// +// A basic tank drive subsystem class using 2 Kraken/TalonFX motors +// per side. +// +package frc.robot.subsystems; + +import frc.robot.Constants.*; +import frc.robot.commands.ArcadeDriveCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class DriveSubsystem extends SubsystemBase +{ + private final TalonFX m_leftMotor = new TalonFX(DrivetrainConstants.kLeftMotorCANID); + private final TalonFX m_rightMotor = new TalonFX(DrivetrainConstants.kRightMotorCANID); + private DifferentialDrive m_Drivetrain; + private double m_leftSpeed = 0.0; + private double m_rightSpeed = 0.0; + private double m_speedDivider = 2.0; // Default 1.0 + private double m_modeMultiplier = 1.0; + private boolean m_driveStraight = false; + + /** Creates a new DriveSubsystem. */ + public DriveSubsystem() + { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + + // NOTE: Invert needs to be done for both motors BEFORE follow + m_rightMotor.setInverted(true); + + // Set all motors to brake mode for safety. In the default, coast mode, + // the robot takes very much longer to stop if the joystick is released or + // and emergency stop occurs. In this mode, it stops very quickly. + m_leftMotor.setNeutralMode(NeutralModeValue.Brake); + m_rightMotor.setNeutralMode(NeutralModeValue.Brake); + + m_Drivetrain = new DifferentialDrive(m_leftMotor, m_rightMotor); + } + + public void initDefaultCommand(Joystick leftJoystick) + { + setDefaultCommand(new ArcadeDriveCommand(this, leftJoystick)); + } + + // Sets left and right motors to set speeds to support tank drive models. + // rightInput is ignored when straight mode is enabled. + public void setTankSpeeds(double leftInput, double rightInput) + { + m_leftSpeed = (leftInput / m_speedDivider) * m_modeMultiplier; + m_rightSpeed = m_driveStraight ? m_leftSpeed : (rightInput / m_speedDivider) * m_modeMultiplier; + + // NOTE: We are squaring the input to improve driver response + m_Drivetrain.tankDrive(m_leftSpeed, m_rightSpeed, true); + } + + public void setArcadeSpeeds(double speed, double rotation) + { + m_leftSpeed = (speed / m_speedDivider) * m_modeMultiplier; + m_rightSpeed = rotation; // NOTE: Deliberately did not slow down in low gear. + + // NOTE: We are squaring the input to improve driver response + m_Drivetrain.arcadeDrive(m_leftSpeed, m_rightSpeed, true); + } + + public double getSpeed(boolean bLeft) + { + if (bLeft) + { + return m_leftSpeed; + } + else + { + return m_rightSpeed; + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} \ No newline at end of file diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ExampleSubsystem.java new file mode 100644 index 0000000..a697f95 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ExampleSubsystem.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 edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ExampleSubsystem extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + public ExampleSubsystem() {} + + /** + * Example command factory method. + * + * @return a command + */ + public Command exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> { + /* one-time action goes here */ + }); + } + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} \ No newline at end of file From 66d42735c02443749b4907adcfbcda9b4c9f81a2 Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 29 Oct 2024 18:53:24 -0500 Subject: [PATCH 5/9] Started on wheeledIntake test code It has 2 motors controled via CANsparkmax --- .../src/main/java/frc/robot/Constants.java | 5 ++ .../subsystems/WheeledIntakeSubsystem.java | 18 +++++ .../vendordeps/REVLib.json | 74 +++++++++++++++++++ 3 files changed, 97 insertions(+) create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java create mode 100644 Prototype_Projects/BasicDriveSingleTalon/vendordeps/REVLib.json diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java index f2870aa..828f2c8 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java @@ -27,4 +27,9 @@ public static class DriverConstants { public static class OperatorConstants { public static final int kOperatorController = 2; } + + public static class WheeledIntakeConstants { + public static final int kLeftMotorCANID = 30; + public static final int kRightMotorCANID = 40; + } } \ No newline at end of file diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java new file mode 100644 index 0000000..f68cf02 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems; + + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.WheeledIntakeConstants; + +public class WheeledIntakeSubsystem extends SubsystemBase { + private final CANSparkMax m_leftMotor = new CANSparkMax(WheeledIntakeConstants.kLeftMotorCANID, + MotorType.kBrushless); + + public WheeledIntakeSubsystem() + { + + } +} diff --git a/Prototype_Projects/BasicDriveSingleTalon/vendordeps/REVLib.json b/Prototype_Projects/BasicDriveSingleTalon/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/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 e14075cca03dab29706b14c8b722d968ef698bb1 Mon Sep 17 00:00:00 2001 From: Capyblapy <111537871+Capyblapy@users.noreply.github.com> Date: Thu, 7 Nov 2024 12:46:38 -0600 Subject: [PATCH 6/9] Finished WheeledIntake test code When trigger is held down it should move the wheels at -0.6 speed, this is untested. I set the current limit to 30, may need to raise. --- .../src/main/java/frc/robot/Constants.java | 9 +++- .../main/java/frc/robot/RobotContainer.java | 12 ++++- .../commands/WheeledIntakeSpeedCommand.java | 46 +++++++++++++++++++ .../subsystems/WheeledIntakeSubsystem.java | 41 +++++++++++++++++ 4 files changed, 106 insertions(+), 2 deletions(-) create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java index 828f2c8..2f8ed70 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java @@ -20,8 +20,10 @@ public static class DrivetrainConstants { public static class DriverConstants { public static final int kDriveJoystick = 0; - public static final double kTurnDivider = 2.0; + + // Buttons + public static final int kIntake = 1; // Joystick Trigger } public static class OperatorConstants { @@ -31,5 +33,10 @@ public static class OperatorConstants { public static class WheeledIntakeConstants { public static final int kLeftMotorCANID = 30; public static final int kRightMotorCANID = 40; + + public static final int kLeftMotorCurrentLimit = 30; + public static final int kRightMotorCurrentLimit = 30; + + public static final double kRollerSpeed = -0.6; // "random" value } } \ No newline at end of file diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java index f5ce86e..48c414c 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java @@ -6,8 +6,11 @@ import edu.wpi.first.wpilibj.Joystick; //import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.*; +import frc.robot.commands.WheeledIntakeSpeedCommand; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.WheeledIntakeSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -18,11 +21,16 @@ public class RobotContainer { // The robot's subsystems are defined here private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + private final WheeledIntakeSubsystem m_wheeledIntakeSubsystem = new WheeledIntakeSubsystem(); // Joysticks //private final XboxController m_operatorController = new XboxController(OperatorConstants.kOperatorController); private final Joystick m_driverJoystick = new Joystick(DriverConstants.kDriveJoystick); + // Joystick Buttons + public final JoystickButton m_intakeButton = + new JoystickButton(m_driverJoystick, DriverConstants.kIntake); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings @@ -32,7 +40,9 @@ public RobotContainer() { // The function that connects the buttons to commands / subsystems. private void configureBindings() { - + // Wheeled Intake Bindings + m_intakeButton.onTrue(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, WheeledIntakeConstants.kRollerSpeed)); + m_intakeButton.onFalse(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, 0)); } /** diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java new file mode 100644 index 0000000..1bbf848 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java @@ -0,0 +1,46 @@ +// This Supports setting a motor to a set speed. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.WheeledIntakeSubsystem; + +/** An example command that uses an example subsystem. */ +public class WheeledIntakeSpeedCommand extends Command { + private final WheeledIntakeSubsystem m_subsystem; + private double m_speed = 0.0; + + /** + * Creates a new WinchStartCommand. + * + * @param subsystem The subsystem used by this command. + */ + public WheeledIntakeSpeedCommand(WheeledIntakeSubsystem subsystem, double speed) { + m_subsystem = subsystem; + m_speed = speed; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_subsystem.setLeftSpeed(m_speed); + m_subsystem.setRightSpeed(m_speed); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java index f68cf02..58ef0c0 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java @@ -10,9 +10,50 @@ public class WheeledIntakeSubsystem extends SubsystemBase { private final CANSparkMax m_leftMotor = new CANSparkMax(WheeledIntakeConstants.kLeftMotorCANID, MotorType.kBrushless); + private final CANSparkMax m_rightMotor = new CANSparkMax(WheeledIntakeConstants.kLeftMotorCANID, + MotorType.kBrushless); + + private double m_leftSpeed = 0.0; + private double m_rightSpeed = 0.0; + /** Creates a new WheeledIntakeSubsystem. */ public WheeledIntakeSubsystem() { + m_leftMotor.setSmartCurrentLimit(WheeledIntakeConstants.kLeftMotorCurrentLimit); + m_rightMotor.setSmartCurrentLimit(WheeledIntakeConstants.kRightMotorCurrentLimit); + } + + public void setLeftSpeed(double speed) + { + m_leftMotor.set(speed); + m_leftSpeed = speed; + } + + // Returns last COMMANDED speed + public double getLeftSpeed() + { + return m_leftSpeed; + } + + public void setRightSpeed(double speed) + { + m_rightMotor.set(speed); + m_rightSpeed = speed; + } + // Returns last COMMANDED speed + public double getRightSpeed() + { + return m_rightSpeed; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation } } From 7699023f4b4e5c00d29712446736567d10dea1ce Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 12 Nov 2024 18:57:36 -0600 Subject: [PATCH 7/9] Improved WheeldIntake code to meet requirements --- .../BasicDriveSingleTalon/networktables.json | 1 + .../src/main/java/frc/robot/Constants.java | 10 ++++-- .../src/main/java/frc/robot/Robot.java | 12 +++++++ .../main/java/frc/robot/RobotContainer.java | 32 ++++++++++++++++--- .../commands/WheeledIntakeSpeedCommand.java | 3 +- .../subsystems/WheeledIntakeSubsystem.java | 18 ++++++++++- 6 files changed, 67 insertions(+), 9 deletions(-) create mode 100644 Prototype_Projects/BasicDriveSingleTalon/networktables.json diff --git a/Prototype_Projects/BasicDriveSingleTalon/networktables.json b/Prototype_Projects/BasicDriveSingleTalon/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/networktables.json @@ -0,0 +1 @@ +[] diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java index 2f8ed70..1068caa 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java @@ -13,6 +13,11 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public static class RobotConstants { + public static final int periodicTicksPerSecond = 50; + public static final int intakeReportingFreq = 1; + } + public static class DrivetrainConstants { public static final int kLeftMotorCANID = 10; public static final int kRightMotorCANID = 20; @@ -24,6 +29,7 @@ public static class DriverConstants { // Buttons public static final int kIntake = 1; // Joystick Trigger + public static final double kThrottleMultiplier = -1.0; } public static class OperatorConstants { @@ -34,8 +40,8 @@ public static class WheeledIntakeConstants { public static final int kLeftMotorCANID = 30; public static final int kRightMotorCANID = 40; - public static final int kLeftMotorCurrentLimit = 30; - public static final int kRightMotorCurrentLimit = 30; + public static final int kLeftMotorCurrentLimit = 3; + public static final int kRightMotorCurrentLimit = 3; public static final double kRollerSpeed = -0.6; // "random" value } diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java index a6f152a..6869f7c 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Robot.java @@ -28,6 +28,18 @@ public void robotInit() { m_robotContainer = new RobotContainer(); } + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + + // Send back telemetry to driver station + m_robotContainer.periodicFunction(); + } + /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() {} diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java index 48c414c..fab37fa 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,13 @@ package frc.robot; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.*; import frc.robot.commands.WheeledIntakeSpeedCommand; -import frc.robot.subsystems.DriveSubsystem; +//import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.WheeledIntakeSubsystem; /** @@ -19,10 +20,14 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + private int m_tickCount = 0; + // The robot's subsystems are defined here - private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + //private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final WheeledIntakeSubsystem m_wheeledIntakeSubsystem = new WheeledIntakeSubsystem(); + // TODO: Control Speed via Slider axis 3 (need testing) + // Joysticks //private final XboxController m_operatorController = new XboxController(OperatorConstants.kOperatorController); private final Joystick m_driverJoystick = new Joystick(DriverConstants.kDriveJoystick); @@ -31,17 +36,36 @@ public class RobotContainer { public final JoystickButton m_intakeButton = new JoystickButton(m_driverJoystick, DriverConstants.kIntake); + // Joystick Values + public Double m_driverThrottle = 0.0; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); + + m_tickCount = 0; + periodicFunction(); + } + + public void periodicFunction() { + // Updating Values + m_driverThrottle = m_driverJoystick.getThrottle() * DriverConstants.kThrottleMultiplier; + + // Reporting Status' + if(m_tickCount % (RobotConstants.periodicTicksPerSecond/RobotConstants.intakeReportingFreq) == 0) + { + SmartDashboard.putNumber("Intake Speed", m_driverThrottle); + } + m_tickCount += 1; } // The function that connects the buttons to commands / subsystems. private void configureBindings() { // Wheeled Intake Bindings - m_intakeButton.onTrue(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, WheeledIntakeConstants.kRollerSpeed)); + // inversing the throttle so up = posive and down = negitive + m_intakeButton.onTrue(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, m_driverThrottle)); m_intakeButton.onFalse(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, 0)); } @@ -57,6 +81,6 @@ public Command getAutonomousCommand() { // Set drive command to arcade public void setDriveType() { - m_driveSubsystem.initDefaultCommand(m_driverJoystick); + //m_driveSubsystem.initDefaultCommand(m_driverJoystick); } } diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java index 1bbf848..3b5503f 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/WheeledIntakeSpeedCommand.java @@ -26,8 +26,7 @@ public WheeledIntakeSpeedCommand(WheeledIntakeSubsystem subsystem, double speed) // Called when the command is initially scheduled. @Override public void initialize() { - m_subsystem.setLeftSpeed(m_speed); - m_subsystem.setRightSpeed(m_speed); + m_subsystem.setSpeed(m_speed); } // Called every time the scheduler runs while the command is scheduled. diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java index 58ef0c0..c720c11 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/WheeledIntakeSubsystem.java @@ -10,7 +10,7 @@ public class WheeledIntakeSubsystem extends SubsystemBase { private final CANSparkMax m_leftMotor = new CANSparkMax(WheeledIntakeConstants.kLeftMotorCANID, MotorType.kBrushless); - private final CANSparkMax m_rightMotor = new CANSparkMax(WheeledIntakeConstants.kLeftMotorCANID, + private final CANSparkMax m_rightMotor = new CANSparkMax(WheeledIntakeConstants.kRightMotorCANID, MotorType.kBrushless); private double m_leftSpeed = 0.0; @@ -23,6 +23,22 @@ public WheeledIntakeSubsystem() m_rightMotor.setSmartCurrentLimit(WheeledIntakeConstants.kRightMotorCurrentLimit); } + // TODO: Make motors spin at the same speed in opisate directions (NEED TEST) + public void setSpeed(double speed) + { + m_leftMotor.set(speed); + m_leftSpeed = speed; + + m_rightMotor.set(-speed); + m_rightSpeed = -speed; + } + + // Returns last COMMANDED speed + public double getSpeed() + { + return m_leftSpeed; + } + public void setLeftSpeed(double speed) { m_leftMotor.set(speed); From 2b71443da7941a0422421b5fcf2931a92f7bd3da Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 17 Dec 2024 18:51:39 -0600 Subject: [PATCH 8/9] Added Clawdia compatibility to BasicDriveSingleTalon (mule code) --- .../src/main/java/frc/robot/Constants.java | 13 +++++- .../main/java/frc/robot/RobotContainer.java | 42 ++++++++++++----- .../robot/commands/ClawdiaSpeedCommand.java | 45 +++++++++++++++++++ .../robot/subsystems/ClawdiaSubsystem.java | 43 ++++++++++++++++++ 4 files changed, 131 insertions(+), 12 deletions(-) create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ClawdiaSpeedCommand.java create mode 100644 Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ClawdiaSubsystem.java diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java index 1068caa..a1aa390 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java @@ -33,7 +33,10 @@ public static class DriverConstants { } public static class OperatorConstants { - public static final int kOperatorController = 2; + public static final int kOperatorController = 1; + + public static final int kRunForwardButton = 4; // Button Y + public static final int kRunBackwardButton = 3; // Button X } public static class WheeledIntakeConstants { @@ -45,4 +48,12 @@ public static class WheeledIntakeConstants { public static final double kRollerSpeed = -0.6; // "random" value } + + public static class ClawdiaConstants { + public static final int kMotorCANID = 50; + + public static final int kMotorCurrentLimit = 3; + + public static final double kClawSpeed = 0.1; // low value for testing + } } \ No newline at end of file diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java index fab37fa..9b55b5b 100644 --- a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,15 @@ package frc.robot; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.*; +import frc.robot.commands.ClawdiaSpeedCommand; import frc.robot.commands.WheeledIntakeSpeedCommand; +import frc.robot.subsystems.ClawdiaSubsystem; //import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.WheeledIntakeSubsystem; @@ -24,17 +27,27 @@ public class RobotContainer { // The robot's subsystems are defined here //private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final WheeledIntakeSubsystem m_wheeledIntakeSubsystem = new WheeledIntakeSubsystem(); + //private final WheeledIntakeSubsystem m_wheeledIntakeSubsystem = new WheeledIntakeSubsystem(); + private final ClawdiaSubsystem m_clawdiaSubsystem = new ClawdiaSubsystem(); // TODO: Control Speed via Slider axis 3 (need testing) // Joysticks //private final XboxController m_operatorController = new XboxController(OperatorConstants.kOperatorController); - private final Joystick m_driverJoystick = new Joystick(DriverConstants.kDriveJoystick); + //private final Joystick m_driverJoystick = new Joystick(DriverConstants.kDriveJoystick); // Joystick Buttons - public final JoystickButton m_intakeButton = - new JoystickButton(m_driverJoystick, DriverConstants.kIntake); + //public final JoystickButton m_intakeButton = + // new JoystickButton(m_driverJoystick, DriverConstants.kIntake); + + // Controllers + private final XboxController m_operatorController = new XboxController(OperatorConstants.kOperatorController); + + // Controller Buttons + public final JoystickButton m_runForwardButton = + new JoystickButton(m_operatorController, OperatorConstants.kRunForwardButton); + public final JoystickButton m_runBackwardButton = + new JoystickButton(m_operatorController, OperatorConstants.kRunBackwardButton); // Joystick Values public Double m_driverThrottle = 0.0; @@ -50,13 +63,13 @@ public RobotContainer() { public void periodicFunction() { // Updating Values - m_driverThrottle = m_driverJoystick.getThrottle() * DriverConstants.kThrottleMultiplier; + //m_driverThrottle = m_driverJoystick.getThrottle() * DriverConstants.kThrottleMultiplier; // Reporting Status' - if(m_tickCount % (RobotConstants.periodicTicksPerSecond/RobotConstants.intakeReportingFreq) == 0) - { - SmartDashboard.putNumber("Intake Speed", m_driverThrottle); - } + //if(m_tickCount % (RobotConstants.periodicTicksPerSecond/RobotConstants.intakeReportingFreq) == 0) + //{ + // SmartDashboard.putNumber("Intake Speed", m_driverThrottle); + //} m_tickCount += 1; } @@ -65,8 +78,15 @@ private void configureBindings() { // Wheeled Intake Bindings // inversing the throttle so up = posive and down = negitive - m_intakeButton.onTrue(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, m_driverThrottle)); - m_intakeButton.onFalse(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, 0)); + //m_intakeButton.onTrue(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, m_driverThrottle)); + //m_intakeButton.onFalse(new WheeledIntakeSpeedCommand(m_wheeledIntakeSubsystem, 0)); + + // Clawdia Bindings + m_runForwardButton.onTrue(new ClawdiaSpeedCommand(m_clawdiaSubsystem, ClawdiaConstants.kClawSpeed)); + m_runForwardButton.onFalse(new ClawdiaSpeedCommand(m_clawdiaSubsystem, 0)); + + m_runBackwardButton.onTrue(new ClawdiaSpeedCommand(m_clawdiaSubsystem, -ClawdiaConstants.kClawSpeed)); + m_runBackwardButton.onFalse(new ClawdiaSpeedCommand(m_clawdiaSubsystem, 0)); } /** diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ClawdiaSpeedCommand.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ClawdiaSpeedCommand.java new file mode 100644 index 0000000..f77db79 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/commands/ClawdiaSpeedCommand.java @@ -0,0 +1,45 @@ +// This Supports setting a motor to a set speed. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ClawdiaSubsystem; + +/** An example command that uses an example subsystem. */ +public class ClawdiaSpeedCommand extends Command { + private final ClawdiaSubsystem m_subsystem; + private double m_speed = 0.0; + + /** + * Creates a new WinchStartCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ClawdiaSpeedCommand(ClawdiaSubsystem subsystem, double speed) { + m_subsystem = subsystem; + m_speed = speed; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_subsystem.setSpeed(m_speed); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ClawdiaSubsystem.java b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ClawdiaSubsystem.java new file mode 100644 index 0000000..0edd682 --- /dev/null +++ b/Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/subsystems/ClawdiaSubsystem.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems; + + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ClawdiaConstants; + +public class ClawdiaSubsystem extends SubsystemBase { + private final CANSparkMax m_motor = new CANSparkMax(ClawdiaConstants.kMotorCANID, + MotorType.kBrushless); + + private double m_motorSpeed = 0.0; + + /** Creates a new WheeledIntakeSubsystem. */ + public ClawdiaSubsystem() + { + m_motor.setSmartCurrentLimit(ClawdiaConstants.kMotorCurrentLimit); + } + + // Returns last COMMANDED speed + public double getSpeed() + { + return m_motorSpeed; + } + + public void setSpeed(double speed) + { + m_motor.set(speed); + m_motorSpeed = speed; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} From 3b2491d9262ff2b7bf54695af4b0e8e642171882 Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Sat, 8 Feb 2025 10:19:54 -0600 Subject: [PATCH 9/9] Updated shuffleboard --- .../Shuffleboard/driveStation-Feb1-2025.json | 310 ++++++++++++++++++ Resources/Shuffleboard/driveStation.json | 79 ++++- 2 files changed, 384 insertions(+), 5 deletions(-) create mode 100644 Resources/Shuffleboard/driveStation-Feb1-2025.json diff --git a/Resources/Shuffleboard/driveStation-Feb1-2025.json b/Resources/Shuffleboard/driveStation-Feb1-2025.json new file mode 100644 index 0000000..0081680 --- /dev/null +++ b/Resources/Shuffleboard/driveStation-Feb1-2025.json @@ -0,0 +1,310 @@ +{ + "tabPane": [ + { + "title": "Untitled Goose Bot", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "0,0": { + "size": [ + 4, + 3 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://Driver Camera", + "_title": "Camera Stream", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Reverse Mode", + "_title": "Reverse Mode", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Compressor Running", + "_title": "Compressor Running", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "5,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Goose Set Point", + "_title": "Goose Set Point", + "_glyph": 148, + "_showGlyph": false + } + }, + "5,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Pressure", + "_title": "Pressure", + "_glyph": 148, + "_showGlyph": false + } + }, + "6,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Goose Raw Measurement", + "_title": "Goose Raw Measurement", + "_glyph": 148, + "_showGlyph": false + } + }, + "6,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Goose Current Angle", + "_title": "Goose Current Angle", + "_glyph": 148, + "_showGlyph": false + } + }, + "7,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + } + } + } + }, + { + "title": "Mule", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "5,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Is Elevator Down", + "_title": "/SmartDashboard/Is Elevator Down", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "3,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Elevator Encoder", + "_title": "/SmartDashboard/Elevator Encoder", + "_glyph": 148, + "_showGlyph": false + } + } + } + } + }, + { + "title": "New Robot", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "6,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "Auto Program", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Algae Subsystem", + "_title": "Algae Subsystem", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "2,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Coral Subsystem", + "_title": "Coral Subsystem", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "0,1": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Elevator Subsystem", + "_title": "Elevator Subsystem", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "2,1": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Intake Subsystem", + "_title": "Intake Subsystem", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "6,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/LIDAR Distance", + "_title": "/SmartDashboard/LIDAR Distance", + "_glyph": 148, + "_showGlyph": false + } + }, + "7,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Elevator Down", + "_title": "/SmartDashboard/Elevator Down", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Elevator Encoder", + "_title": "/SmartDashboard/Elevator Encoder", + "_glyph": 148, + "_showGlyph": false + } + } + } + } + } + ], + "windowGeometry": { + "x": -4.0, + "y": 0.0, + "width": 1290.0, + "height": 480.0 + } +} \ No newline at end of file diff --git a/Resources/Shuffleboard/driveStation.json b/Resources/Shuffleboard/driveStation.json index a6d05ea..76a7387 100644 --- a/Resources/Shuffleboard/driveStation.json +++ b/Resources/Shuffleboard/driveStation.json @@ -1,7 +1,7 @@ { "tabPane": [ { - "title": "Tab 3", + "title": "Untitled Goose Bot", "autoPopulate": false, "autoPopulatePrefix": "", "widgetPane": { @@ -129,12 +129,81 @@ } } } + }, + { + "title": "Mule", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "5,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Is Elevator Down", + "_title": "/SmartDashboard/Is Elevator Down", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "3,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Elevator Encoder", + "_title": "/SmartDashboard/Elevator Encoder", + "_glyph": 148, + "_showGlyph": false + } + } + } + } + }, + { + "title": "New Robot", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "3,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + } + } + } } ], "windowGeometry": { - "x": -6.0, - "y": 0.6666666865348816, - "width": 1292.0, - "height": 685.3333129882812 + "x": -4.0, + "y": 0.0, + "width": 1290.0, + "height": 480.0 } } \ No newline at end of file