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

x /= i^2

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-24 17:59:44 -07:00
parent 761a56cf0f
commit 07841b303a
6 changed files with 71 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+5 -2
View File
@@ -65,6 +65,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.DeferredBlock;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.configurable.ConfigurableString;
@@ -292,7 +293,10 @@ public class RobotContainer {
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
new DeferredBlock(() -> { // Called on robot enable
TimesNegativeOne.update();
m_robotSwerveDrive.resetGyro();
});
DriverStation.silenceJoystickConnectionWarning(true);
// CameraServer.startAutomaticCapture();
@@ -462,7 +466,6 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
src/main/java/frc4388/robot/commands/GotoLastApril.java
+3 -11
View File
@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
@@ -75,18 +76,9 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
if (alliance == Alliance.Red) {
xerr = -(targetpos.getX() - vision.getPose2d().getX());
yerr = (targetpos.getY() - vision.getPose2d().getY());
}
else{
xerr = targetpos.getX() - vision.getPose2d().getX();
yerr = targetpos.getY() - vision.getPose2d().getY();
}
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis);
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
// SmartDashboard.putNumber("PID X Error", xerr);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+8 -19
View File
@@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.Status.ReportLevel;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.config.PIDConstants;
@@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem {
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
// var alliance = DriverStation.getAlliance();
// if (alliance.isPresent()) {
// return alliance.get() == DriverStation.Alliance.Red;
// }
return TimesNegativeOne.isRed;
},
this // Reference to this subsystem to set requirements
);
@@ -141,18 +140,8 @@ public class SwerveDrive extends Subsystem {
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
Optional<Alliance> alliance = DriverStation.getAlliance();
if(!alliance.isEmpty()){
if (alliance.get() == Alliance.Red)
leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
else
leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
// if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
}
if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
leftStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
stopped = false;
src/main/java/frc4388/utility/Alliance.java
+5
View File
@@ -0,0 +1,5 @@
package frc4388.utility;
public class Alliance {
}
src/main/java/frc4388/utility/ReefPositionHelper.java
+4 -7
View File
@@ -63,14 +63,11 @@ public class ReefPositionHelper {
* Function to find closest tag location based on side
*/
public static Pose2d getNearestTag(Pose2d position) {
Optional<Alliance> ally = DriverStation.getAlliance();
if (!ally.isPresent())
return new Pose2d();
if (ally.get() == Alliance.Red)
if(TimesNegativeOne.isRed)
return getNearestTag(RED_TAGS, position);
if (ally.get() == Alliance.Blue)
return getNearestTag(BLUE_TAGS, position);
return new Pose2d();
else
return getNearestTag(BLUE_TAGS, position);
}
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
src/main/java/frc4388/utility/TimesNegativeOne.java
+46
View File
@@ -0,0 +1,46 @@
package frc4388.utility;
import java.util.Optional;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc4388.robot.Constants.SwerveDriveConstants;
// Class that holds weather the drivers sticks should be inverted
public class TimesNegativeOne {
public static boolean XAxis = SwerveDriveConstants.INVERT_X;
public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
public static boolean isRed = false;
private static boolean isRed() {
Optional<Alliance> alliance = DriverStation.getAlliance();
if(alliance.isEmpty()) return false;
return (alliance.get() == Alliance.Red);
}
public static void update(){
isRed = isRed();
XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
RotAxis = SwerveDriveConstants.INVERT_ROTATION;
}
public static double invert(double num, boolean invert){
return invert ? -num : num;
}
public static Translation2d invert(Translation2d stick, boolean invertXY){
if(invertXY) return stick.times(-1);
else return stick;
}
public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
return new Translation2d(
invert(stick.getX(), invertX),
invert(stick.getY(), invertY)
);
}
}