Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #27 from Team4388/odometry

Browse Source 
Odometry --> vision-april-lime
This commit is contained in:
Aarav Shah
2023-02-10 16:50:33 -07:00
committed by GitHub
parent 9cda086c01 f5f704bcbe
commit dd8209d8c5
8 changed files with 227 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+13 -8
View File
@@ -24,7 +24,7 @@ import frc4388.utility.LEDPatterns;
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 2.0;
public static final double ROTATION_SPEED = -0.7;
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
@@ -65,13 +65,13 @@ public final class Constants {
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value
public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value
public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
public static final double WHEEL_DIAMETER_INCHES = 3.9;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
@@ -105,11 +105,11 @@ public final class Constants {
}
public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
// dimensions
public static final double WIDTH = 18.5; // TODO: find the actual value
public static final double HEIGHT = 18.5; // TODO: find the actual value
public static final double WIDTH = 18.5;
public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
@@ -137,5 +137,10 @@ public final class Constants {
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final double LEFT_AXIS_DEADBAND = 0.1;
public static final double RIGHT_AXIS_DEADBAND = 0.6;
public static final boolean SKEW_STICKS = true;
}
}
src/main/java/frc4388/robot/RobotContainer.java
+42 -32
View File
@@ -10,11 +10,12 @@ package frc4388.robot;
import edu.wpi.first.wpilibj.Joystick;
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.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.DriveWithInput;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
@@ -30,13 +31,16 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
// private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
// private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -46,15 +50,11 @@ public class RobotContainer {
/* Default Commands */
m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive,
() -> getDriverController().getLeftXAxis(),
() -> getDriverController().getLeftYAxis(),
() -> getDriverController().getRightXAxis(),
false));
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() ->
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true)
, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
}
@@ -67,18 +67,20 @@ public class RobotContainer {
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
// .onFalse()
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
// // .onFalse()
/* Operator Buttons */
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// interrupt button
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.onTrue(new InstantCommand());
// /* Operator Buttons */
// // interrupt button
// new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand());
}
/**
@@ -94,28 +96,36 @@ public class RobotContainer {
/**
* Add your docs here.
*/
public IHandController getDriverController() {
return m_driverXbox;
// public IHandController getDriverController() {
// return m_driverXbox;
// }
public DeadbandedXboxController getDeadbandedDriverController() {
return this.m_driverXbox;
}
public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox;
}
/**
* Add your docs here.
*/
public IHandController getOperatorController() {
return m_operatorXbox;
}
// public IHandController getOperatorController() {
// return m_operatorXbox;
// }
/**
* Add your docs here.
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
}
// public Joystick getOperatorJoystick() {
// return m_operatorXbox.getJoyStick();
// }
/**
* Add your docs here.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
}
// public Joystick getDriverJoystick() {
// return m_driverXbox.getJoyStick();
// }
}
src/main/java/frc4388/robot/RobotMap.java
+7
View File
@@ -7,6 +7,7 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
@@ -113,6 +114,12 @@ public class RobotMap {
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// set neutral mode
leftFrontSteer.setNeutralMode(NeutralMode.Brake);
rightFrontSteer.setNeutralMode(NeutralMode.Brake);
leftBackSteer.setNeutralMode(NeutralMode.Brake);
rightBackSteer.setNeutralMode(NeutralMode.Brake);
// initialize SwerveModules
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
src/main/java/frc4388/robot/commands/AutoBalance.java
+3 -2
View File
@@ -5,6 +5,7 @@
package frc4388.robot.commands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.RobotGyro;
@@ -15,7 +16,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
/** Creates a new AutoBalanceTF2. */
public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
super(1.0, 0, 0, 0);
super(0.6, 0, 0, 0);
this.gyro = gyro;
this.drive = drive;
@@ -34,7 +35,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
public void runWithOutput(double output) {
double out2 = MathUtil.clamp(output / 40, -.5, .5);
if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
drive.drive(out2, 0, 0, false);
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
}
@Override
src/main/java/frc4388/robot/commands/DriveWithInput.java
-75
View File
@@ -1,75 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import java.util.function.Supplier;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveDrive;
public class DriveWithInput extends CommandBase {
private final SwerveDrive swerve;
private final Supplier<Double> xSpeed;
private final Supplier<Double> ySpeed;
private final Supplier<Double> rot;
private final boolean fieldRelative;
private final SlewRateLimiter xLimiter, yLimiter, rotLimiter;
/** Creates a new DriveWithInput. */
public DriveWithInput(SwerveDrive swerve, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rot, boolean fieldRelative) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.xSpeed = xSpeed;
this.ySpeed = ySpeed;
this.rot = rot;
this.fieldRelative = fieldRelative;
this.xLimiter = new SlewRateLimiter(3);
this.yLimiter = new SlewRateLimiter(3);
this.rotLimiter = new SlewRateLimiter(3);
addRequirements(swerve);
}
// 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() {
double x = xSpeed.get();
double y = ySpeed.get();
double r = rot.get();
x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI));
swerve.drive(x, y, r, fieldRelative);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
System.out.println("----------------------------------------------------------------");
System.out.println("DriveWithInput ended");
System.out.println("Interrupted: " + interrupted);
System.out.println("----------------------------------------------------------------");
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+106 -66
View File
@@ -4,20 +4,27 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
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.RobotGyro;
public class SwerveDrive extends SubsystemBase {
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
private SwerveModule[] modules;
@@ -26,39 +33,64 @@ public class SwerveDrive extends SubsystemBase {
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
// private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
// kinematics,
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
private RobotGyro gyro;
private SwerveDriveOdometry odometry;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public Rotation2d rotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) {
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.gyro = gyro;
this.odometry = new SwerveDriveOdometry(
kinematics,
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
}
);
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
// WPILib swerve drive example
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
// );
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
if (rightStick.getNorm() > 0.1) {
rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
}
double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// if (rightStick.getNorm() < .1) {
// rot = 0;
// }
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
@@ -66,70 +98,71 @@ public class SwerveDrive extends SubsystemBase {
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
module.setDesiredState(state, false);
}
}
public double getGyroAngle() {
return gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
setOdometry(getOdometry());
rotTarget = new Rotation2d(0);
}
/**
* Updates the odometry of the SwerveDrive.
*/
// public void updateOdometry() {
// odometry.update(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
// }
public void updateOdometry() {
odometry.update(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
}
);
}
/**
* Gets the odometry of the SwerveDrive.
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
*/
// public Pose2d getOdometry() {
// return odometry.getPoseMeters();
// }
public Pose2d getOdometry() {
return odometry.getPoseMeters();
}
/**
* Sets the odometry of the SwerveDrive.
* @param pose Pose to set the odometry to.
*/
// public void setOdometry(Pose2d pose) {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// pose
// );
// }
public void setOdometry(Pose2d pose) {
odometry.resetPosition(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
},
pose
);
}
/**
* Resets the odometry of the SwerveDrive to 0.
* *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
* *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
*/
// public void resetOdometry() {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// new Pose2d()
// );
// }
public void resetOdometry() {
setOdometry(new Pose2d());
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
@@ -138,7 +171,14 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
// updateOdometry();
updateOdometry();
SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
}
/**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+29 -7
View File
@@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
public WPI_TalonFX driveMotor;
public WPI_TalonFX angleMotor;
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
private CANCoder encoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -44,6 +44,9 @@ public class SwerveModule extends SubsystemBase {
angleMotor.configAllSettings(angleConfig);
encoder.configMagnetOffset(offset);
driveMotor.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
}
/**
@@ -79,6 +82,18 @@ public class SwerveModule extends SubsystemBase {
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
}
public double getAngularVel() {
return this.angleMotor.getSelectedSensorVelocity();
}
public double getDrivePos() {
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
}
public double getDriveVel() {
return this.driveMotor.getSelectedSensorVelocity(0);
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
@@ -111,25 +126,32 @@ public class SwerveModule extends SubsystemBase {
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
*/
public void setDesiredState(SwerveModuleState desiredState) {
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative
// calculate the new absolute position of the SwerveModule based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
// convert the CANCoder from its position reading to ticks
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
}
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
// double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12;
driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
// driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
// driveMotor.set(0.1);
// double angleCorrection = getAngularVel() * 2.69;
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
}
public void reset(double position) {
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+27
View File
@@ -0,0 +1,27 @@
package frc4388.utility.controller;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import frc4388.robot.Constants.OIConstants;
public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); }
@Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
}