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

Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

Browse Source
This commit is contained in:
66945
2022-03-22 16:26:46 -06:00
parent 2316b1d426 8af4535191
commit 2c01e41b2c
16 changed files with 368 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+16 -16
View File
@@ -46,18 +46,18 @@ public final class Constants {
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2; // *
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // *
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // *
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // *
public static final int LEFT_BACK_STEER_CAN_ID = 6; // *
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // *
public static final int RIGHT_BACK_STEER_CAN_ID = 8; // *
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // *
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // *
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // *
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // *
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
public static final int LEFT_BACK_STEER_CAN_ID = 6;
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int GYRO_ID = 14;
// offsets are in degrees
@@ -85,8 +85,8 @@ public final class Constants {
// swerve auto constants
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI));
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI/2));
public static final boolean PATH_RECORD_VELOCITY = true;
public static final double PATH_MAX_VELOCITY = 5.0;
public static final double PATH_MAX_ACCELERATION = 5.0;
@@ -326,8 +326,8 @@ public final class Constants {
public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?)
public static final double H_FOV = 59.6;
public static final double V_FOV = 45.7;
public static final double LIME_HIXELS = 920;
public static final double LIME_VIXELS = 720;
public static final double LIME_HIXELS = 640;
public static final double LIME_VIXELS = 480;
public static final double TURRET_kP = 0.1;
public static final double POINTS_THRESHOLD = 400;
src/main/java/frc4388/robot/Robot.java
+1 -2
View File
@@ -213,8 +213,7 @@ public class Robot extends TimedRobot {
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
src/main/java/frc4388/robot/RobotContainer.java
+59 -16
View File
@@ -46,6 +46,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Filesystem;
@@ -57,8 +59,10 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.NotifierCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Claws;
@@ -71,7 +75,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
import frc4388.robot.commands.ClimberCommands.RunClaw;
import frc4388.robot.commands.ClimberCommands.RunClimberPath;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
import frc4388.robot.commands.DriveCommands.RotateUntilTarget;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.AimToCenter;
import frc4388.robot.commands.ShooterCommands.Seek;
@@ -97,6 +102,7 @@ import frc4388.utility.PathPlannerUtil;
import frc4388.utility.Vector2D;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedRawXboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -128,8 +134,8 @@ public class RobotContainer {
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
// Autonomous
@@ -218,6 +224,11 @@ public class RobotContainer {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
}, m_robotClimber));
m_robotBoomBoom.setDefaultCommand(
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
);
// autoInit();
// recordInit();
}
@@ -267,8 +278,8 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
// X > Toggles extender in and out
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
// A > Spit Out Ball
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
@@ -281,11 +292,11 @@ public class RobotContainer {
//! Test Buttons
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
@@ -366,8 +377,11 @@ public class RobotContainer {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
PathPlannerState initState = traj.getInitialState();
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
@@ -378,12 +392,13 @@ public class RobotContainer {
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
@@ -416,6 +431,13 @@ public class RobotContainer {
// }).withName("No Autonomous Path");
// }
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
loadPath("Move Forward");
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
@@ -424,8 +446,30 @@ public class RobotContainer {
// * null,
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
// // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
// new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive),
// "Diamond"));
return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
// * assume turret is already pointed towards target.
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
new ParallelRaceGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
));
// TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
// * aim with RotateUntilTarget
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
// new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
// new ParallelRaceGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
// ));
}
public static XboxController getDriverController() {
@@ -558,7 +602,7 @@ public class RobotContainer {
}
private void saveRecording() {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
// ! IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = PATHPLANNER_DIRECTORY
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
@@ -579,8 +623,7 @@ public class RobotContainer {
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
// actual robot velocity.
// * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
m_robotSwerveDrive.getChassisSpeeds()[1]);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+59
View File
@@ -0,0 +1,59 @@
// 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.DriveCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
public class DriveWithInputForTime extends CommandBase {
// subsystems
SwerveDrive swerve;
double[] inputs;
// timing
long start;
long elapsed;
double duration;
/**
* DriveWithInput for a specified amount of time.
* @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot).
* @param time Time to DriveWithInput for, in seconds.
*/
public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.inputs = inputs;
this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
start = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
elapsed = System.currentTimeMillis() - start;
this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
}
// 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 (elapsed >= duration);
}
}
src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
+59
View File
@@ -0,0 +1,59 @@
// 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.DriveCommands;
import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.VisionObscuredException;
public class RotateUntilTarget extends CommandBase {
// subsystems
SwerveDrive swerve;
VisionOdometry visionOdometry;
double rotateSpeed;
/** Creates a new RotateUntilTarget. */
public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.visionOdometry = visionOdometry;
this.rotateSpeed = rotateSpeed;
addRequirements(this.swerve, this.visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.visionOdometry.setLEDs(true);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true);
}
// 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() {
try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; }
return true;
// return this.visionOdometry.m_camera.getLatestResult().hasTargets();
}
}
src/main/java/frc4388/robot/commands/RunCommandForTime.java
+78
View File
@@ -0,0 +1,78 @@
// 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;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
public class RunCommandForTime extends CommandBase {
// command
Command command;
// timing
long start;
long elapsed;
double duration;
// override
boolean override;
/**
* Runs given command for given time.
* @param command Command to run.
* @param duration Time to run for, in seconds.
* @param override If true: end command when time ends, even if the command isn't finished. If false: end command when it finished and time has ended.
*/
public RunCommandForTime(Command command, double duration, boolean override) {
// Use addRequirements() here to declare subsystem dependencies.
this.command = command;
this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
this.override = override;
addRequirements(this.command.getRequirements().toArray(Subsystem[]::new));
}
/**
* Runs given command for given time.
* @param command Command to run.
* @param duration Time to run for, in seconds.
*/
public RunCommandForTime(Command command, double duration) {
this(command, duration, false);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.start = System.currentTimeMillis();
this.command.initialize();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
this.elapsed = System.currentTimeMillis() - this.start;
this.command.execute();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
this.command.end(interrupted);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (this.override) {
return (this.elapsed >= this.duration);
} else {
return (this.command.isFinished() && (this.elapsed >= this.duration));
}
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+1 -1
View File
@@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry));
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+63 -30
View File
@@ -35,6 +35,8 @@ public class TrackTarget extends CommandBase {
BoomBoom m_boomBoom;
Hood m_hood;
boolean isAuto;
static double velocity;
static double hoodPosition;
@@ -46,19 +48,35 @@ public class TrackTarget extends CommandBase {
boolean isExecuted = false;
public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
m_swerve = swerve;
// timing
boolean isAimed;
boolean timerStarted;
long startTime;
private double timeTolerance;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
this.isAuto = isAuto;
this.timeTolerance = 1000;
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
}
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
this(turret, boomBoom, hood, visionOdometry, false);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timerStarted = false;
startTime = 0;
velocity = 0;
hoodPosition = 0;
}
@@ -67,32 +85,33 @@ public class TrackTarget extends CommandBase {
@Override
public void execute() {
try {
m_visionOdometry.setDriverMode(false);
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
points = filterPoints(points);
// points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2;
m_turret.runTurretWithInput(output);
double position = m_turret.m_boomBoomRotateEncoder.getPosition();
// double position = m_turret.m_boomBoomRotateEncoder.getPosition();
if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
else
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
true);
// if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
// Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
// else
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
// RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
// true);
double regressedDistance = getDistance(average.y);
// ! add 30 to the distance to get in target. May need to be adjusted
velocity = m_boomBoom.getVelocity(regressedDistance + 30);
hoodPosition = m_boomBoom.getHood(regressedDistance + 30);
// ! no longer a +30 lol -aarav
velocity = m_boomBoom.getVelocity(regressedDistance);
hoodPosition = m_boomBoom.getHood(regressedDistance);
m_boomBoom.runDrumShooterVelocityPID(velocity);
m_hood.runAngleAdjustPID(hoodPosition);
@@ -110,27 +129,30 @@ public class TrackTarget extends CommandBase {
} catch (Exception e){
e.printStackTrace();
}
// run storage
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
Point average = VisionOdometry.averagePoint(points);
// public ArrayList<Point> filterPoints(ArrayList<Point> points) {
// Point average = VisionOdometry.averagePoint(points);
HashMap<Point, Double> pointDistances = new HashMap<>();
double distanceSum = 0;
// HashMap<Point, Double> pointDistances = new HashMap<>();
// double distanceSum = 0;
for(Point point : points) {
Vector2D difference = new Vector2D(point);
difference.subtract(new Vector2D(average));
// for(Point point : points) {
// Vector2D difference = new Vector2D(point);
// difference.subtract(new Vector2D(average));
double mag = difference.magnitude();
distanceSum += mag;
// double mag = difference.magnitude();
// distanceSum += mag;
pointDistances.put(point, mag);
}
// pointDistances.put(point, mag);
// }
final double averageDist = distanceSum / points.size();
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
}
// final double averageDist = distanceSum / points.size();
// return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
// }
public final double getDistance(double averageY) {
double y_rot = averageY / VisionConstants.LIME_VIXELS;
@@ -142,7 +164,8 @@ public class TrackTarget extends CommandBase {
double regressedDistance = distanceRegression(distance);
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
SmartDashboard.putNumber("Distance from Lime 123", distance);
SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance);
return regressedDistance;
}
@@ -153,12 +176,22 @@ public class TrackTarget extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_visionOdometry.setLEDs(false);
m_visionOdometry.setDriverMode(true);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
if (this.isAuto) {
if (targetLocked& !timerStarted) {
timerStarted = true;
startTime = System.currentTimeMillis();
}
return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
} else {
return false;
}
// return isExecuted && Math.abs(output) < .1;
}
}
src/main/java/frc4388/robot/commands/StorageCommands/Drum.java
View File
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -4
View File
@@ -158,6 +158,13 @@ public class SwerveDrive extends SubsystemBase {
// modules[0].setDesiredState(desiredStates[0], false);
}
public void setModuleRotationsToAngle(double angle) {
for (int i = 0; i < modules.length; i++) {
SwerveModule module = modules[i];
module.rotateToAngle(angle);
}
}
@Override
public void periodic() {
@@ -228,10 +235,16 @@ public class SwerveDrive extends SubsystemBase {
* Updates the field relative position of the robot.
*/
public void updateOdometry() {
m_poseEstimator.update( getRegGyro(),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2));
Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians());
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+4
View File
@@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase {
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
this.angleMotor.set(TalonFXControlMode.Position, angle);
}
@Override
public void periodic() {
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+7
View File
@@ -47,6 +47,9 @@ public class VisionOdometry extends SubsystemBase {
m_camera = new PhotonCamera(VisionConstants.NAME);
m_drive = drive;
m_shooter = shooter;
setLEDs(false);
setDriverMode(true);
}
/** Gets the vision points from the limelight
@@ -93,6 +96,10 @@ public class VisionOdometry extends SubsystemBase {
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void setDriverMode(boolean driverMode) {
m_camera.setDriverMode(driverMode);
}
public Point getTargetOffset() throws VisionObscuredException {
ArrayList<Point> screenPoints = getTargetPoints();