Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'Prep-For-Denver' into DeBounce

Browse Source
This commit is contained in:
Connor Peach
2024-03-16 12:10:34 -06:00
parent 4430b9f0c8 4efa4385ff
commit eacc88350a
14 changed files with 323 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -10
View File
@@ -7,12 +7,10 @@
package frc4388.robot;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.utility.LEDPatterns;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -40,10 +38,10 @@ public final class Constants {
public static final double TURBO_SPEED = 4.0;
public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625;
public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
public static final double BACK_LEFT_ROT_OFFSET = -277.646484375;
public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625;
public static final double FRONT_LEFT_ROT_OFFSET = 220;
public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
public static final double BACK_LEFT_ROT_OFFSET = -279.08349884;
public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656;
}
public static final class IDs {
@@ -191,8 +189,8 @@ public final class Constants {
public static final class ClimbConstants {
public static final int CLIMB_MOTOR_ID = 19;
public static final double CLIMB_OUT_SPEED = 0.3;
public static final double CLIMB_IN_SPEED = -0.3;
public static final double CLIMB_IN_SPEED = -1.0;
public static final double CLIMB_OUT_SPEED = 0.87;
}
public static final class OIConstants {
src/main/java/frc4388/robot/RobotContainer.java
+60 -32
View File
@@ -87,8 +87,8 @@ public class RobotContainer {
private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup(
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.idle()),
new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0)))
new InstantCommand(() -> m_robotShooter.idle())
// new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
// new InstantCommand(() -> m_robotShooter.spin())
);
@@ -133,8 +133,8 @@ public class RobotContainer {
);
private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
@@ -327,34 +327,46 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()));
// * /* D-Pad Stuff */
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
// ! /* Auto Recording */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// () -> getDeadbandedDriverController().getLeftX(),
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "Taxi.txt"))
// .onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
@@ -368,6 +380,18 @@ public class RobotContainer {
true, false))
.onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// () -> getDeadbandedDriverController().getLeftX(),
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "Taxi.txt"))
// .onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
// .onFalse(new InstantCommand());
// ! /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
@@ -376,11 +400,11 @@ public class RobotContainer {
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .whileTrue(new InstantCommand(() ->
// m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
// new Translation2d(0, 0),
// true), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.whileTrue(new InstantCommand(() ->
m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
new Translation2d(0, 0),
true), m_robotSwerveDrive));
//? /* Operator Buttons */
@@ -430,8 +454,12 @@ public class RobotContainer {
.onTrue(emergencyRetract);
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotShooter.idle()))
.onFalse(new InstantCommand(() -> m_robotShooter.stop()));
.onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
.onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java
+107
View File
@@ -0,0 +1,107 @@
// package frc4388.robot.commands.Autos;
// import java.io.File;
// import java.util.ArrayList;
// import java.util.HashMap;
// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
// import edu.wpi.first.wpilibj2.command.Command;
// import edu.wpi.first.wpilibj2.command.InstantCommand;
// import frc4388.robot.commands.Swerve.JoystickPlayback;
// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.utility.controller.VirtualController;
// public class neoPlaybackChooser {
// private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
// private final SwerveDrive m_swerve;
// private final VirtualController[] m_controllers;
// // private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
// // private SendableChooser<Command> m_playback = null;
// private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
// // private final HashMap<String, Command> m_commandPool = new HashMap<>();
// // private final File m_dir = new File("/home/lvuser/autos/");
// // private int m_cmdNum = 0;
// // commands
// private Command m_noAuto = new InstantCommand();
// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
// m_teamChosser.addOption("Red", "red");
// m_teamChosser.setDefaultOption("Blue", "blue");
// m_teamChosser.addOption("Nuetral", "nuetral");
// m_possitionChosser.addOption("AMP", "amp");
// m_possitionChosser.setDefaultOption("Center", "center");
// m_possitionChosser.addOption("Source", "source");
// m_swerve = swerve;
// m_controllers = controllers;
// }
// public neoPlaybackChooser addOption(String name, String option) {
// m_autoNameChosser.addOption(name, option);
// return this;
// }
// // public PlaybackChooser buildDisplay() {
// // for (int i = 0; i < 10; i++) {
// // appendCommand();
// // }
// // m_playback = m_choosers.get(0);
// // nextChooser();
// // // ! This does not work, why?
// // Shuffleboard.getTab("Auto Chooser")
// // .add("Add Sequence", new InstantCommand(() -> nextChooser()))
// // .withPosition(4, 0);
// // return this;
// // }
// // This will be bound to a button for the time being
// public void render() {
// // var chooser = new SendableChooser<Command>();
// // // chooser.setDefaultOption("No Auto", m_noAuto);
// // m_choosers.add(chooser);
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
// .add("Command: " + m_choosers.size(), chooser)
// .withSize(4, 1)
// .withPosition(0, m_choosers.size() - 1)
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
// .withWidget(BuiltInWidgets.kComboBoxChooser);
// m_widgets.add(widget);
// }
// // public void nextChooser() {
// // SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
// // String[] dirs = m_dir.list();
// // if(dirs != null){ // Fix funny error
// // for (String auto : dirs) {
// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
// // }
// // }
// // for (var cmd_name : m_commandPool.keySet()) {
// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
// // }
// // }
// public String autoName() {
// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
// }
// public Command getCommand() {
// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
// }
// }
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+42
View File
@@ -12,6 +12,11 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
/**
* The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s
* @author Zachary Wilke
*/
public class neoJoystickPlayback extends Command {
private final SwerveDrive swerve;
private final VirtualController[] controllers;
@@ -29,6 +34,14 @@ public class neoJoystickPlayback extends Command {
private byte m_numControllers = 0;
private short m_numFrames = -1;
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param shouldfree Unloads the auto on compleation or intruption.
* @param instantload Load the auto on object instantiation
*/
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
this.swerve = swerve;
this.filenameGetter = filenameGetter;
@@ -38,18 +51,44 @@ public class neoJoystickPlayback extends Command {
if (instantload) loadAuto();
addRequirements(this.swerve);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filename a String containing the name of the auto file you wish to playback.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param shouldfree unloads the auto on compleation or intruption.
* @param instantload load the auto on object instantiation
*/
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
this(swerve, () -> filename, controllers, shouldfree, instantload);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
*/
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
this(swerve, filenameGetter, controllers, true, false);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filename a String containing the name of the auto file you wish to playback.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
*/
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
this(swerve, () -> filename, controllers, true, false);
}
/**
* Load the auto file from disk into memory
* @return Returns true if loading was successful, else wise; return false
* @implNote if the auto is already loaded, it will return true.
*/
public boolean loadAuto() {
filename = filenameGetter.get();
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
@@ -101,6 +140,9 @@ public class neoJoystickPlayback extends Command {
}
}
/**
* Unloads the auto.
*/
public void unloadAuto() {
System.out.println("AUTOPLAYBACK: Auto unloaded");
frames.clear();
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+16
View File
@@ -13,6 +13,10 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.DeadbandedXboxController;
/**
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
* @author Zachary Wilke
*/
public class neoJoystickRecorder extends Command {
private final SwerveDrive swerve;
private final XboxController[] controllers;
@@ -21,6 +25,12 @@ public class neoJoystickRecorder extends Command {
private long startTime = -1;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
*/
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
this.swerve = swerve;
this.controllers = controllers;
@@ -30,6 +40,12 @@ public class neoJoystickRecorder extends Command {
addRequirements(this.swerve);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param filename a String containing the name of the auto file you wish to playback.
*/
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
this(swerve, controllers, () -> filename);
}
src/main/java/frc4388/robot/subsystems/Climber.java
+12 -6
View File
@@ -7,10 +7,12 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimbConstants;
import frc4388.robot.Constants.IntakeConstants;
//! 6.5C Scoring Criteria for Stage
@@ -21,6 +23,7 @@ public class Climber extends SubsystemBase {
public Climber(TalonFX climbMotor) {
this.climbMotor = climbMotor;
this.climbMotor.setInverted(true);
var slot0Configs = new Slot0Configs();
slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error
@@ -30,15 +33,17 @@ public class Climber extends SubsystemBase {
}
public void climbOut() {
PositionVoltage request = new PositionVoltage(0);
climbMotor.setControl(request.withPosition(-520));
//climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
//PositionVoltage request = new PositionVoltage(0);
//climbMotor.setControl(request.withPosition(-520));
climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED);
}
public void climbIn() {
PositionVoltage request = new PositionVoltage(-520);
climbMotor.setControl(request.withPosition(0));
// climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
//PositionVoltage request = new PositionVoltage(-520);
//climbMotor.setControl(request.withPosition(0));
climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
;
}
public void stopClimb() {
@@ -48,5 +53,6 @@ public class Climber extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue());
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+1 -1
View File
@@ -333,7 +333,7 @@ public class Intake extends SubsystemBase {
resetArmPosition();
SmartDashboard.putNumber("Pivot Position", getArmPos());
//SmartDashboard.putNumber("Pivot Position", getArmPos());
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
src/main/java/frc4388/robot/subsystems/Limelight.java
+2 -2
View File
@@ -55,8 +55,8 @@ public class Limelight extends SubsystemBase {
isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
SmartDashboard.putNumber("speakerDistance", distance);
//SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
//SmartDashboard.putNumber("speakerDistance", distance);
}
public Pose2d getPose() {
src/main/java/frc4388/robot/subsystems/Shooter.java
+1 -1
View File
@@ -99,7 +99,7 @@ public class Shooter extends SubsystemBase {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
//smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
// If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
// Else if the robot is not near the speaker, then set the speed back to idle.
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+2 -2
View File
@@ -81,7 +81,7 @@ public class SwerveDrive extends SubsystemBase {
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX(), gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
@@ -218,7 +218,7 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
// SmartDashboard.putNumber("Gyro", getGyroAngle());
}
public void shiftDown() {
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+28 -2
View File
@@ -4,26 +4,34 @@
package frc4388.robot.subsystems;
import javax.swing.text.StyleContext.SmallAttributeSet;
import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.SensorInitializationStrategy;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.configurable.ConfigurableDouble;
public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
private CANCoder encoder;
private int selfid;
private ConfigurableDouble offsetGetter;
private static int swerveId = 0;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
@@ -31,7 +39,9 @@ public class SwerveModule extends SubsystemBase {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.encoder = encoder;
this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
this.selfid = swerveId;
swerveId++;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
angleConfig.slot0.kP = swerveGains.kP;
angleConfig.slot0.kI = swerveGains.kI;
@@ -43,12 +53,23 @@ public class SwerveModule extends SubsystemBase {
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleConfig);
//encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
reset(0);
encoder.configMagnetOffset(offset);
driveMotor.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
}
@Override
public void periodic() {
//encoder.configMagnetOffset(offsetGetter.get());
//SmartDashboard.putString("Error Code: " + selfid, getstuff());
// SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
// SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
// SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
// SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
}
/**
* Get the drive motor of the SwerveModule
* @return the drive motor of the SwerveModule
@@ -158,4 +179,9 @@ public class SwerveModule extends SubsystemBase {
public double getVoltage() {
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
// public String getstuff() {
// encoder.getPosition();
// return "" + encoder.getLastError().value;
// }
}
src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java
-23
View File
@@ -1,23 +0,0 @@
package frc4388.utility.configurable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class ConfigurableDouble {
private double defualtValue;
private String name;
/**
* Creates an new ConfigurableDouble through Smart Dashboard.
* @param name the name of the Smart Dashboard key.
* @param defualtValue the initilization value
*/
public ConfigurableDouble(String name, double defualtValue) {
this.name = name;
this.defualtValue = defualtValue;
SmartDashboard.putNumber(name, defualtValue);
}
public double get() {
return SmartDashboard.getNumber(name, defualtValue);
}
}
src/main/java/frc4388/utility/controller/VirtualController.java
+35
View File
@@ -2,22 +2,39 @@ package frc4388.utility.controller;
import edu.wpi.first.wpilibj.GenericHID;
/**
* A virtual controller that can be bound like an standard controller.
* @author Zachary Wilke
*/
public class VirtualController extends GenericHID {
private short m_buttonStates = 0;
private short m_buttonStatesLastFrame = 0;
private double[] m_axes = new double[6];
private short[] m_pov = new short[1];
/**
* Create an virtual controller
* @param port virtual port (merely a formality).
*/
public VirtualController(int port) {
super(port);
}
/**
* Set the curent inputs to the new frames.
* @param axes joystick axes, (i.e. joysticks and triggers).
* @param buttonFlags the bit packed button states.
* @param pov the array of dpads.
*/
public void setFrame(double[] axes, short buttonFlags, short[] pov) {
m_axes = axes;
setOutputs(buttonFlags);
m_pov = pov;
}
/**
* Zero outs the controls.
*/
public void zeroControls() {
m_axes = new double[6];
m_buttonStates = 0;
@@ -25,6 +42,12 @@ public class VirtualController extends GenericHID {
m_pov = new short[1];
}
/**
* Gets the value of a bitflag from an int
* @param value int to search
* @param index index of bit
* @return if the bit is set
*/
public static boolean getFlag(int value, int index) {
return ((value & 1 << index) != 0);
}
@@ -90,6 +113,10 @@ public class VirtualController extends GenericHID {
Hopefully this isn't a problem */
}
/**
* Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}.
* this is an no-op overide.
*/
@Override
public void setOutput(int outputNumber, boolean value) {
// do not use
@@ -97,12 +124,20 @@ public class VirtualController extends GenericHID {
//m_buttonStates[outputNumber - 1] = value;
}
/**
* Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame}
*/
@Override
public void setOutputs(int value) {
m_buttonStatesLastFrame = m_buttonStates;
m_buttonStates = (short) value;
}
/**
* Why are you Setting rumble on an virtual controller?
* @param type the rumble type (even though it won't do anything)
* @param value the rumble strength (always multiplyed by 0.0)
*/
@Override
public void setRumble(RumbleType type, double value) {
System.out.println("Why are you Setting rumble on an virtual controller?");