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

Heat seeking guided missiles

Browse Source
This commit is contained in:
66945
2022-03-20 16:25:27 -06:00
parent f2c86c4888
commit 5d9d3ac1f1
1 changed files with 13 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+13 -5
View File
@@ -12,7 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants; import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Hood;
@@ -25,6 +25,7 @@ import frc4388.utility.desmos.DesmosServer;
public class TrackTarget extends CommandBase { public class TrackTarget extends CommandBase {
/** Creates a new TrackTarget. */ /** Creates a new TrackTarget. */
SwerveDrive m_swerve;
Turret m_turret; Turret m_turret;
VisionOdometry m_visionOdometry; VisionOdometry m_visionOdometry;
BoomBoom m_boomBoom; BoomBoom m_boomBoom;
@@ -51,8 +52,9 @@ public class TrackTarget extends CommandBase {
// public static Gains m_aimGains; // public static Gains m_aimGains;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
m_swerve = swerve;
m_turret = turret; m_turret = turret;
m_boomBoom = boomBoom; m_boomBoom = boomBoom;
m_hood = hood; m_hood = hood;
@@ -96,10 +98,16 @@ public class TrackTarget extends CommandBase {
} }
output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 4; output *= 2;
// output *= 0.5; // //output *= 0.5;
DesmosServer.putDouble("output", output); // //DesmosServer.putDouble("output", output);
m_turret.runTurretWithInput(output); m_turret.runTurretWithInput(output);
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(0, 0, output, false);
double y_rot = average.y / VisionConstants.LIME_VIXELS; double y_rot = average.y / VisionConstants.LIME_VIXELS;
y_rot *= Math.toRadians(VisionConstants.V_FOV); y_rot *= Math.toRadians(VisionConstants.V_FOV);