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

sorta working / some fixes (namely funky turning after shooter)

Browse Source
This commit is contained in:
Ryan
2022-03-21 20:00:34 -06:00
parent 065e48fed1
commit 4c5106d14f
4 changed files with 40 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -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 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 H_FOV = 59.6;
public static final double V_FOV = 45.7; public static final double V_FOV = 45.7;
public static final double LIME_HIXELS = 920; public static final double LIME_HIXELS = 640;
public static final double LIME_VIXELS = 720; public static final double LIME_VIXELS = 480;
public static final double TURRET_kP = 0.1; public static final double TURRET_kP = 0.1;
public static final double POINTS_THRESHOLD = 400; public static final double POINTS_THRESHOLD = 400;
src/main/java/frc4388/robot/RobotContainer.java
+11 -6
View File
@@ -218,6 +218,11 @@ public class RobotContainer {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
}, m_robotClimber)); }, m_robotClimber));
m_robotBoomBoom.setDefaultCommand(
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
);
// autoInit(); // autoInit();
// recordInit(); // recordInit();
} }
@@ -267,8 +272,8 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
// X > Toggles extender in and out // X > Toggles extender in and out
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value) new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
// A > Spit Out Ball // A > Spit Out Ball
new JoystickButton(getOperatorController(), XboxController.Button.kA.value) new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
@@ -281,11 +286,11 @@ public class RobotContainer {
//! Test Buttons //! Test Buttons
new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value) new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
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) { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
// Add your commands in the addCommands() call, e.g. // Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand()); // 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
+26 -26
View File
@@ -46,8 +46,7 @@ public class TrackTarget extends CommandBase {
boolean isExecuted = false; boolean isExecuted = false;
public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
m_swerve = swerve;
m_turret = turret; m_turret = turret;
m_boomBoom = boomBoom; m_boomBoom = boomBoom;
m_hood = hood; m_hood = hood;
@@ -71,22 +70,22 @@ public class TrackTarget extends CommandBase {
m_visionOdometry.setLEDs(true); m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints(); points = m_visionOdometry.getTargetPoints();
points = filterPoints(points); // points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points); Point average = VisionOdometry.averagePoint(points);
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2; output *= 2;
m_turret.runTurretWithInput(output); 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 || // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) // Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
else // else
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
true); // true);
double regressedDistance = getDistance(average.y); double regressedDistance = getDistance(average.y);
@@ -113,25 +112,25 @@ public class TrackTarget extends CommandBase {
} }
} }
public ArrayList<Point> filterPoints(ArrayList<Point> points) { // public ArrayList<Point> filterPoints(ArrayList<Point> points) {
Point average = VisionOdometry.averagePoint(points); // Point average = VisionOdometry.averagePoint(points);
HashMap<Point, Double> pointDistances = new HashMap<>(); // HashMap<Point, Double> pointDistances = new HashMap<>();
double distanceSum = 0; // double distanceSum = 0;
for(Point point : points) { // for(Point point : points) {
Vector2D difference = new Vector2D(point); // Vector2D difference = new Vector2D(point);
difference.subtract(new Vector2D(average)); // difference.subtract(new Vector2D(average));
double mag = difference.magnitude(); // double mag = difference.magnitude();
distanceSum += mag; // distanceSum += mag;
pointDistances.put(point, mag); // pointDistances.put(point, mag);
} // }
final double averageDist = distanceSum / points.size(); // final double averageDist = distanceSum / points.size();
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); // return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
} // }
public final double getDistance(double averageY) { public final double getDistance(double averageY) {
double y_rot = averageY / VisionConstants.LIME_VIXELS; double y_rot = averageY / VisionConstants.LIME_VIXELS;
@@ -143,7 +142,8 @@ public class TrackTarget extends CommandBase {
double regressedDistance = distanceRegression(distance); double regressedDistance = distanceRegression(distance);
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; 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; return regressedDistance;
} }