Team4388/Swerve-Drive
1
0
Fork 0
mirror of https://github.com/Team4388/Swerve-Drive.git synced 2026-06-09 00:38:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

FIX

Browse Source
This commit is contained in:
Aarav Shah
2021-12-27 17:38:12 -07:00
parent 80067b527c
commit a968157065
4 changed files with 17 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -1
View File
@@ -35,7 +35,9 @@ public final class Constants {
public static final double WHEEL_SPEED = 0.1; public static final double WHEEL_SPEED = 0.1;
public static final double WIDTH = 22; public static final double WIDTH = 22;
public static final double HEIGHT = 22; public static final double HEIGHT = 22;
public static final double MAX_SPEED_FEET_PER_SEC = 5; public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
public static final double MAX_SPEED_FEET_PER_SEC = 16;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
public static final int LEFT_FRONT_STEER_CAN_ID = 2; public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
src/main/java/frc4388/robot/RobotMap.java
+1 -1
View File
@@ -130,7 +130,7 @@ public class RobotMap {
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// config cancoder as remote encoder for swerve steer motors // config cancoder as remote encoder for swerve steer motors
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -4
View File
@@ -88,7 +88,7 @@ public class SwerveDrive extends SubsystemBase
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder) // Back Right new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right
}; };
//gyro.reset(); //gyro.reset();
} }
@@ -105,12 +105,14 @@ public class SwerveDrive extends SubsystemBase
{ {
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
driveFromSpeeds(speeds);*/ driveFromSpeeds(speeds);*/
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
SwerveModuleState[] states = SwerveModuleState[] states =
kinematics.toSwerveModuleStates( kinematics.toSwerveModuleStates(
fieldRelative fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot)); : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(states, SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); SwerveDriveKinematics.normalizeWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) { for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i]; SwerveModule module = modules[i];
SwerveModuleState state = states[i]; SwerveModuleState state = states[i];
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+7 -9
View File
@@ -4,24 +4,21 @@
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.wpilibj.util.Units;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains; import frc4388.utility.Gains;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
public class SwerveModule extends SubsystemBase { public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor; private WPI_TalonFX driveMotor;
@@ -60,6 +57,7 @@ public class SwerveModule extends SubsystemBase {
driveMotor.configAllSettings(driveTalonFXConfiguration);*/ driveMotor.configAllSettings(driveTalonFXConfiguration);*/
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
//CANCODER CONFIG
canCoder.configAllSettings(canCoderConfiguration); canCoder.configAllSettings(canCoderConfiguration);
} }
@@ -82,7 +80,7 @@ public class SwerveModule extends SubsystemBase {
Rotation2d rotationDelta = state.angle.minus(currentRotation); Rotation2d rotationDelta = state.angle.minus(currentRotation);
// Find the new absolute position of the module based on the difference in rotation // Find the new absolute position of the module based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360) * kEncoderTicksPerRotation; double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
// Convert the CANCoder from it's position reading back to ticks // Convert the CANCoder from it's position reading back to ticks
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
double desiredTicks = currentTicks + deltaTicks; double desiredTicks = currentTicks + deltaTicks;
@@ -90,6 +88,6 @@ public class SwerveModule extends SubsystemBase {
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
} }
} }