diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 58513e1..b6c4823 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -79,7 +79,7 @@ public final class Constants { } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; + public static final int CANCODER_TICKS_PER_ROTATION = 1; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; @@ -87,7 +87,7 @@ public final class Constants { 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 TICKS_PER_MOTOR_REV = 2048; + public static final double TICKS_PER_MOTOR_REV = 0.5; public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2af9581..570e650 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -178,7 +178,7 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); /* Default Commands */ // drives the robot with a two-axis input from the driver controller diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 3619e3c..5377415 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -10,6 +10,8 @@ import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -21,6 +23,7 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; // import com.ctre.phoenix.sensors.SensorInitializationStrategy; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -83,9 +86,11 @@ public class SwerveModule extends SubsystemBase { encoder.getConfigurator().apply(coder_cfg); reset(0); // encoder.configMagnetOffset(offset); - driveMotor. - driveMotor.setSelectedSensorPosition(0); - driveMotor.config_kP(0, 0.2); + // driveMotor. + driveMotor.setPosition(0); + driveMotor.getConfigurator().apply(new Slot0Configs().withKP(0.2)); + // driveMotor.setSelectedSensorPosition(0); + // driveMotor.config_kP(0, 0.2); } @Override @@ -153,7 +158,8 @@ public class SwerveModule extends SubsystemBase { } public void rotateToAngle(double angle) { - angleMotor.set(TalonFXControlMode.Position, angle); + angleMotor.setControl(new PositionVoltage(angle)); + // angleMotor.set(TalonFXControlMode.Position, angle); } /** @@ -162,7 +168,8 @@ public class SwerveModule extends SubsystemBase { */ public SwerveModuleState getState() { return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + Units.inchesToMeters(driveMotor.getVelocity().getValue() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + // Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, getAngle() ); } @@ -172,7 +179,7 @@ public class SwerveModule extends SubsystemBase { * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. */ public SwerveModulePosition getPosition() { - return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getPosition().getValue() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); } /** @@ -181,19 +188,29 @@ public class SwerveModule extends SubsystemBase { */ public void setDesiredState(SwerveModuleState desiredState) { Rotation2d currentRotation = this.getAngle(); - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); + // Rotation2d rotationDelta = state.angle.minus(currentRotation); + + // rotationDelta.getDegrees(); // calculate the new absolute position of the SwerveModule based on the difference in rotation - double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + // (new CANCoder(13)).getPosition() - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + // double currentTicks = encoder.getPosition().getValue() / encoder.configGetFeedbackCoefficient(); + + // double currentTicks = encoder.getPosition().getValue() / 0.087890625; + + // angleMotor.setControl(new PositionVoltage(currentTicks + deltaTicks)); + + System.out.println(state.angle.getDegrees()); + angleMotor.setControl(new PositionVoltage(state.angle.getDegrees())); + // angleMotor.setControl(new PositionVoltage(0)); + // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); @@ -201,15 +218,19 @@ public class SwerveModule extends SubsystemBase { } public void reset(double position) { - encoder.setPositionToAbsolute(); + encoder.setPosition(encoder.getAbsolutePosition().getValue()); + // (new CANCoder(13)).setPositionToAbsolute(); } public double getCurrent() { - return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + // angleMotor.getSupplyVoltage() + // return angleMotor.getMotorVoltage().getValue() + driveMotor.getMotorVoltage().getValue(); + return angleMotor.getSupplyCurrent().getValue() + driveMotor.getSupplyCurrent().getValue(); } public double getVoltage() { - return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + return Math.abs(angleMotor.getMotorVoltage().getValue()) + Math.abs(driveMotor.getMotorVoltage().getValue()); + // return (Math.abs((new WPI_TalonFX(1).getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); } // public String getstuff() {