Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

updated to 2026

Browse Source 
removed 2025
This commit is contained in:
C4llSqin
2026-01-08 19:42:41 -07:00
parent 725287a284
commit 17c3ff1ec9
32 changed files with 543 additions and 922 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.autos;
package frc4388.robot.commands.Autos;
// package frc4388.robot.commands.Autos;
// import java.io.File;
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+1 -1
View File
@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.swerve;
package frc4388.robot.commands.Swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.swerve;
package frc4388.robot.commands.Swerve;
import java.io.FileInputStream;
import java.util.ArrayList;
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.swerve;
package frc4388.robot.commands.Swerve;
import java.io.FileOutputStream;
import java.util.ArrayList;
src/main/java/frc4388/robot/commands/alignment/DriveToReef.java
-188
View File
@@ -1,188 +0,0 @@
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.ReefPositionHelper;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.compute.ReefPositionHelper.Side;
import frc4388.utility.structs.Gains;
public class DriveToReef extends Command {
// private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
// private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
// private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive;
Vision vision;
double distance;
Side side;
boolean waitVelocityZero;
/**
* Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive
*/
public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.distance = distance;
this.side = side;
this.waitVelocityZero = waitVelocityZero && false;
addRequirements(swerveDrive);
}
public static double tagRelativeXError = -1;
private static void setTagRelativeXError(double val){
tagRelativeXError = val;
}
@Override
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(
this.vision.getPose2d(),
side,
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
);
}
double xerr;
double yerr;
double roterr;
double xoutput;
double youtput;
double rotoutput;
@Override
public void execute() {
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
// xerr = targetpos.getX() - vision.getPose2d().getX();
// yerr = targetpos.getX() - vision.getPose2d().getY();
// roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
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);
SmartDashboard.putNumber("PID Rot Error", roterr);
xoutput = xPID.update(xerr);
youtput = yPID.update(yerr);
// rotoutput = rotPID.update(roterr);
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
// rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1)
// 0,0
);
// Translation2d rightStick = new Translation2d(
// Math.max(Math.min(rotoutput, 1), -1),
// 0
// );
setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation())
.getCos());
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
}
@Override
public final boolean isFinished() {
boolean finished = (
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
);
// System.out.println(finished);
if(finished)
swerveDrive.softStop();
return finished;
// this statement is a boolean in and of itself
}
private class PID {
protected Gains gains;
private double output = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(Gains gains, double tolerance) {
this.gains = gains;
}
// Called when the command is initially scheduled.
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
public double update(double error) {
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
return output;
}
}
}
src/main/java/frc4388/robot/commands/alignment/LidarAlign.java
+1 -1
View File
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = (DriveToReef.tagRelativeXError < 0);
//this.headedRight = (DriveToReef.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java
-36
View File
@@ -1,36 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.wait;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.elevator.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitElevatorRefrence extends Command {
/** Creates a new waitElevatorRefrence. */
private Elevator elevator;
public waitElevatorRefrence(Elevator elevator) {
this.elevator = elevator;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.elevatorAtReference();
}
}
src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java
-36
View File
@@ -1,36 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.wait;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.elevator.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitEndefectorRefrence extends Command {
/** Creates a new waitElevatorRefrence. */
private Elevator elevator;
public waitEndefectorRefrence(Elevator elevator) {
this.elevator = elevator;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.endeffectorAtReference();
}
}
src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java
-36
View File
@@ -1,36 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.wait;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.elevator.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitFeedCoral extends Command {
/** Creates a new waitElevatorRefrence. */
private Elevator elevator;
public waitFeedCoral(Elevator elevator) {
this.elevator = elevator;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.hasCoral();
}
}