diff --git a/build.gradle b/build.gradle index 283effa..ccc227f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,4 +1,4 @@ -import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO +import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" @@ -28,7 +28,7 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable visualvm - jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" + //jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" } // Static files artifact diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index aa9be76..e31c27d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d2e1fde..926cfc6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -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); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index c2af4e3..6ec5a2f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -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); } }