From 93f83986e5d51a7e9c91def40d4961e4ff0f4700 Mon Sep 17 00:00:00 2001 From: Ryan Manley Date: Tue, 18 Jan 2022 19:53:14 -0700 Subject: [PATCH] This has errors lol --- src/main/java/frc4388/robot/Constants.java | 3 + .../frc4388/robot/subsystems/SwerveDrive.java | 66 +++++++++---------- .../robot/subsystems/SwerveModule.java | 10 +++ 3 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 01b3b37..817d46d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -65,6 +65,9 @@ public final class Constants { public static final double OPEN_LOOP_RAMP_RATE = 0.2; public static final int REMOTE_0 = 0; + // conversions + // 5.12 rev motor = 1 rev wheel + // misc public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0c5df2c..61fe8f1 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -12,6 +12,9 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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; @@ -52,8 +55,20 @@ public class SwerveDrive extends SubsystemBase { // setSwerveGains(); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); + public SwerveModule[] modules; - public WPI_PigeonIMU m_gyro; //TODO Add Gyro Lol + public WPI_PigeonIMU m_gyro; + + /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used + below are robot specific, and should be tuned. */ + private final SwerveDrivePoseEstimator m_poseEstimator = + new SwerveDrivePoseEstimator( + m_gyro.getRotation2d(), + new Pose2d(), + m_kinematics, + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(Units.degreesToRadians(0.01)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; @@ -101,40 +116,7 @@ public class SwerveDrive extends SubsystemBase { else ignoreAngles = false; speeds[0] *= speeds[0] * speeds[0]; speeds[1] *= speeds[1] * speeds[1]; - // // Temporary janky raw joysticks - // float[] driveController = new float[HAL.kMaxJoystickAxes]; - // HAL.getJoystickAxes((byte) 0, driveController); - // // ByteBuffer buttons = new - // ByteBuffer num_buttons_buffer = ByteBuffer.allocateDirect(1); - // int buttons_int = HAL.getJoystickButtons((byte) 0, num_buttons_buffer); - // int num_buttons = num_buttons_buffer.get(0); - // boolean[] buttons = new boolean[num_buttons]; - // for (int i = 0; i < num_buttons; i++) { - // buttons[i] = (buttons_int & 1 << i) > 0; - // } - // if (buttons[0]) - // m_gyro.reset(); - // float leftXAxis = driveController[0]; - // float leftYAxis = driveController[1]; - // float rightXAxis = driveController[4]; - // leftXAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftXAxis; - // leftYAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftYAxis; - // rightXAxis = XboxController.inDeadZone(rightXAxis) ? 0.f : rightXAxis; - // leftXAxis *= leftXAxis * leftXAxis; - // leftYAxis *= leftYAxis * leftYAxis; - // rightXAxis *= rightXAxis * rightXAxis; - // double[] dashboardNums = new double[HAL.kMaxJoystickAxes]; - // for (int i = 0; i < HAL.kMaxJoystickAxes; i++) { - // dashboardNums[i] = (double)((int)(driveController[i] * 100.f)) / 100.d; - // } - // SmartDashboard.putNumberArray("axes", dashboardNums); - // speeds = XboxController.ClampJoystickAxis(leftXAxis, leftYAxis); - // rot = -rightXAxis; - - // System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]); - /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); - driveFromSpeeds(speeds);*/ double xSpeedMetersPerSecond = -speeds[0] * speedAdjust; double ySpeedMetersPerSecond = speeds[1] * speedAdjust; SwerveModuleState[] states = @@ -159,6 +141,22 @@ public class SwerveDrive extends SubsystemBase { super.periodic(); } + /** Updates the field relative position of the robot. */ + public void updateOdometry() { + m_poseEstimator.update( m_gyro.getRotation2d(), + m_frontLeftLocation.getState(), + m_frontRightLocation.getState(), + m_backLeftLocation.getState(), + m_backRightLocation.getState()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on + // a real robot, this must be calculated based either on latency or timestamps. + m_poseEstimator.addVisionMeasurement( + ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( + m_poseEstimator.getEstimatedPosition()), + Timer.getFPGATimestamp() - 0.3); + } + public void highSpeed(boolean shift){ if (shift){ speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 450668c..20aaa4b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -93,4 +93,14 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(driveMotor.getSelectedSensorVelocity(), new Rotation2d(canCoder.getPosition())); + } + }