mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
Merge branch 'master' into 2024x2025
This commit is contained in:
C4llSiqn
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Example Path"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.415049342105263,
|
||||
"y": 4.785115131578947
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.905756578947368,
|
||||
"y": 4.794736842105262
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.8294407894736837,
|
||||
"y": 5.862746710526316
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.0693256578947365,
|
||||
"y": 5.872368421052631
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -2.4366482468102095
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
{
|
||||
"robotWidth": 0.9,
|
||||
"robotLength": 0.9,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 3.0,
|
||||
"defaultMaxAccel": 3.0,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 22.6796,
|
||||
"robotMOI": 6.883,
|
||||
"robotTrackwidth": 0.546,
|
||||
"driveWheelRadius": 0.048,
|
||||
"driveGearing": 5.143,
|
||||
"maxDriveSpeed": 5.45,
|
||||
"driveMotorType": "krakenX60",
|
||||
"driveCurrentLimit": 60.0,
|
||||
"wheelCOF": 1.2,
|
||||
"flModuleX": 0.273,
|
||||
"flModuleY": 0.273,
|
||||
"frModuleX": 0.273,
|
||||
"frModuleY": -0.273,
|
||||
"blModuleX": -0.273,
|
||||
"blModuleY": 0.273,
|
||||
"brModuleX": -0.273,
|
||||
"brModuleY": -0.273,
|
||||
"bumperOffsetX": 0.0,
|
||||
"bumperOffsetY": 0.0,
|
||||
"robotFeatures": []
|
||||
}
|
||||
@@ -102,51 +102,76 @@ public final class Constants {
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
|
||||
// public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
|
||||
|
||||
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
||||
|
||||
private static final class ModuleSpecificConstants {
|
||||
private static final class ModuleSpecificConstants { //2025
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
}
|
||||
|
||||
/* private static final class ModuleSpecificConstants { // 2024
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
|
||||
// private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
|
||||
// private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
|
||||
// private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
// private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
|
||||
// private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
// private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
|
||||
// private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
// private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
}
|
||||
} */
|
||||
|
||||
public static final class IDs {
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
|
||||
|
||||
@@ -28,13 +28,14 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
|
||||
// Autos
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.GotoPositionCommand;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
// Subsystems
|
||||
// import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
@@ -156,26 +157,6 @@ public class RobotContainer {
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
// @ /* Trim Test Buttons */
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp()));
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown()));
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load()));
|
||||
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
|
||||
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
@@ -248,9 +229,21 @@ public class RobotContainer {
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
//return autoPlayback;
|
||||
return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
||||
try{
|
||||
// Load the path you want to follow using its name in the GUI
|
||||
return new PathPlannerAuto("New Auto");
|
||||
} catch (Exception e) {
|
||||
DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
||||
return Commands.none();
|
||||
}
|
||||
// zach told me to do the below comment
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
||||
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
|
||||
* @param joystickA A controller
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.Optional;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
@@ -30,258 +31,304 @@ import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
|
||||
public class SwerveDrive extends Subsystem {
|
||||
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
private Vision vision;
|
||||
private Vision vision;
|
||||
|
||||
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
private boolean stopped = false;
|
||||
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
private boolean stopped = false;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25%
|
||||
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25%
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
super();
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
super();
|
||||
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
this.vision = vision;
|
||||
}
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
this.vision = vision;
|
||||
|
||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
|
||||
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// // rightStick.getAngle()
|
||||
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
|
||||
// // System.out.println(ang);
|
||||
// // module.go(ang);
|
||||
// // Rotation2d rot = Rotation2d.fromRadians(ang);
|
||||
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
|
||||
// SwerveModuleState state = new SwerveModuleState(speed, rot);
|
||||
// module.setDesiredState(state);
|
||||
// }
|
||||
RobotConfig config;
|
||||
try{
|
||||
config = RobotConfig.fromGUISettings();
|
||||
} catch (Exception e) {
|
||||
// Handle exception as needed
|
||||
config = null;
|
||||
}
|
||||
// DoubleSupplier a = () -> 1.d;
|
||||
AutoBuilder.configure(
|
||||
() -> {
|
||||
var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
|
||||
// pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
|
||||
return pose;//getRotation().times(-1)
|
||||
}, // Robot pose supplier
|
||||
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
|
||||
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
|
||||
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
|
||||
.withSpeeds(speeds)
|
||||
), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
|
||||
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
|
||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
|
||||
),
|
||||
config, // The robot configuration
|
||||
() -> {
|
||||
// Boolean supplier that controls when the path will be mirrored for the red alliance
|
||||
// This will flip the path being followed to the red side of the field.
|
||||
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
var alliance = DriverStation.getAlliance();
|
||||
if (alliance.isPresent()) {
|
||||
return alliance.get() == DriverStation.Alliance.Red;
|
||||
}
|
||||
return false;
|
||||
},
|
||||
this // Reference to this subsystem to set requirements
|
||||
);
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
}
|
||||
|
||||
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
|
||||
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// // rightStick.getAngle()
|
||||
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
|
||||
// // System.out.println(ang);
|
||||
// // module.go(ang);
|
||||
// // Rotation2d rot = Rotation2d.fromRadians(ang);
|
||||
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
|
||||
// SwerveModuleState state = new SwerveModuleState(speed, rot);
|
||||
// module.setDesiredState(state);
|
||||
// }
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
|
||||
if (fieldRelative) {
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(leftStick.getX()*-speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withVelocityX(leftStick.getX()*speedAdjust)
|
||||
.withVelocityY(-leftStick.getY()*speedAdjust)
|
||||
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
|
||||
);
|
||||
// double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
// dependant on if the new odomitry system acounts for rotational drift, this may not be needed.
|
||||
// if (rightStick.getNorm() > 0.05) {
|
||||
// rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
|
||||
// swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
// .withVelocityX(leftStick.getX()*speedAdjust)
|
||||
// .withVelocityY(leftStick.getY()*speedAdjust)
|
||||
// .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
|
||||
// );
|
||||
// ! drift correction
|
||||
// dependant on if the new odomitry system acounts for rotational drift, this may not be needed.
|
||||
// if (rightStick.getNorm() > 0.05) {
|
||||
// rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
|
||||
// swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
// .withVelocityX(leftStick.getX()*speedAdjust)
|
||||
// .withVelocityY(leftStick.getY()*speedAdjust)
|
||||
// .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
|
||||
// );
|
||||
|
||||
// // SmartDashboard.putBoolean("drift correction", false);
|
||||
// stopped = false;
|
||||
// } else if(leftStick.getNorm() > 0.05) {
|
||||
// if (!stopped) {
|
||||
// stopModules();
|
||||
// stopped = true;
|
||||
// }
|
||||
// // SmartDashboard.putBoolean("drift correction", false);
|
||||
// stopped = false;
|
||||
// } else if(leftStick.getNorm() > 0.05) {
|
||||
// if (!stopped) {
|
||||
// stopModules();
|
||||
// stopped = true;
|
||||
// }
|
||||
|
||||
// // SmartDashboard.putBoolean("drift correction", true);
|
||||
// // SmartDashboard.putBoolean("drift correction", true);
|
||||
|
||||
// // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
// // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
// }
|
||||
// }
|
||||
|
||||
// // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
// // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
// // 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.
|
||||
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX()*-speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX()*speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withTargetDirection(rightStick.getAngle())
|
||||
);
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(angle))
|
||||
);
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
// // Convert field-relative speeds to robot-relative speeds.
|
||||
// // chassisSpeeds = chassisSpeeds.
|
||||
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX()*speedAdjust)
|
||||
.withVelocityY(-leftStick.getY()*speedAdjust)
|
||||
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
// stopModules(); // stop the swerve
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
// if (leftStick.getNorm() < 0.05) //if no imput
|
||||
// return; // don't bother doing swerve drive math and return early.
|
||||
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
|
||||
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX()*speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withTargetDirection(rightStick.getAngle())
|
||||
);
|
||||
}
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX()*-speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withTargetDirection(rot)
|
||||
);
|
||||
// double
|
||||
}
|
||||
public boolean rotateToTarget(double angle) {
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(angle))
|
||||
);
|
||||
|
||||
public double getGyroAngle() {
|
||||
return swerveDriveTrain.getRotation3d().getAngle();
|
||||
}
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
swerveDriveTrain.tareEverything();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
// stopModules(); // stop the swerve
|
||||
|
||||
// if (leftStick.getNorm() < 0.05) //if no imput
|
||||
// return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX()*-speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withTargetDirection(rot)
|
||||
);
|
||||
// double
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return swerveDriveTrain.getRotation3d().getAngle();
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
swerveDriveTrain.tareEverything();
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
double time = Vision.getTime();
|
||||
double time = Vision.getTime();
|
||||
|
||||
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
|
||||
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
|
||||
|
||||
if(vision.isTag()){
|
||||
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
|
||||
if(vision.isTag()){
|
||||
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
}
|
||||
private void reset_index() {
|
||||
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?)
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?)
|
||||
}
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1) i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1) i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
public void setToSlow() {
|
||||
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
|
||||
gear_index = 0;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
|
||||
gear_index = 0;
|
||||
}
|
||||
public void setToFast() {
|
||||
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
|
||||
gear_index = 1;
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
|
||||
gear_index = 1;
|
||||
}
|
||||
public void setToTurbo() {
|
||||
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
|
||||
gear_index = 2;
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
|
||||
gear_index = 2;
|
||||
}
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
GenericEntry sbGyro = subsystemLayout
|
||||
.add("Gyro angle", 0)
|
||||
.withWidget(BuiltInWidgets.kGyro)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbGyro = subsystemLayout
|
||||
.add("Gyro angle", 0)
|
||||
.withWidget(BuiltInWidgets.kGyro)
|
||||
.getEntry();
|
||||
GenericEntry sbShiftState = subsystemLayout
|
||||
.add("Shift State", 0)
|
||||
.withWidget(BuiltInWidgets.kNumberBar)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbShiftState = subsystemLayout
|
||||
.add("Shift State", 0)
|
||||
.withWidget(BuiltInWidgets.kNumberBar)
|
||||
.getEntry();
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
sbGyro.setDouble(getGyroAngle());
|
||||
sbShiftState.setDouble(this.speedAdjust);
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
sbGyro.setDouble(getGyroAngle());
|
||||
sbShiftState.setDouble(this.speedAdjust);
|
||||
//TODO: Add more status things
|
||||
}
|
||||
|
||||
//TODO: Add more status things
|
||||
}
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
|
||||
|
||||
status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
|
||||
|
||||
return status;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user