Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -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();
src/main/java/frc4388/utility/RobotGyro.java
+15 -8
View File
@@ -8,11 +8,13 @@
package frc4388.utility;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
@@ -20,7 +22,7 @@ import edu.wpi.first.math.MathUtil;
public class RobotGyro implements Gyro {
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_Pigeon2 m_pigeon = null;
private Pigeon2 m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
@@ -34,7 +36,7 @@ public class RobotGyro implements Gyro {
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(WPI_Pigeon2 gyro) {
public RobotGyro(Pigeon2 gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
@@ -54,8 +56,8 @@ public class RobotGyro implements Gyro {
public void resetZeroValues() {
if (!m_isGyroAPigeon) return;
pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll();
// pitchZero = m_pigeon.getPitch();
// rollZero = m_pigeon.getRoll();
}
/**
@@ -181,10 +183,15 @@ public class RobotGyro implements Gyro {
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
m_pigeon.getAngle();
var rotation = m_pigeon.getRotation3d();
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)};
}
@Override
public Rotation2d getRotation2d() {
return m_pigeon.getRotation2d();
}
@Override
@@ -253,7 +260,7 @@ public class RobotGyro implements Gyro {
}
}
public WPI_Pigeon2 getPigeon(){
public Pigeon2 getPigeon(){
return m_pigeon;
}