diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..b6681c9 --- /dev/null +++ b/readme.md @@ -0,0 +1 @@ +# I don't know what to put here \ 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 03327da..f7d5575 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,10 +42,15 @@ public final class Constants { public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // ofsets are in degrees - public static final float LEFT_FRONT_ENCODER_OFFSET = 0; - public static final float RIGHT_FRONT_ENCODER_OFFSET = 0; - public static final float LEFT_BACK_ENCODER_OFFSET = 0; - public static final float RIGHT_BACK_ENCODER_OFFSET = 0; + //ofsets are in degrees + // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; + // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; + // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531; + // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594; + 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; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -62,6 +67,16 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } + public static final class SerializerConstants { + public static final double SERIALIZER_BELT_SPEED = 1.0; // TODO (currently max power, DO NOT RUN) + public static final double SERIALIZER_SHOOTER_BELT_SPEED = 1.0; // TODO (currently max power, DO NOT RUN) + + // CAN IDs + public static final int SERIALIZER_BELT = -1; // TODO + public static final int SERIALIZER_SHOOTER_BELT = -1; // TODO + public static final int SERIALIZER_BELT_BEAM = -1; // TODO + } + public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 85b3bf4..362c955 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Serializer; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; @@ -41,6 +42,8 @@ public class RobotContainer { ); private final Intake m_robotIntake = new Intake(null, null); + private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt); + private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ @@ -56,8 +59,8 @@ public class RobotContainer { /* Default Commands */ // drives the swerve drive with a two-axis input from the driver controller m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(), - getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), + getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( @@ -88,6 +91,10 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam())) + .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam())); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 843323c..250c8cb 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,6 +9,7 @@ import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.SerializerConstants; import frc4388.robot.Constants.SwerveDriveConstants; /** @@ -92,4 +93,8 @@ public class RobotMap { //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); } + /* Serializer Subsystem */ + + public final Spark serializerBelt = new Spark(SerializerConstants.SERIALIZER_BELT); + public final Spark serializerShooterBelt = new Spark(SerializerConstants.SERIALIZER_SHOOTER_BELT); } diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java new file mode 100644 index 0000000..06815e8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -0,0 +1,48 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; +import edu.wpi.first.wpilibj.DigitalInput; + +public class Serializer extends SubsystemBase{ + private Spark m_serializerBelt; + private Spark m_serializerShooterBelt; + private DigitalInput m_serializerBeam; + private boolean serializerState; + + public Serializer(Spark serializerBelt, Spark serializerShooterBelt) { + m_serializerBelt = serializerBelt; + m_serializerShooterBelt = serializerShooterBelt; + m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM); + + serializerState = false; + setSerializerState(serializerState); + } + public boolean getBeam() { + return m_serializerBeam.get(); + } + public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) { + boolean total = ctrlbutter || beambroken; + setSerializerState(total); + } + public void setSerializerState(boolean state) { + setSerializerBeltState(state); + setSerializerShooterBeltState(state); + serializerState = state; + } + + public void setSerializerBeltState(boolean state) { + double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d; + m_serializerBelt.set(serializerBeltSpeed); + } + + public void setSerializerShooterBeltState(boolean state) { + double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d; + m_serializerShooterBelt.set(serializerShooterBeltSpeed); + } + + public boolean getSerializerState() { + return serializerState; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 18b7b4a..93cffc9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -14,9 +14,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { SwerveDriveKinematics m_kinematics; @@ -28,147 +30,157 @@ public class SwerveDrive extends SubsystemBase { private WPI_TalonFX m_leftBackWheelMotor; private WPI_TalonFX m_rightBackSteerMotor; private WPI_TalonFX m_rightBackWheelMotor; - private CANCoder m_leftFrontEncoder; + private CANCoder m_leftFrontEncoder; private CANCoder m_rightFrontEncoder; private CANCoder m_leftBackEncoder; - private CANCoder m_rightBackEncoder; + private CANCoder m_rightBackEncoder; double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth)); - Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); - // setSwerveGains(); + Translation2d m_frontLeftLocation = + new Translation2d( + Units.inchesToMeters(halfHeight), + Units.inchesToMeters(halfWidth)); + Translation2d m_frontRightLocation = + new Translation2d( + Units.inchesToMeters(halfHeight), + Units.inchesToMeters(-halfWidth)); + Translation2d m_backLeftLocation = + new Translation2d( + Units.inchesToMeters(-halfHeight), + Units.inchesToMeters(halfWidth)); + Translation2d m_backRightLocation = + new Translation2d( + Units.inchesToMeters(-halfHeight), + Units.inchesToMeters(-halfWidth)); + //setSwerveGains(); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; - public Gyro gyro; // TODO Add Gyro Lol + public RobotGyro gyro; //TODO Add Gyro Lol - public SwerveDrive(WPI_TalonFX leftFrontSteerMotor, WPI_TalonFX leftFrontWheelMotor, - WPI_TalonFX rightFrontSteerMotor, WPI_TalonFX rightFrontWheelMotor, - WPI_TalonFX leftBackSteerMotor, WPI_TalonFX leftBackWheelMotor, - WPI_TalonFX rightBackSteerMotor, WPI_TalonFX rightBackWheelMotor, - CANCoder leftFrontEncoder, CANCoder rightFrontEncoder, - CANCoder leftBackEncoder, CANCoder rightBackEncoder) { - m_leftFrontSteerMotor = leftFrontSteerMotor; - m_leftFrontWheelMotor = leftFrontWheelMotor; - m_rightFrontSteerMotor = rightFrontSteerMotor; - m_rightFrontWheelMotor = rightFrontWheelMotor; - m_leftBackSteerMotor = leftBackSteerMotor; - m_leftBackWheelMotor = leftBackWheelMotor; - m_rightBackSteerMotor = rightBackSteerMotor; - m_rightBackWheelMotor = rightBackWheelMotor; - m_leftFrontEncoder = leftFrontEncoder; - m_rightFrontEncoder = rightFrontEncoder; - m_leftBackEncoder = leftBackEncoder; - m_rightBackEncoder = rightBackEncoder; - modules = new SwerveModule[] { - new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left - new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right - new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left - new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right - }; - // gyro.reset(); + public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor, + WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder, + CANCoder rightFrontEncoder, + CANCoder leftBackEncoder, + CANCoder rightBackEncoder) + { + m_leftFrontSteerMotor = leftFrontSteerMotor; + m_leftFrontWheelMotor = leftFrontWheelMotor; + m_rightFrontSteerMotor = rightFrontSteerMotor; + m_rightFrontWheelMotor = rightFrontWheelMotor; + m_leftBackSteerMotor = leftBackSteerMotor; + m_leftBackWheelMotor = leftBackWheelMotor; + m_rightBackSteerMotor = rightBackSteerMotor; + m_rightBackWheelMotor = rightBackWheelMotor; + m_leftFrontEncoder = leftFrontEncoder; + m_rightFrontEncoder = rightFrontEncoder; + m_leftBackEncoder = leftBackEncoder; + m_rightBackEncoder = rightBackEncoder; + + modules = new SwerveModule[] { + new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left + new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right + new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left + new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right + }; + //gyro.reset(); + } +//https://github.com/ZachOrr/MK3-Swerve-Example + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) + { + /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); + driveFromSpeeds(speeds);*/ + double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; + double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; + SwerveModuleState[] states = + kinematics.toSwerveModuleStates( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3)); + SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + for (int i = 0; i < states.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = states[i]; + module.setDesiredState(state); + } + } + //Converts a ChassisSpeed to SwerveModuleStates (targets) + public void driveFromSpeeds(ChassisSpeeds speeds) + { + //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html + // Convert to module states + SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); + + // Front left module state + SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition())); + // Front right module state + SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition())); + // Back left module state + SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition())); + // Back right module state + SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition())); + + //Set the motors + setSwerveMotors(leftFront, leftBack, rightFront, rightBack); } - // https://github.com/ZachOrr/MK3-Swerve-Example - /** - * Method to drive the robot using joystick info. - * - * @param xSpeed Speed of the robot in the x direction (forward). - * @param ySpeed Speed of the robot in the y direction (sideways). - * @param rot Angular rate of the robot. - * @param fieldRelative Whether the provided x and y speeds are relative to the - * field. - */ - public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - - // var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); - // driveFromSpeeds(speeds); - - double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - SwerveModuleState[] states = kinematics.toSwerveModuleStates( - fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); - SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); - for (int i = 0; i < states.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = states[i]; - module.setDesiredState(state); - } - } - - // Converts a ChassisSpeed to SwerveModuleStates (targets) - public void driveFromSpeeds(ChassisSpeeds speeds) { - // https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - // Convert to module states - SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); - - // Front left module state - SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition())); - // Front right module state - SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition())); - // Back left module state - SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition())); - // Back right module state - SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition())); - - // Set the motors - setSwerveMotors(leftFront, leftBack, rightFront, rightBack); - } - //Sets steering motors to PID values + //Sets steering motors to PID values public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack) - {/* - //Set the Wheel motor speeds - m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - - //PID - m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000); - m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000); - m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000); - m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees()); - - System.out.println("Target: " + leftFront.angle.getDegrees()); - */} + { + /*//Set the Wheel motor speeds + m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); + m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); + m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); + m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); + //PID + m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000); + m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000); + m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000); + m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees()); + System.out.println("Target: " + leftFront.angle.getDegrees());*/ + } /*public void setSwerveGains(){ - m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + }*/ // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate) // { - // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) + // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, + // rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) // } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 7754ef3..e9df20a 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; @@ -27,8 +28,10 @@ public class SwerveModule extends SubsystemBase { private static double kEncoderTicksPerRotation = 4096; + + /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) { + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.canCoder = canCoder; @@ -45,35 +48,32 @@ public class SwerveModule extends SubsystemBase { angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleTalonFXConfiguration); - /* - * TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - * - * driveTalonFXConfiguration.slot0.kP = kDriveP; - * driveTalonFXConfiguration.slot0.kI = kDriveI; - * driveTalonFXConfiguration.slot0.kD = kDriveD; - * driveTalonFXConfiguration.slot0.kF = kDriveF; - * - * driveMotor.configAllSettings(driveTalonFXConfiguration); - */ + /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); + driveTalonFXConfiguration.slot0.kP = kDriveP; + driveTalonFXConfiguration.slot0.kI = kDriveI; + driveTalonFXConfiguration.slot0.kD = kDriveD; + driveTalonFXConfiguration.slot0.kF = kDriveF; + driveMotor.configAllSettings(driveTalonFXConfiguration);*/ CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); - // CANCODER CONFIG + canCoderConfiguration.magnetOffsetDegrees = offset; canCoder.configAllSettings(canCoderConfiguration); } + public Rotation2d getAngle() { - // Note: This assumes the CANCoders are setup with the default feedback coefficient and the sesnor value reports degrees. + // Note: This assumes the CANCoders are setup with the default feedback coefficient + // and the sesnor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } /** * Set the speed + rotation of the swerve module from a SwerveModuleState object - * - * @param desiredState - A SwerveModuleState representing the desired new state - * of the module + * @param desiredState - A SwerveModuleState representing the desired new state of the module */ public void setDesiredState(SwerveModuleState desiredState) { Rotation2d currentRotation = getAngle(); + SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // Find the difference between our current rotational position + our new rotational position @@ -86,7 +86,8 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; angleMotor.set(TalonFXControlMode.Position, desiredTicks); + double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/RobotGyro.java.old b/src/main/java/frc4388/utility/RobotGyro.java similarity index 100% rename from src/main/java/frc4388/utility/RobotGyro.java.old rename to src/main/java/frc4388/utility/RobotGyro.java