Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

constants reworked (needs testing, which will be done with odometry testing)

Browse Source
This commit is contained in:
aarav18
2022-01-20 19:00:42 -07:00
parent 93f83986e5
commit bfa044e340
3 changed files with 36 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+19 -6
View File
@@ -30,7 +30,8 @@ public final class Constants {
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
//IDs
//IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
@@ -44,11 +45,8 @@ public final class Constants {
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int GYRO_ID = 14;
// ofsets are in degrees
// public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
// public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
@@ -66,7 +64,22 @@ public final class Constants {
public static final int REMOTE_0 = 0;
// conversions
// 5.12 rev motor = 1 rev wheel
// gear ratio: 5.12 rev motor = 1 rev wheel
// wheel diameter: official = 4 in, measured = 3.8 in
/* Ratio Calculation */
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double WHEEL_DIAMETER_INCHES = 3.8;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double INCHES_PER_METER = 39.370;
public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
public static final double WHEEL_REV_PER_MOTOR_REV = 1/MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 0.1;
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+16 -9
View File
@@ -22,6 +22,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -52,6 +53,12 @@ public class SwerveDrive extends SubsystemBase {
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
SwerveModule m_frontLeft = new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
SwerveModule m_frontRight = new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
SwerveModule m_backLeft = new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
SwerveModule m_backRight = new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
// setSwerveGains();
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
@@ -94,10 +101,10 @@ public class SwerveDrive extends SubsystemBase {
m_gyro = gyro;
modules = new SwerveModule[] {
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
m_frontLeft, // Front Left
m_frontRight, // Front Right
m_backLeft, // Back Left
m_backRight // Back Right
};
m_gyro.reset();
}
@@ -144,17 +151,17 @@ public class SwerveDrive extends SubsystemBase {
/** 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());
m_frontLeft.getState(),
m_frontRight.getState(),
m_backLeft.getState(),
m_backRight.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);
Timer.getFPGATimestamp() - 0.1);
}
public void highSpeed(boolean shift){
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+1 -1
View File
@@ -100,7 +100,7 @@ public class SwerveModule extends SubsystemBase {
* @return The current state of the module.
*/
public SwerveModuleState getState() {
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity(), new Rotation2d(canCoder.getPosition()));
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, new Rotation2d(canCoder.getPosition()));
}
}