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));
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.oneModuleTest( m_robotSwerveDrive.driveWithInput(
m_robotMap.rightFront,
getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight()); getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)); }, 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.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.LEDConstants;
@@ -29,9 +30,9 @@ import frc4388.utility.RobotGyro;
* testing and modularization. * testing and modularization.
*/ */
public class RobotMap { public class RobotMap {
// private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); private Pigeon2 m_pigeon2 = new Pigeon2(14);
// public RobotGyro gyro = new RobotGyro(m_pigeon2); public RobotGyro gyro = new RobotGyro(m_pigeon2);
public RobotGyro gyro = null; // public RobotGyro gyro = null;
public SwerveModule leftFront; public SwerveModule leftFront;
public SwerveModule rightFront; 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; boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
// if (fieldRelative) { if (fieldRelative) {
// double rot = 0; double rot = 0;
// // ! drift correction // ! drift correction
// if (rightStick.getNorm() > 0.05) { if (rightStick.getNorm() > 0.05) {
// rotTarget = gyro.getAngle(); rotTarget = gyro.getAngle();
// rot = rightStick.getX(); rot = rightStick.getX();
// // SmartDashboard.putBoolean("drift correction", false); // SmartDashboard.putBoolean("drift correction", false);
// stopped = false; stopped = false;
// } else if(leftStick.getNorm() > 0.05) { } else if(leftStick.getNorm() > 0.05) {
// if (!stopped) { if (!stopped) {
// stopModules(); stopModules();
// stopped = true; 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. // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// // Convert field-relative speeds to robot-relative speeds. // Convert field-relative speeds to robot-relative speeds.
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
// } else { // Create robot-relative speeds. } else { // Create robot-relative speeds.
// chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
// } }
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
} }
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { 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. * Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set. * @param desiredStates Array of module states to set.
*/ */
// public void setModuleStates(SwerveModuleState[] desiredStates) { public void setModuleStates(SwerveModuleState[] desiredStates) {
// SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
// for (int i = 0; i < desiredStates.length; i++) { for (int i = 0; i < desiredStates.length; i++) {
// SwerveModule module = modules[i]; SwerveModule module = modules[i];
// SwerveModuleState state = desiredStates[i]; SwerveModuleState state = desiredStates[i];
// module.setDesiredState(state); module.setDesiredState(state);
// } }
// } }
public boolean rotateToTarget(double angle) { public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle(); double currentAngle = getGyroAngle();
src/main/java/frc4388/utility/RobotGyro.java
+15 -8
View File
@@ -8,11 +8,13 @@
package frc4388.utility; package frc4388.utility;
import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.kauailabs.navx.frc.AHRS; import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.GyroBase; // import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil; 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 * 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 { public class RobotGyro implements Gyro {
private RobotTime m_robotTime = RobotTime.getInstance(); private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_Pigeon2 m_pigeon = null; private Pigeon2 m_pigeon = null;
private AHRS m_navX = null; private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX 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 * Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro * @param gyro the gyroscope to use for Gyro
*/ */
public RobotGyro(WPI_Pigeon2 gyro) { public RobotGyro(Pigeon2 gyro) {
m_pigeon = gyro; m_pigeon = gyro;
m_isGyroAPigeon = true; m_isGyroAPigeon = true;
} }
@@ -54,8 +56,8 @@ public class RobotGyro implements Gyro {
public void resetZeroValues() { public void resetZeroValues() {
if (!m_isGyroAPigeon) return; if (!m_isGyroAPigeon) return;
pitchZero = m_pigeon.getPitch(); // pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll(); // rollZero = m_pigeon.getRoll();
} }
/** /**
@@ -181,10 +183,15 @@ public class RobotGyro implements Gyro {
* Roll is within [-90,+90] degrees. * Roll is within [-90,+90] degrees.
*/ */
private double[] getPigeonAngles() { private double[] getPigeonAngles() {
double[] ypr = new double[3]; m_pigeon.getAngle();
m_pigeon.getYawPitchRoll(ypr); 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 @Override
@@ -253,7 +260,7 @@ public class RobotGyro implements Gyro {
} }
} }
public WPI_Pigeon2 getPigeon(){ public Pigeon2 getPigeon(){
return m_pigeon; return m_pigeon;
} }