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

FIX and FAST

Browse Source
This commit is contained in:
Aarav Shah
2022-01-11 19:57:50 -07:00
parent a87c9a1e34
commit 06091a9bd9
5 changed files with 161 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+18 -17
View File
@@ -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);
}
}
}