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

fix: excise ballerinas from robot code

Browse Source
This commit is contained in:
C4llSiqn
2025-02-25 19:49:10 -07:00
parent 11861bf580
commit 57a9435c5d
10 changed files with 70 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -366,7 +366,7 @@ public final class Constants {
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17);
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
src/main/java/frc4388/robot/Robot.java
+2
View File
@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.CanDevice;
import frc4388.utility.DeferredBlock;
import frc4388.utility.DeferredBlockMulti;
import frc4388.utility.RobotTime;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
@@ -113,6 +114,7 @@ public class Robot extends TimedRobot {
@Override
public void disabledExit() {
DeferredBlock.execute();
DeferredBlockMulti.execute();
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+5 -2
View File
@@ -63,6 +63,7 @@ import frc4388.robot.subsystems.SwerveDrive;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.DeferredBlockMulti;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
@@ -293,10 +294,12 @@ public class RobotContainer {
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> { // Called on robot enable
TimesNegativeOne.update();
new DeferredBlock(() -> { // Called on first robot enable
m_robotSwerveDrive.resetGyro();
});
new DeferredBlockMulti(() -> { // Called on every robot enable
TimesNegativeOne.update();
});
DriverStation.silenceJoystickConnectionWarning(true);
// CameraServer.startAutomaticCapture();
src/main/java/frc4388/robot/commands/GotoLastApril.java
+18 -2
View File
@@ -77,9 +77,25 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis);
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
// xerr = targetpos.getX() - vision.getPose2d().getX();
// yerr = targetpos.getX() - vision.getPose2d().getY();
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
// roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
boolean invert = Math.abs(roterr) > 180;
if(roterr > 180){
roterr -= 360;
}else if(roterr < -180){
roterr += 360;
}
SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID error", roterr);
// SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr);
src/main/java/frc4388/robot/commands/LidarAlign.java
+11 -1
View File
@@ -21,6 +21,7 @@ public class LidarAlign extends Command {
private boolean foundReef;
private boolean headedRight;
private double speed;
private int bounces;
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
@@ -40,6 +41,7 @@ public class LidarAlign extends Command {
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.bounces = 0;
}
@@ -55,6 +57,8 @@ public class LidarAlign extends Command {
if (currentFinderTick > 10) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
bounces++;
if (bounces == 2) return;
}
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
@@ -78,7 +82,10 @@ public class LidarAlign extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (foundReef && lidar.withinDistance()) { // spot on
if (lidar.getDistance() < 4) {
swerveDrive.stopModules();
return true;
} else if (foundReef && lidar.withinDistance()) { // spot on
swerveDrive.stopModules();
return true;
} else if (foundReef && !lidar.withinDistance()) { // over shot
@@ -87,6 +94,9 @@ public class LidarAlign extends Command {
currentFinderTick = 0;
foundReef = false;
return false;
} else if (bounces == 2) {
swerveDrive.stopModules();
return true;
} else {
return false;
}
src/main/java/frc4388/robot/commands/MoveForTimeCommand.java
+1
View File
@@ -5,6 +5,7 @@ import java.time.Instant;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
// Command to repeat a joystick movement for a specific time.
public class MoveForTimeCommand extends Command {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+5 -4
View File
@@ -139,13 +139,14 @@ public class SwerveDrive extends Subsystem {
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
leftStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
stopped = false;
if (fieldRelative) {
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
// ! drift correction
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();