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

denver comp (im tweakin)

Browse Source
This commit is contained in:
Abhishrek05
2024-03-21 09:01:42 -06:00
parent 037d2172d7
commit 6b0e4b9cad
7 changed files with 388 additions and 296 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+158 -68
View File
@@ -7,7 +7,10 @@
package frc4388.robot;
import org.opencv.video.Video;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.VideoMode;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
@@ -24,7 +27,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.commands.Autos.AutoAlign;
//import frc4388.robot.commands.Autos.AutoAlign;
import frc4388.robot.commands.Autos.PlaybackChooser;
import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.commands.Swerve.JoystickRecorder;
@@ -87,7 +90,7 @@ public class RobotContainer {
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup(
private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.idle())
// new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
@@ -108,18 +111,18 @@ public class RobotContainer {
// ! Teleop Commands
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
//private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
// MoveToSpeaker,
autoAlign,
//autoAlign,
new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(3.0),
new InstantCommand(() -> m_robotShooter.idle()),
new InstantCommand(() -> autoAlign.reverse()),
autoAlign.asProxy()
new InstantCommand(() -> m_robotShooter.idle())
// new InstantCommand(() -> autoAlign.reverse()),
// autoAlign.asProxy()
);
@@ -290,7 +293,7 @@ public class RobotContainer {
DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture();
CameraServer.startAutomaticCapture().setConfigJson("{ \"width\": -512, \"height\": 512 }");
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
@@ -329,58 +332,58 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()));
// * /* D-Pad Stuff */
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
new Translation2d(0, 0),
true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0),
true)));
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
// ! /* Auto Recording */
// new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
// .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
// new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
// () -> autoplaybackName.get()))
// .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
() -> autoplaybackName.get()))
.onFalse(new InstantCommand());
// new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
// () -> autoplaybackName.get(),
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
// true, false))
// .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(),
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
@@ -475,23 +478,99 @@ public class RobotContainer {
}
private void configureVirtualButtonBindings() {
/* Driver Buttons */
// new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
// /* Speed */
// new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// ? /* Driver Buttons */
// new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
new JoystickButton(getVirtualDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()));
new JoystickButton(getVirtualDriverController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()));
// /* Operator Buttons */
// * /* D-Pad Stuff */
// new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
// new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
// new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
// new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
// new Translation2d(0, 0),
// true)))
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
// new Translation2d(0, 0),
// true)));
// ! /* Auto Recording */
// new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
// .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
// new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
// () -> autoplaybackName.get()))
// .onFalse(new InstantCommand());
// new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
// () -> autoplaybackName.get(),
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
// true, false))
// .onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// () -> getDeadbandedDriverController().getLeftX(),
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "Taxi.txt"))
// .onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
// .onFalse(new InstantCommand());
// ! /* Speed */
new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
new Trigger(() -> getVirtualDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
new Trigger(() -> getVirtualDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
// new JoystickButton(getVirtualDriverController(), XboxController.Y_BUTTON)
// .whileTrue(new InstantCommand(() ->
// m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
// new Translation2d(0, 0),
// true), m_robotSwerveDrive));
//? /* Operator Buttons */
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
@@ -506,37 +585,48 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract.asProxy());
.onTrue(emergencyRetract);
// Override Intake Position encoder: out
new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake));
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-55), m_robotIntake));
// Override Intake Position encoder: in
new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake));
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-6.2), m_robotIntake));
new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter))
.onFalse(turnOffShoot.asProxy());
.onFalse(turnOffShoot);
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(i.asProxy())
.onTrue(i)
.onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
//spins up shooter, no wind down
new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
// new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
// new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake));
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
.onTrue(emergencyRetract.asProxy());
.onTrue(emergencyRetract);
new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getVirtualOperatorController().getPOV() == 0)
.onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
}
/**
src/main/java/frc4388/robot/commands/Autos/AutoAlign.java
+190 -190
View File
@@ -1,208 +1,208 @@
package frc4388.robot.commands.Autos;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.Constants.AutoAlignConstants;
import frc4388.robot.Constants.VisionConstants;
import edu.wpi.first.wpilibj2.command.Command;
// package frc4388.robot.commands.Autos;
// import frc4388.robot.subsystems.Limelight;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.robot.Constants.AutoAlignConstants;
// import frc4388.robot.Constants.VisionConstants;
// import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Rotation2d;
// import edu.wpi.first.math.geometry.Translation2d;
// import edu.wpi.first.math.geometry.Rotation2d;
import java.util.Optional;
// import java.util.Optional;
import org.opencv.core.RotatedRect;
// import org.opencv.core.RotatedRect;
import edu.wpi.first.math.geometry.Pose2d;
// import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
// import edu.wpi.first.wpilibj.DriverStation;
// import edu.wpi.first.wpilibj.DriverStation.Alliance;
public class AutoAlign extends Command {
private SwerveDrive swerve;
private Limelight limelight;
private Pose2d pose;
private Translation2d targetPos;
private Rotation2d targetRot;
// public class AutoAlign extends Command {
// private SwerveDrive swerve;
// private Limelight limelight;
// private Pose2d pose;
// private Translation2d targetPos;
// private Rotation2d targetRot;
private Optional<Alliance> alliance;
private boolean isRed;
// private Optional<Alliance> alliance;
// private boolean isRed;
private boolean isFinished;
private boolean isReverseFinished;
// private boolean isFinished;
// private boolean isReverseFinished;
private boolean reverseAfterFinish;
private Translation2d[] moveStickReplayArr;
private Translation2d[] rotStickReplayArr;
private int replayIndex;
// private boolean reverseAfterFinish;
// private Translation2d[] moveStickReplayArr;
// private Translation2d[] rotStickReplayArr;
// private int replayIndex;
// PID Stuff
private double prevError;
private double cumError;
// // PID Stuff
// private double prevError;
// private double cumError;
public AutoAlign(SwerveDrive swerve, Limelight limelight) {
this.swerve = swerve;
this.limelight = limelight;
}
// Calc the closest point on a circle, to the center of the speaker
private Translation2d calcTargetPos(){
final double R = VisionConstants.targetPosDistance;
final double cX;
final double cY;
if(isRed){
cX = VisionConstants.RedSpeakerCenter.getX();
cY = VisionConstants.RedSpeakerCenter.getY();
}else{
cX = VisionConstants.BlueSpeakerCenter.getX();
cY = VisionConstants.BlueSpeakerCenter.getY();
}
final double pX = pose.getX();
final double pY = pose.getY();
// Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point
double vX = pX - cX;
double vY = pY - cY;
double magV = Math.sqrt(vX*vX + vY*vY);
double aX = cX + vX / magV * R;
double aY = cY + vY / magV * R;
return new Translation2d(aX, aY);
}
// Calculate the angle facing the center, at the target rot
private Rotation2d calcTargetRot() {
final double R = VisionConstants.targetPosDistance;
final double cX;
final double cY;
if(isRed){
cX = VisionConstants.RedSpeakerCenter.getX();
cY = VisionConstants.RedSpeakerCenter.getY();
}else{
cX = VisionConstants.BlueSpeakerCenter.getX();
cY = VisionConstants.BlueSpeakerCenter.getY();
}
final double pX = pose.getX();
final double pY = pose.getY();
final double dX = pX - cX;
final double dY = pY - cY;
final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360);
return Rotation2d.fromDegrees(yaw);
}
// Clamp to a circle, like an xbox controller
private Translation2d clamp(double oldX, double oldY){
// Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley
final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360;
final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI));
final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI));
final double newX = Math.max(-maxX, Math.min(maxX, oldX));
final double newY = Math.max(-maxY, Math.min(maxY, oldY));
return new Translation2d(newX, newY);
}
private Translation2d calcMoveStick(){
final double curX = pose.getX();
final double curY = pose.getY();
// I think this might work, assuming the constants are good
double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed;
double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed;
return clamp(stickX, stickY);
}
private Translation2d calcRotStick(){
double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
cumError += error * .02; // 20 ms
double delta = error - prevError;
final double kP = 4;
final double kI = 4;
final double kD = 4;
final double kF = 4;
double output = error * kP;
output += cumError * kI;
output += delta * kD;
output += kF;
prevError = error;
return clamp(output, 0);
}
public void reverse() {
this.reverseAfterFinish = true;
}
// Called when the command is initially scheduled.
@Override
public final void initialize() {
isRed = alliance.get() == DriverStation.Alliance.Red;
if(reverseAfterFinish){
// isReverseFinished = false;
replayIndex = 0;
}else{
moveStickReplayArr = new Translation2d[]{};
rotStickReplayArr = new Translation2d[]{};
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// Update limelight pos
pose = limelight.getPose();
// These must be 0, or it will error
Translation2d moveStick = new Translation2d(0, 0);
Translation2d rotStick = new Translation2d(0, 0);
// Regular replay
if(!isFinished){
targetPos = calcTargetPos();
targetRot = calcTargetRot();
moveStick = calcMoveStick();
rotStick = calcRotStick();
// This is a way of appending...
moveStickReplayArr[moveStickReplayArr.length] = moveStick;
rotStickReplayArr[rotStickReplayArr.length] = rotStick;
// if(isFinished != limelight.isNearSpeaker() && isReverseFinished){
// replayIndex
// public AutoAlign(SwerveDrive swerve, Limelight limelight) {
// this.swerve = swerve;
// this.limelight = limelight;
// }
isFinished = limelight.isNearSpeaker();
// If reverseAfterFinish, then loop back over and replay in reverse
}else if(reverseAfterFinish && !isReverseFinished){
// Get reverse direction
moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1];
// // Calc the closest point on a circle, to the center of the speaker
// private Translation2d calcTargetPos(){
// final double R = VisionConstants.targetPosDistance;
// final double cX;
// final double cY;
// if(isRed){
// cX = VisionConstants.RedSpeakerCenter.getX();
// cY = VisionConstants.RedSpeakerCenter.getY();
// }else{
// cX = VisionConstants.BlueSpeakerCenter.getX();
// cY = VisionConstants.BlueSpeakerCenter.getY();
// }
// final double pX = pose.getX();
// final double pY = pose.getY();
// Invert sticks
moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1);
rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1);
// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point
// double vX = pX - cX;
// double vY = pY - cY;
// double magV = Math.sqrt(vX*vX + vY*vY);
// double aX = cX + vX / magV * R;
// double aY = cY + vY / magV * R;
replayIndex++;
// return new Translation2d(aX, aY);
// }
if(replayIndex >= moveStickReplayArr.length){
reverseAfterFinish = true;
}
}
// // Calculate the angle facing the center, at the target rot
// private Rotation2d calcTargetRot() {
// final double R = VisionConstants.targetPosDistance;
// final double cX;
// final double cY;
// if(isRed){
// cX = VisionConstants.RedSpeakerCenter.getX();
// cY = VisionConstants.RedSpeakerCenter.getY();
// }else{
// cX = VisionConstants.BlueSpeakerCenter.getX();
// cY = VisionConstants.BlueSpeakerCenter.getY();
// }
// final double pX = pose.getX();
// final double pY = pose.getY();
// This would greatly benifit from having feild Relative implemented.
swerve.driveWithInput(moveStick, rotStick, true);
}
// final double dX = pX - cX;
// final double dY = pY - cY;
// Returns true when the command should end.
@Override
public final boolean isFinished() {
return isFinished && (isReverseFinished || !reverseAfterFinish);
}
}
// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360);
// return Rotation2d.fromDegrees(yaw);
// }
// // Clamp to a circle, like an xbox controller
// private Translation2d clamp(double oldX, double oldY){
// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley
// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360;
// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI));
// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI));
// final double newX = Math.max(-maxX, Math.min(maxX, oldX));
// final double newY = Math.max(-maxY, Math.min(maxY, oldY));
// return new Translation2d(newX, newY);
// }
// private Translation2d calcMoveStick(){
// final double curX = pose.getX();
// final double curY = pose.getY();
// // I think this might work, assuming the constants are good
// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed;
// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed;
// return clamp(stickX, stickY);
// }
// private Translation2d calcRotStick(){
// double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
// cumError += error * .02; // 20 ms
// double delta = error - prevError;
// final double kP = 4;
// final double kI = 4;
// final double kD = 4;
// final double kF = 4;
// double output = error * kP;
// output += cumError * kI;
// output += delta * kD;
// output += kF;
// prevError = error;
// return clamp(output, 0);
// }
// public void reverse() {
// this.reverseAfterFinish = true;
// }
// // Called when the command is initially scheduled.
// @Override
// public final void initialize() {
// isRed = alliance.get() == DriverStation.Alliance.Red;
// if(reverseAfterFinish){
// // isReverseFinished = false;
// replayIndex = 0;
// }else{
// moveStickReplayArr = new Translation2d[]{};
// rotStickReplayArr = new Translation2d[]{};
// }
// }
// // Called every time the scheduler runs while the command is scheduled.
// @Override
// public void execute() {
// // Update limelight pos
// pose = limelight.getPose();
// // These must be 0, or it will error
// Translation2d moveStick = new Translation2d(0, 0);
// Translation2d rotStick = new Translation2d(0, 0);
// // Regular replay
// if(!isFinished){
// targetPos = calcTargetPos();
// targetRot = calcTargetRot();
// moveStick = calcMoveStick();
// rotStick = calcRotStick();
// // This is a way of appending...
// moveStickReplayArr[moveStickReplayArr.length] = moveStick;
// rotStickReplayArr[rotStickReplayArr.length] = rotStick;
// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){
// // replayIndex
// // }
// isFinished = limelight.isNearSpeaker();
// // If reverseAfterFinish, then loop back over and replay in reverse
// }else if(reverseAfterFinish && !isReverseFinished){
// // Get reverse direction
// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1];
// // Invert sticks
// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1);
// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1);
// replayIndex++;
// if(replayIndex >= moveStickReplayArr.length){
// reverseAfterFinish = true;
// }
// }
// // This would greatly benifit from having feild Relative implemented.
// swerve.driveWithInput(moveStick, rotStick, true);
// }
// // Returns true when the command should end.
// @Override
// public final boolean isFinished() {
// return isFinished && (isReverseFinished || !reverseAfterFinish);
// }
// }
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+1 -1
View File
@@ -68,7 +68,7 @@ public class JoystickRecorder extends CommandBase {
new Translation2d(inputs.rightX, inputs.rightY),
true);
System.out.println("RECORDING");
//System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Intake.java
+4 -2
View File
@@ -117,9 +117,9 @@ public class Intake extends SubsystemBase {
// in init function, set slot 0 gains
var slot0Configs = new Slot0Configs();
slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error
slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output
talonPivot.getConfigurator().apply(slot0Configs);
@@ -336,5 +336,7 @@ public class Intake extends SubsystemBase {
// SmartDashboard.putNumber("Pivot Position", getArmPos());
//smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
//SmartDashboard.putBoolean("Limit Switch State", getTalonIntakeLimitSwitchState());
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
+44 -44
View File
@@ -18,65 +18,65 @@ import frc4388.robot.Constants.VisionConstants;
public class Limelight extends SubsystemBase {
// [X, Y, Z, Roll, Pitch, Yaw]
private double[] cameraPose;
private boolean isTag;
// // [X, Y, Z, Roll, Pitch, Yaw]
// private double[] cameraPose;
// private boolean isTag;
private Pose2d pose;
private boolean isNearSpeaker;
// private Pose2d pose;
// private boolean isNearSpeaker;
public boolean getIsTag() {
return isTag;
}
// public boolean getIsTag() {
// return isTag;
// }
private void update() {
SmartDashboard.putBoolean("Apriltag", isTag);
if(!isTag){
return;
}
// private void update() {
// SmartDashboard.putBoolean("Apriltag", isTag);
// if(!isTag){
// return;
// }
double x = cameraPose[0];
double y = cameraPose[1];
double yaw = cameraPose[5];
// double x = cameraPose[0];
// double y = cameraPose[1];
// double yaw = cameraPose[5];
Rotation2d rot = Rotation2d.fromDegrees(yaw);
// Rotation2d rot = Rotation2d.fromDegrees(yaw);
pose = new Pose2d(x, y, rot);
// pose = new Pose2d(x, y, rot);
boolean isRed = DriverStation.getAlliance().get() == Alliance.Red;
// boolean isRed = DriverStation.getAlliance().get() == Alliance.Red;
double distance;
// double distance;
if(isRed){
distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
}else{
distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
}
// if(isRed){
// distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
// }else{
// distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
// }
isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
// isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
//SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
//SmartDashboard.putNumber("speakerDistance", distance);
}
// //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
// //SmartDashboard.putNumber("speakerDistance", distance);
// }
public Pose2d getPose() {
return pose;
}
// public Pose2d getPose() {
// return pose;
// }
public boolean isNearSpeaker() {
return isNearSpeaker;
}
// public boolean isNearSpeaker() {
// return isNearSpeaker;
// }
@Override
public void periodic() {
// This method will be called once per scheduler run
// @Override
// public void periodic() {
// // This method will be called once per scheduler run
//isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
//double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
// //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
// //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
//if(newPose != cameraPose){
// cameraPose = newPose;
//update();
// //if(newPose != cameraPose){
// // cameraPose = newPose;
// //update();
// //}
// }
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -6
View File
@@ -62,7 +62,7 @@ public class SwerveDrive extends SubsystemBase {
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
@@ -144,7 +144,7 @@ public class SwerveDrive extends SubsystemBase {
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
@@ -183,22 +183,22 @@ public class SwerveDrive extends SubsystemBase {
public void resetGyro() {
gyro.reset();
rotTarget = 0.0;
rotTarget = gyro.getAngle();
}
public void resetGyroFlip() {
gyro.resetFlip();
rotTarget = 0.0;
rotTarget = gyro.getAngle();
}
public void resetGyroRightBlue() {
gyro.resetRightSideBlue();
rotTarget = 0.0;
rotTarget = gyro.getAngle();
}
public void resetGyroRightAmp() {
gyro.resetAmpSide();
rotTarget = 0.0;
rotTarget = gyro.getAngle();
}
public void stopModules() {
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+2 -2
View File
@@ -30,7 +30,7 @@ public class SwerveModule extends SubsystemBase {
private WPI_TalonFX angleMotor;
private CANCoder encoder;
private int selfid;
private ConfigurableDouble offsetGetter;
// private ConfigurableDouble offsetGetter;
private static int swerveId = 0;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -39,7 +39,7 @@ public class SwerveModule extends SubsystemBase {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.encoder = encoder;
this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
// this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
this.selfid = swerveId;
swerveId++;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();