Skip to content

Commit

Permalink
Organizing the commands and whatnot
Browse files Browse the repository at this point in the history 
	modified:   src/main/java/frc/robot/RobotContainer.java
	modified:   src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
	renamed:    src/main/java/frc/robot/Telemetry.java -> src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java
  • Loading branch information
@tbowers7
tbowers7 committed Oct 30, 2024
1 parent 2ce0bdc commit 0b5e080
Show file tree
Hide file tree
Showing 3 changed files with 87 additions and 84 deletions.
130 changes: 49 additions & 81 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,10 @@

package frc.robot;

import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -37,6 +36,7 @@
import frc.robot.subsystems.flywheel_example.FlywheelIO;
import frc.robot.subsystems.flywheel_example.FlywheelIOSim;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import frc.robot.subsystems.swervedrive.SwerveTelemetry;
import frc.robot.subsystems.vision.Vision;
import frc.robot.subsystems.vision.VisionIO;
import frc.robot.subsystems.vision.VisionIOPhoton;
Expand All @@ -47,7 +47,7 @@
public class RobotContainer {

// Define the Driver and, optionally, the Operator/Co-Driver Controllers
// Replace with CommandPS4Controller or CommandJoystick if needed
// Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
final CommandXboxController operatorXbox = new CommandXboxController(1);
final OverrideSwitches overrides = new OverrideSwitches(2);
Expand All @@ -61,6 +61,9 @@ public class RobotContainer {
// Dashboard inputs
private final LoggedDashboardChooser<Command> autoChooser;

// Swerve Drive Telemetry
private final SwerveTelemetry logger = new SwerveTelemetry(DrivebaseConstants.kMaxLinearSpeed);

/** Returns the current AprilTag layout type. */
public AprilTagLayoutType getAprilTagLayoutType() {
return AprilTagConstants.defaultAprilTagType;
Expand Down Expand Up @@ -106,58 +109,17 @@ public RobotContainer() {

// Configure the trigger bindings
configureBindings();
// Define TeleOp commands
defineTeleopCommands();
// Define Auto commands
defineAutoCommands();
// Set up the SmartDashboard Auto Chooser
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
}

/** Use this method to define your TeleOp commands. */
private void defineTeleopCommands() {

// Field-centric driving in open loop mode
final SwerveRequest.FieldCentric drive =
new SwerveRequest.FieldCentric()
.withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1)
.withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);

final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
final SwerveRequest.RobotCentric forwardStraight =
new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();

m_drivebase.setDefaultCommand( // Drivetrain will execute this command periodically
m_drivebase
.applyRequest(
() ->
drive
.withVelocityX(
-driverXbox.getLeftY()
* DrivebaseConstants.kMaxLinearSpeed) // Drive forward with
// negative Y (forward)
.withVelocityY(
-driverXbox.getLeftX()
* DrivebaseConstants
.kMaxLinearSpeed) // Drive left with negative X (left)
.withRotationalRate(
-driverXbox.getRightX()
* DrivebaseConstants
.kMaxAngularSpeed) // Drive counterclockwise with negative X
// (left)
)
.ignoringDisable(true));
// Set up the logger
m_drivebase.registerTelemetry(logger::telemeterize);
}

/** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */
private void defineAutoCommands() {

Command runAuto = m_drivebase.getAutoPath("Tests");

final Telemetry logger = new Telemetry(DrivebaseConstants.kMaxLinearSpeed);

NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.tareEverything()));
}

Expand All @@ -172,42 +134,48 @@ private void defineAutoCommands() {
*/
private void configureBindings() {

// Manually Re-Zero the Gyro
// Manually Re-Zero the Gyro & Odometry
driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.tareEverything()));

// Example button bindings from YAGSL
// if (DriverStation.isTest())
// {
// driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());
// driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
// driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2));
// driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
// driverXbox.back().whileTrue(drivebase.centerModulesCommand());
// driverXbox.leftBumper().onTrue(Commands.none());
// driverXbox.rightBumper().onTrue(Commands.none());
// drivebase.setDefaultCommand(
// !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity :
// driveFieldOrientedDirectAngleSim);
// } else
// {
// driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
// driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
// driverXbox.b().whileTrue(
// Commands.deferredProxy(() -> drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4),
// Rotation2d.fromDegrees(0)))
// ));
// driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2));
// driverXbox.start().whileTrue(Commands.none());
// driverXbox.back().whileTrue(Commands.none());
// driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock,
// drivebase).repeatedly());
// driverXbox.rightBumper().onTrue(Commands.none());
// drivebase.setDefaultCommand(
// !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle :
// driveFieldOrientedDirectAngleSim);
// }

