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

Fix odometry to use meters as units

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-12 21:32:33 -07:00
parent 80868d1fd7
commit bff9793caf
2 changed files with 24 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1
View File
@@ -63,6 +63,7 @@ public final class Constants {
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
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 INCHES_PER_METER = 39.370;
}
public static final class IntakeConstants {
src/main/java/frc4388/robot/subsystems/Drive.java
+23 -7
View File
@@ -283,10 +283,9 @@ public class Drive extends SubsystemBase {
// e.printStackTrace(System.err);
}
m_odometry.update(Rotation2d.fromDegrees(getHeading()),
m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(),
m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition());
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
inchesToMeters(getDistanceInches(m_rightFrontMotor)));
}
/**
@@ -472,7 +471,6 @@ public class Drive extends SubsystemBase {
/**
* Returns the heading of the robot
*
* @return The robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
@@ -492,8 +490,8 @@ public class Drive extends SubsystemBase {
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds( m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(),
m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftFrontMotor)),
inchesToMeters(getVelocityInchesPerSecond(m_rightFrontMotor)));
}
/**
@@ -523,6 +521,15 @@ public class Drive extends SubsystemBase {
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
}
/**
* Gets the encoder value (velocity) of a motor
* @param falcon The motor to get the velocity of
* @return The velocity of the motor in inches per second
*/
public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()/DriveConstants.TICK_TIME_TO_SECONDS);
}
/**
* Converts a value in ticks to inches.
* @param ticks The value in ticks to convert
@@ -531,6 +538,15 @@ public class Drive extends SubsystemBase {
public double ticksToInches(double ticks) {
return ticks * DriveConstants.INCHES_PER_TICK;
}
/**
* Converts a value in inches to meters.
* @param inches The value in inches to convert
* @return The converted value in meters
*/
public double inchesToMeters(double inches) {
return inches / DriveConstants.INCHES_PER_METER;
}
/*
* Set to high or low gear based on boolean state, true = high, false = low