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

AngleMotor + DriveMotor correction

Browse Source
This commit is contained in:
Ryan Manley
2022-02-19 15:48:39 -07:00
parent 9530cbdaa7
commit 97ba1e132c
4 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -2
View File
@@ -53,6 +53,8 @@ public final class Constants {
public static final int GYRO_ID = 14;
// ofsets are in degrees
//NATHAN if you truncate or round or simplify these i will cry
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
@@ -79,10 +81,11 @@ public final class Constants {
public static final int REMOTE_0 = 0;
// conversions
// gear ratio: 5.12 rev motor = 1 rev wheel
// gear ratio: 5.14 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 MOTOR_REV_PER_STEER_REV = 12.8;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857;
public static final double WHEEL_DIAMETER_INCHES = 4.0;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
src/main/java/frc4388/robot/RobotMap.java
+1 -1
View File
@@ -91,7 +91,7 @@ public class RobotMap {
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
NeutralMode mode = NeutralMode.Brake;
NeutralMode mode = NeutralMode.Coast;
leftFrontSteerMotor.setNeutralMode(mode);
leftFrontWheelMotor.setNeutralMode(mode);//Coast
rightFrontSteerMotor.setNeutralMode(mode);
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+10 -6
View File
@@ -91,13 +91,16 @@ public class SwerveModule extends SubsystemBase {
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
// Add this line to correct for the slight drive movement the angle motor makes when turning in place.
// driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
// driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
// driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC) + angleMotor.get());
driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
//driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));// - angleMotor.get());
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
System.out.println(angleCorrection);
// angleMotor.set(1.0);
// driveMotor.set(0.6375);
}
/**
@@ -120,7 +123,8 @@ public class SwerveModule extends SubsystemBase {
@Override
public void periodic(){
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
}
}