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

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/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_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right
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();
}
@@ -105,12 +105,14 @@ public class SwerveDrive extends SubsystemBase
{
/*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(xSpeed, ySpeed, rot, gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(states, SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(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];
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+7 -9
View File
@@ -4,24 +4,21 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
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.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.util.Units;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor;
@@ -60,6 +57,7 @@ public class SwerveModule extends SubsystemBase {
driveMotor.configAllSettings(driveTalonFXConfiguration);*/
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
//CANCODER CONFIG
canCoder.configAllSettings(canCoderConfiguration);
}
@@ -82,7 +80,7 @@ public class SwerveModule extends SubsystemBase {
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// 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
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
double desiredTicks = currentTicks + deltaTicks;
@@ -90,6 +88,6 @@ public class SwerveModule extends SubsystemBase {
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);
}
}