Skip to content

Commit

Permalink
Upgrading to 2024.7.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@thenetworkgrinch
thenetworkgrinch committed Nov 6, 2024
1 parent b8e06f2 commit 9fe6551
Show file tree
Hide file tree
Showing 14 changed files with 211 additions and 182 deletions.
109 changes: 59 additions & 50 deletions swervelib/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,18 +104,19 @@ public class SwerveDrive
*/
public boolean autonomousChassisVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
*/
public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)}
* during auto.
public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto.
*/
public boolean autonomousAngularVelocityCorrection = false;
public boolean autonomousAngularVelocityCorrection = false;
/**
* Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
*/
public double angularVelocityCoefficient = 0;
public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
Expand All @@ -133,10 +134,10 @@ public class SwerveDrive
*/
private SwerveIMU imu;
/**
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/
private IMUVelocity imuVelocity;
private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
Expand Down Expand Up @@ -586,11 +587,12 @@ public double getMaximumAngularVelocity()
/**
* Set the module states (azimuth and velocity) directly.
*
* @param desiredStates A list of SwerveModuleStates to send to the modules.
* @param desiredStates A list of SwerveModuleStates to send to the modules.
* @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop)
private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed,
boolean isOpenLoop)
{
// Desaturates wheel speeds
if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
Expand All @@ -612,10 +614,10 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds
}

/**
* Set the module states (azimuth and velocity) directly. Used primarily for auto paths.
* Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates,
* ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond,
* double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
* Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of
* desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, double
* attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double
* attainableMaxRotationalVelocityRadiansPerSecond)
*
* @param desiredStates A list of SwerveModuleStates to send to the modules.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
Expand All @@ -640,7 +642,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{

chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection);
chassisSpeeds = movementOptimizations(chassisSpeeds,
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);

SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
Expand Down Expand Up @@ -869,13 +873,15 @@ public void setMotorIdleMode(boolean brake)
}

/**
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
* @param enabled Enable state
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a
* few seconds.
*
* @param enabled Enable state
* @param deadband Deadband in degrees, default is 3 degrees.
*/
public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband)
{
for(SwerveModule swerveModule : swerveModules)
for (SwerveModule swerveModule : swerveModules)
{
swerveModule.setEncoderAutoSynchronize(enabled, deadband);
}
Expand Down Expand Up @@ -1259,25 +1265,26 @@ public void setCosineCompensator(boolean enabled)
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
{
if(!SwerveDriveTelemetry.isSimulation)
if (!SwerveDriveTelemetry.isSimulation)
{
chassisVelocityCorrection = enable;
discretizationdtSeconds = dtSeconds;
}
}

/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
* and/or auto
*
* @param useInTeleop Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @param useInTeleop Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
{
if(!SwerveDriveTelemetry.isSimulation)
if (!SwerveDriveTelemetry.isSimulation)
{
chassisVelocityCorrection = useInTeleop;
autonomousChassisVelocityCorrection = useInAuto;
Expand All @@ -1286,22 +1293,22 @@ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, dou
}

/**
* Enables angular velocity skew correction in teleop and/or autonomous
* and sets the angular velocity coefficient for both modes
* Enables angular velocity skew correction in teleop and/or autonomous and sets the angular velocity coefficient for
* both modes
*
* @param useInTeleop Enables angular velocity correction in teleop.
* @param useInAuto Enables angular velocity correction in autonomous.
* @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15.
* Start with a value of 0.1, test in teleop.
* When enabling for the first time if the skew is significantly worse try inverting the value.
* Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick.
* Change the value until you are visually happy with the skew.
* Ensure your tune works with different translational and rotational magnitudes.
* If this reduces skew in teleop, it may improve auto.
* @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. Start with a
* value of 0.1, test in teleop. When enabling for the first time if the skew is
* significantly worse try inverting the value. Tune by moving in a straight line while
* rotating. Testing is best done with angular velocity controls on the right stick.
* Change the value until you are visually happy with the skew. Ensure your tune works
* with different translational and rotational magnitudes. If this reduces skew in teleop,
* it may improve auto.
*/
public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff)
{
if(!SwerveDriveTelemetry.isSimulation)
if (!SwerveDriveTelemetry.isSimulation)
{
imuVelocity = IMUVelocity.createIMUVelocity(imu);
angularVelocityCorrection = useInTeleop;
Expand All @@ -1313,20 +1320,21 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut
/**
* Correct for skew that worsens as angular velocity increases
*
* @param velocity The chassis speeds to set the robot to achieve.
* @param velocity The chassis speeds to set the robot to achieve.
* @return {@link ChassisSpeeds} of the robot after angular velocity skew correction.
*/
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
{
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
if(angularVelocity.getRadians() != 0.0){
velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
velocity.vxMetersPerSecond,
velocity.vyMetersPerSecond,
velocity.omegaRadiansPerSecond,
getOdometryHeading());
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
}
if (angularVelocity.getRadians() != 0.0)
{
velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
velocity.vxMetersPerSecond,
velocity.vyMetersPerSecond,
velocity.omegaRadiansPerSecond,
getOdometryHeading());
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
}
return velocity;
}

Expand All @@ -1336,12 +1344,13 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
* @param velocity The chassis speeds to set the robot to achieve.
* @param uesChassisDiscretize Correct chassis velocity using 254's correction.
* @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
* @return The chassis speeds after optimizations.
* @return The chassis speeds after optimizations.
*/
private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection)
private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize,
boolean useAngularVelocitySkewCorrection)
{

if(useAngularVelocitySkewCorrection)
if (useAngularVelocitySkewCorrection)
{
velocity = angularVelocitySkewCorrection(velocity);
}
Expand Down
21 changes: 13 additions & 8 deletions swervelib/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ public class SwerveModule
/**
* Anti-Jitter AKA auto-centering disabled.
*/
private boolean antiJitterEnabled = true;
private boolean antiJitterEnabled = true;
/**
* Last swerve module state applied.
*/
Expand All @@ -111,15 +111,15 @@ public class SwerveModule
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;
/**
* Encoder, Absolute encoder synchronization enabled.
*/
private boolean synchronizeEncoderEnabled = false;
private boolean synchronizeEncoderEnabled = false;
/**
* Encoder synchronization deadband in degrees.
*/
private double synchronizeEncoderDeadband = 3;
private double synchronizeEncoderDeadband = 3;


/**
Expand Down Expand Up @@ -257,8 +257,10 @@ public void queueSynchronizeEncoders()
}

/**
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
* @param enabled Enable state
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a
* few seconds.
*
* @param enabled Enable state
* @param deadband Deadband in degrees, default is 3 degrees.
*/
public void setEncoderAutoSynchronize(boolean enabled, double deadband)
Expand All @@ -268,7 +270,9 @@ public void setEncoderAutoSynchronize(boolean enabled, double deadband)
}

