mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 00:37:59 -06:00
integrated gyro + odometry, trying to fix drift
This commit is contained in:
Abhi
@@ -30,7 +30,7 @@ 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);
|
||||
|
||||
|
||||
@@ -50,7 +50,7 @@ public class RobotContainer {
|
||||
() -> getDriverController().getLeftXAxis(),
|
||||
() -> getDriverController().getLeftYAxis(),
|
||||
() -> getDriverController().getRightXAxis(),
|
||||
false));
|
||||
true));
|
||||
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
|
||||
@@ -68,14 +68,16 @@ public class RobotContainer {
|
||||
/* Driver Buttons */
|
||||
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
|
||||
// .onFalse()
|
||||
|
||||
/* Operator Buttons */
|
||||
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
|
||||
/* Operator Buttons */
|
||||
// interrupt button
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand());
|
||||
|
||||
@@ -4,20 +4,28 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
|
||||
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,27 +34,32 @@ 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
|
||||
|
||||
/** 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};
|
||||
}
|
||||
@@ -54,11 +67,23 @@ public class SwerveDrive extends SubsystemBase {
|
||||
// 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())
|
||||
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, Rotation2d.fromDegrees(-gyro.getRotation2d().getDegrees()))
|
||||
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
|
||||
// );
|
||||
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
|
||||
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(0.3, 0, 0));
|
||||
|
||||
// // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
// modules[0].getDriveMotor().set(0.2);
|
||||
// modules[1].getDriveMotor().set(0.2);
|
||||
// modules[2].getDriveMotor().set(0.2);
|
||||
// modules[3].getDriveMotor().set(0.2);
|
||||
|
||||
// modules[0].getAngleMotor().set(TalonFXControlMode.Position, 1017);
|
||||
// modules[1].getAngleMotor().set(TalonFXControlMode.Position, 509);
|
||||
// modules[2].getAngleMotor().set(TalonFXControlMode.Position, 683);
|
||||
// modules[3].getAngleMotor().set(TalonFXControlMode.Position, 1816);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -74,62 +99,62 @@ public class SwerveDrive extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
setOdometry(getOdometry());
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 +163,27 @@ public class SwerveDrive extends SubsystemBase {
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
// updateOdometry();
|
||||
updateOdometry();
|
||||
|
||||
// SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()});
|
||||
|
||||
// SmartDashboard.putNumber("LF CC Angle", modules[0].getAngle().getDegrees());
|
||||
// SmartDashboard.putNumber("RF CC Angle", modules[1].getAngle().getDegrees());
|
||||
// SmartDashboard.putNumber("LB CC Angle", modules[2].getAngle().getDegrees());
|
||||
// SmartDashboard.putNumber("RB CC Angle", modules[3].getAngle().getDegrees());
|
||||
|
||||
SmartDashboard.putNumber("LF Vel", modules[0].getDriveVel());
|
||||
SmartDashboard.putNumber("RF Vel", modules[1].getDriveVel());
|
||||
SmartDashboard.putNumber("LB Vel", modules[2].getDriveVel());
|
||||
SmartDashboard.putNumber("RB Vel", modules[3].getDriveVel());
|
||||
|
||||
SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
|
||||
|
||||
SmartDashboard.putNumber("LF Pos", modules[0].getDrivePos());
|
||||
SmartDashboard.putNumber("RF Pos", modules[1].getDrivePos());
|
||||
SmartDashboard.putNumber("LB Pos", modules[2].getDrivePos());
|
||||
SmartDashboard.putNumber("RB Pos", modules[3].getDrivePos());
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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);
|
||||
@@ -117,19 +132,23 @@ public class SwerveModule extends SubsystemBase {
|
||||
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();
|
||||
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
|
||||
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) {
|
||||
|
||||
Reference in New Issue
Block a user