diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..9c09583 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 597ab9c..aa9be76 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -53,10 +53,10 @@ public final class Constants { public static final int GYRO_ID = 14; // ofsets are in degrees - 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; - public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; + 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; + public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index faefdd5..d2e1fde 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -91,14 +91,15 @@ public class RobotMap { rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); - leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); - rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast + NeutralMode mode = NeutralMode.Brake; + leftFrontSteerMotor.setNeutralMode(mode); + leftFrontWheelMotor.setNeutralMode(mode);//Coast + rightFrontSteerMotor.setNeutralMode(mode); + rightFrontWheelMotor.setNeutralMode(mode);//Coast + leftBackSteerMotor.setNeutralMode(mode); + leftBackWheelMotor.setNeutralMode(mode);//Coast + rightBackSteerMotor.setNeutralMode(mode); + rightBackWheelMotor.setNeutralMode(mode);//Coast leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index fdd7ed1..d9c7665 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -15,6 +15,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; @@ -49,9 +50,9 @@ public class SwerveModule extends SubsystemBase { angleMotor.configAllSettings(angleTalonFXConfiguration); TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - driveTalonFXConfiguration.slot0.kP = 0.15; + driveTalonFXConfiguration.slot0.kP = 0.1; driveTalonFXConfiguration.slot0.kI = 0.0; - driveTalonFXConfiguration.slot0.kD = 0.5; + driveTalonFXConfiguration.slot0.kD = 0.0; driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); @@ -72,8 +73,7 @@ public class SwerveModule extends SubsystemBase { */ public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = getAngle(); - - //SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); state = SwerveModuleState.optimize(desiredState, currentRotation); // Find the difference between our current rotational position + our new rotational position @@ -84,17 +84,23 @@ public class SwerveModule extends SubsystemBase { // Convert the CANCoder from it's position reading back to ticks double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient; double desiredTicks = currentTicks + deltaTicks; + if (!ignoreAngle){ angleMotor.set(TalonFXControlMode.Position, desiredTicks); } double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); +<<<<<<< Updated upstream // 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); 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()); +>>>>>>> Stashed changes } /** @@ -114,5 +120,10 @@ public class SwerveModule extends SubsystemBase { driveMotor.set(0); angleMotor.set(0); } + @Override + public void periodic(){ + Rotation2d currentRotation = getAngle(); + SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + } }