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

Kracken stuff

Browse Source
This commit is contained in:
Michael Mikovsky
2024-07-23 09:16:55 -06:00
parent da1c29f913
commit eb232fbe81
4 changed files with 55 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+3 -3
View File
@@ -152,10 +152,10 @@ public class RobotContainer {
// }, m_robotSwerveDrive));
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.oneModuleTest(
m_robotMap.rightFront,
m_robotSwerveDrive.driveWithInput(
getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight());
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive));
src/main/java/frc4388/robot/RobotMap.java
+4 -3
View File
@@ -14,6 +14,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.LEDConstants;
@@ -29,9 +30,9 @@ import frc4388.utility.RobotGyro;
* testing and modularization.
*/
public class RobotMap {
// private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
public RobotGyro gyro = null;
private Pigeon2 m_pigeon2 = new Pigeon2(14);
public RobotGyro gyro = new RobotGyro(m_pigeon2);
// public RobotGyro gyro = null;
public SwerveModule leftFront;
public SwerveModule rightFront;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+33 -33
View File
@@ -68,38 +68,38 @@ public class SwerveDrive extends SubsystemBase {
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
// if (fieldRelative) {
if (fieldRelative) {
// double rot = 0;
double rot = 0;
// // ! drift correction
// if (rightStick.getNorm() > 0.05) {
// rotTarget = gyro.getAngle();
// rot = rightStick.getX();
// // SmartDashboard.putBoolean("drift correction", false);
// stopped = false;
// } else if(leftStick.getNorm() > 0.05) {
// if (!stopped) {
// stopModules();
// stopped = true;
// }
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
// // SmartDashboard.putBoolean("drift correction", true);
// SmartDashboard.putBoolean("drift correction", true);
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
// }
}
// // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// // Convert field-relative speeds to robot-relative speeds.
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
// } else { // Create robot-relative speeds.
// chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
// }
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
@@ -168,14 +168,14 @@ public class SwerveDrive extends SubsystemBase {
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
// public void setModuleStates(SwerveModuleState[] desiredStates) {
// SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
// for (int i = 0; i < desiredStates.length; i++) {
// SwerveModule module = modules[i];
// SwerveModuleState state = desiredStates[i];
// module.setDesiredState(state);
// }
// }
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();