/**
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a
* few seconds.
*
* @param enabled Enable state
*/
public void setEncoderAutoSynchronize(boolean enabled)
Expand Down Expand Up @@ -386,7 +390,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled)
{
double absoluteEncoderPosition = getAbsolutePosition();
if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) {
if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband)
{
angleMotor.setPosition(absoluteEncoderPosition);
}
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
Expand Down
27 changes: 11 additions & 16 deletions swervelib/imu/CanandgyroSwerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,50 +1,45 @@
package swervelib.imu;

import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.reduxrobotics.sensors.canandgyro.Canandgyro;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;

/**
* SwerveIMU interface for the Boron Candandgyro by Redux Robotics
* SwerveIMU interface for the Boron {@link Canandgyro} by Redux Robotics
*/
public class CanandgyroSwerve extends SwerveIMU
{

/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = 0.04;
public static double STATUS_TIMEOUT_SECONDS = 0.04;
/**
* Boron Canandgyro by Redux Robotics.
* Boron {@link Canandgyro} by Redux Robotics.
*/
Canandgyro imu;
private final Canandgyro imu;
/**
* Offset for the Boron Canandgyro.
* Offset for the Boron {@link Canandgyro}.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;

/**
* Generate the SwerveIMU for {@link Canandgyro}.
*
* @param canid CAN ID for the Boron Canandgyro
* @param canid CAN ID for the Boron {@link Canandgyro}
*/
public CanandgyroSwerve(int canid)
{
imu = new Canandgyro(canid);
}

/**
* Reset IMU to factory default.
* Reset {@link Canandgyro} to factory default.
*/
@Override
public void factoryDefault()
Expand All @@ -53,7 +48,7 @@ public void factoryDefault()
}

/**
* Clear sticky faults on IMU.
* Clear sticky faults on {@link Canandgyro}.
*/
@Override
public void clearStickyFaults()
Expand Down Expand Up @@ -128,7 +123,7 @@ public double getRate()
}

/**
* Get the instantiated IMU object.
* Get the instantiated {@link Canandgyro} IMU object.
*
* @return IMU object.
*/
Expand Down
Loading

0 comments on commit 9fe6551

Please # to comment.