forked from frc-team-3341/2022-IAP-Turret
-
Notifications
You must be signed in to change notification settings - Fork 0
/
SearchAndSpin.java
92 lines (64 loc) · 2.45 KB
/
SearchAndSpin.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
// 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 frc.robot.commands;
import frc.robot.RobotContainer;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Limelight;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
public class SearchAndSpin extends CommandBase {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final Limelight limeLight;
private final DriveTrain driveTrain;
private double target;
private double error;
public SearchAndSpin(DriveTrain driveTrain, Limelight limeLight, double target) {
this.driveTrain = driveTrain;
this.limeLight = limeLight;
this.target = target;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
error = target - limeLight.get_tx();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
error = target - limeLight.get_tx();
double speed = .3*error/30;
SmartDashboard.putNumber("Error", error);
SmartDashboard.putNumber("LimelightTVNUM", RobotContainer.getLime().get_tv());
SmartDashboard.putNumber("LimelightX", RobotContainer.getLime().get_tx());
SmartDashboard.putNumber("LimelightY", RobotContainer.getLime().get_ty());
SmartDashboard.putNumber("Speed", speed);
if(limeLight.get_tv() == 1){
if(Math.abs(speed) < .2){
speed = .3 * Math.abs(error)/error;
}
if(Math.abs(error) <= 6){
driveTrain.tankDrive(0,0);
} else{
driveTrain.tankDrive(-speed, speed);
}
} else if(limeLight.get_tv() == 0){
driveTrain.tankDrive(0.2,-0.2);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
SmartDashboard.putBoolean("Interrupt", interrupted);
// This is called when there is a target in sight, so the chassis will stop turning completely
driveTrain.tankDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// This code should run forever until there is a valid target (reflective tape or yellow ball)
//in sight and will continue to repeat
return false;
}
}