// Example Swerve Drive button bindings
// A -> BRAKE
driverXbox.a().whileTrue(m_drivebase.applyRequest(() -> m_drivebase.brake));
// B -> POINT WHEELS AT DIRECTION WITHOUT MOVING
driverXbox
.b()
.whileTrue(
m_drivebase.applyRequest(
() ->
m_drivebase.point.withModuleDirection(
new Rotation2d(-driverXbox.getLeftY(), -driverXbox.getLeftX()))));
// LEFT BUMPER -> reset the field-centric heading
driverXbox.leftBumper().onTrue(m_drivebase.runOnce(() -> m_drivebase.seedFieldRelative()));
// POV UP -> DRIVE FORWARD IN ROBOT-CENTRIC MODE
driverXbox
.pov(0)
.whileTrue(
m_drivebase.applyRequest(
() -> m_drivebase.forwardStraight.withVelocityX(0.5).withVelocityY(0)));
// POV DOWN -> DRIVE BACKWARD IN ROBOT-CENTRIC MODE
driverXbox
.pov(180)
.whileTrue(
m_drivebase.applyRequest(
() -> m_drivebase.forwardStraight.withVelocityX(-0.5).withVelocityY(0)));

// SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE
// NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation
m_drivebase.setDefaultCommand(
m_drivebase
.applyRequest(
() ->
m_drivebase
.drive
.withVelocityX(-driverXbox.getLeftY() * DrivebaseConstants.kMaxLinearSpeed)
.withVelocityY(-driverXbox.getLeftX() * DrivebaseConstants.kMaxLinearSpeed)
.withRotationalRate(
-driverXbox.getRightX() * DrivebaseConstants.kMaxAngularSpeed))
.ignoringDisable(true));
}

/**
Expand All @@ -218,7 +186,7 @@ private void configureBindings() {
public Command getAutonomousCommand() {

// An example command will be run in autonomous
return m_drivebase.getAutonomousCommand("New Auto");
return m_drivebase.getAutoPath("Tests");
// Use the ``autoChooser`` to define your auto path from the SmartDashboard
// return autoChooser.get();
}
Expand Down
15 changes: 15 additions & 0 deletions src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.ctre.phoenix6.signals.NeutralModeValue;
Expand All @@ -47,6 +48,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants.DrivebaseConstants;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.vision.Vision;
import java.util.function.Supplier;
Expand Down Expand Up @@ -169,6 +171,19 @@ private void configurePathPlanner() {
this); // Subsystem for requirements
}

/* Section for defining TeleOp Commands */
// Field-centric driving in open loop mode
public final SwerveRequest.FieldCentric drive =
new SwerveRequest.FieldCentric()
.withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1)
.withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);

public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
public final SwerveRequest.RobotCentric forwardStraight =
new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();

public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
}
Expand Down
26 changes: 23 additions & 3 deletions src/main/java/frc/robot/Telemetry.java → ...bsystems/swervedrive/SwerveTelemetry.java
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,24 @@
package frc.robot;
// Copyright (c) 2024 Az-FIRST
// http://github.com/AZ-First
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// 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.
//
// NOTE: This module based on the CTRE Phoenix6 examples
// https://github.com/CrossTheRoadElec/Phoenix6-Examples

package frc.robot.subsystems.swervedrive;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
Expand All @@ -16,15 +36,15 @@
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;

public class Telemetry {
public class SwerveTelemetry {
private final double MaxSpeed;

/**
* Construct a telemetry object, with the specified max speed of the robot
*
* @param maxSpeed Maximum speed in meters per second
*/
public Telemetry(double maxSpeed) {
public SwerveTelemetry(double maxSpeed) {
MaxSpeed = maxSpeed;
SignalLogger.start();
}
Expand Down

0 comments on commit 0b5e080

Please sign in to comment.