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

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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
@@ -25,6 +25,7 @@ import frc4388.utility.desmos.DesmosServer;
public class TrackTarget extends CommandBase {
/** Creates a new TrackTarget. */
SwerveDrive m_swerve;
Turret m_turret;
VisionOdometry m_visionOdometry;
BoomBoom m_boomBoom;
@@ -51,8 +52,9 @@ public class TrackTarget extends CommandBase {
// 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.
m_swerve = swerve;
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
@@ -96,10 +98,16 @@ public class TrackTarget extends CommandBase {
}
output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 4;
// output *= 0.5;
DesmosServer.putDouble("output", output);
output *= 2;
// //output *= 0.5;
// //DesmosServer.putDouble("output", 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;
y_rot *= Math.toRadians(VisionConstants.V_FOV);