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
2022-01-11 16:53:59 -07:00
parent a968157065
commit 049a7fd4f6
3 changed files with 17 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -5
View File
@@ -36,7 +36,7 @@ public final class Constants {
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 JOYSTICK_TO_METERS_PER_SECOND = 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 MAX_SPEED_FEET_PER_SEC = 20;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; 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;
@@ -51,10 +51,14 @@ public final class Constants {
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
//ofsets are in degrees //ofsets are in degrees
public static final float LEFT_FRONT_ENCODER_OFFSET = 0; // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
public static final float RIGHT_FRONT_ENCODER_OFFSET = 0; // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
public static final float LEFT_BACK_ENCODER_OFFSET = 0; // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
public static final float RIGHT_BACK_ENCODER_OFFSET = 0; // 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 // swerve PID constants
public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_SLOT_IDX = 0;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+4 -4
View File
@@ -85,10 +85,10 @@ public class SwerveDrive extends SubsystemBase
m_rightBackEncoder = rightBackEncoder; m_rightBackEncoder = rightBackEncoder;
modules = new SwerveModule[] { modules = new SwerveModule[] {
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
}; };
//gyro.reset(); //gyro.reset();
} }
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+4 -2
View File
@@ -15,6 +15,7 @@ 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.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
@@ -30,7 +31,7 @@ public class SwerveModule extends SubsystemBase {
/** Creates a new SwerveModule. */ /** 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.driveMotor = driveMotor;
this.angleMotor = angleMotor; this.angleMotor = angleMotor;
this.canCoder = canCoder; this.canCoder = canCoder;
@@ -57,7 +58,7 @@ public class SwerveModule extends SubsystemBase {
driveMotor.configAllSettings(driveTalonFXConfiguration);*/ driveMotor.configAllSettings(driveTalonFXConfiguration);*/
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
//CANCODER CONFIG canCoderConfiguration.magnetOffsetDegrees = offset;
canCoder.configAllSettings(canCoderConfiguration); canCoder.configAllSettings(canCoderConfiguration);
} }
@@ -74,6 +75,7 @@ public class SwerveModule extends SubsystemBase {
*/ */
public void setDesiredState(SwerveModuleState desiredState) { public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = getAngle(); Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// Find the difference between our current rotational position + our new rotational position // Find the difference between our current rotational position + our new rotational position