1+ package org .usfirst .frc .team449 .robot .commands .multiInterface .drive ;
2+
3+ import com .fasterxml .jackson .annotation .*;
4+ import edu .wpi .first .wpilibj .smartdashboard .SmartDashboard ;
5+ import org .jetbrains .annotations .NotNull ;
6+ import org .jetbrains .annotations .Nullable ;
7+ import org .usfirst .frc .team449 .robot .drive .unidirectional .DriveUnidirectional ;
8+ import org .usfirst .frc .team449 .robot .jacksonWrappers .YamlSubsystem ;
9+ import org .usfirst .frc .team449 .robot .oi .fieldoriented .OIFieldOriented ;
10+ import org .usfirst .frc .team449 .robot .other .Logger ;
11+ import org .usfirst .frc .team449 .robot .subsystem .interfaces .AHRS .SubsystemAHRS ;
12+ import org .usfirst .frc .team449 .robot .subsystem .interfaces .AHRS .commands .PIDAngleCommand ;
13+
14+ import java .util .ArrayList ;
15+ import java .util .List ;
16+
17+ /**
18+ * Unidirectional drive with field-oriented control
19+ */
20+ @ JsonTypeInfo (use = JsonTypeInfo .Id .CLASS , include = JsonTypeInfo .As .WRAPPER_OBJECT , property = "@class" )
21+ @ JsonIdentityInfo (generator = ObjectIdGenerators .StringIdGenerator .class )
22+ public class FieldOrientedUnidirectionalDriveCommand <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS > extends PIDAngleCommand {
23+
24+ /**
25+ * The drive this command is controlling.
26+ */
27+ @ NotNull
28+ protected final T subsystem ;
29+
30+ /**
31+ * The OI giving the input stick values.
32+ */
33+ @ NotNull
34+ protected final OIFieldOriented oi ;
35+
36+ /**
37+ * The points to snap the PID controller input to.
38+ */
39+ @ NotNull
40+ private final List <AngularSnapPoint > snapPoints ;
41+
42+ /**
43+ * The absolute angular setpoint for the robot to go to. Field to avoid garbage collection.
44+ */
45+ @ Nullable
46+ private Double theta ;
47+
48+ /**
49+ * Default constructor
50+ *
51+ * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be considered on
52+ * target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0.
53+ * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered within
54+ * tolerance.
55+ * @param minimumOutput The minimum output of the loop. Defaults to zero.
56+ * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is used.
57+ * @param deadband The deadband around the setpoint, in degrees, within which no output is given to the
58+ * motors. Defaults to zero.
59+ * @param inverted Whether the loop is inverted. Defaults to false.
60+ * @param kP Proportional gain. Defaults to zero.
61+ * @param kI Integral gain. Defaults to zero.
62+ * @param kD Derivative gain. Defaults to zero.
63+ * @param subsystem The drive to execute this command on.
64+ * @param oi The OI controlling the robot.
65+ * @param snapPoints The points to snap the PID controller input to.
66+ */
67+ @ JsonCreator
68+ public FieldOrientedUnidirectionalDriveCommand (@ JsonProperty (required = true ) double absoluteTolerance ,
69+ int toleranceBuffer ,
70+ double minimumOutput , @ Nullable Double maximumOutput ,
71+ double deadband ,
72+ boolean inverted ,
73+ double kP ,
74+ double kI ,
75+ double kD ,
76+ @ NotNull @ JsonProperty (required = true ) T subsystem ,
77+ @ NotNull @ JsonProperty (required = true ) OIFieldOriented oi ,
78+ @ Nullable List <AngularSnapPoint > snapPoints ) {
79+ //Assign stuff
80+ super (absoluteTolerance , toleranceBuffer , minimumOutput , maximumOutput , deadband , inverted , subsystem , kP , kI , kD );
81+ this .oi = oi ;
82+ this .subsystem = subsystem ;
83+ this .snapPoints = snapPoints != null ? snapPoints : new ArrayList <>();
84+
85+ //Needs a requires because it's a default command.
86+ requires (this .subsystem );
87+
88+ //Logging, but in Spanish.
89+ Logger .addEvent ("Drive Robot bueno" , this .getClass ());
90+ }
91+
92+ /**
93+ * Initialize PIDController and variables.
94+ */
95+ @ Override
96+ protected void initialize () {
97+ //Reset all values of the PIDController and enable it.
98+ this .getPIDController ().reset ();
99+ this .getPIDController ().enable ();
100+ Logger .addEvent ("FieldOrientedUnidirectionalDriveCommand init." , this .getClass ());
101+ }
102+
103+ /**
104+ * Set PID setpoint to processed controller setpoint.
105+ */
106+ @ Override
107+ protected void execute () {
108+ theta = oi .getThetaCached ();
109+ if (theta != null ) {
110+ for (AngularSnapPoint snapPoint : snapPoints ) {
111+ //See if we should snap
112+ if (snapPoint .getLowerBound () < theta && theta < snapPoint .getUpperBound ()) {
113+ theta = snapPoint .getSnapTo ();
114+ //Break to shorten runtime, we'll never snap twice.
115+ break ;
116+ }
117+ }
118+ this .getPIDController ().setSetpoint (theta );
119+ SmartDashboard .putNumber ("Theta navx setpoint" , theta );
120+ }
121+ }
122+
123+ /**
124+ * Run constantly because this is a defaultDrive
125+ *
126+ * @return false
127+ */
128+ @ Override
129+ protected boolean isFinished () {
130+ return false ;
131+ }
132+
133+ /**
134+ * Log when this command ends
135+ */
136+ @ Override
137+ protected void end () {
138+ Logger .addEvent ("FieldOrientedUnidirectionalDriveCommand End." , this .getClass ());
139+ }
140+
141+ /**
142+ * Log when this command is interrupted.
143+ */
144+ @ Override
145+ protected void interrupted () {
146+ Logger .addEvent ("FieldOrientedUnidirectionalDriveCommand Interrupted!" , this .getClass ());
147+ }
148+
149+ /**
150+ * Give the correct output to the motors based on the PID output and velocity input.
151+ *
152+ * @param output The output of the angular PID loop.
153+ */
154+ @ Override
155+ protected void usePIDOutput (double output ) {
156+ SmartDashboard .putNumber ("Unprocessed loop output" , output );
157+ //Process or zero the input depending on whether the NavX is being overriden.
158+ output = subsystem .getOverrideGyro () ? 0 : processPIDOutput (output );
159+
160+ SmartDashboard .putNumber ("PID loop output" , output );
161+
162+ //Adjust the heading according to the PID output, it'll be positive if we want to go right.
163+ subsystem .setOutput (oi .getVelCached () - output , oi .getVelCached () + output );
164+ }
165+
166+ /**
167+ * A data-holding class representing an angular setpoint to "snap" the controller output to.
168+ */
169+ protected static class AngularSnapPoint {
170+
171+ /**
172+ * The angle to snap the setpoint to, in degrees.
173+ */
174+ private final double snapTo ;
175+
176+ /**
177+ * The upper bound, below which all angles above snapTo are changed to snapTo. Measured in degrees.
178+ */
179+ private final double upperBound ;
180+
181+ /**
182+ * The lower bound, above which all angles below snapTo are changed to snapTo. Measured in degrees.
183+ */
184+ private final double lowerBound ;
185+
186+ /**
187+ * Default constructor.
188+ *
189+ * @param snapTo The angle to snap the setpoint to, in degrees.
190+ * @param upperBound The upper bound, below which all angles above snapTo are changed to snapTo. Measured in
191+ * degrees.
192+ * @param lowerBound The lower bound, above which all angles below snapTo are changed to snapTo. Measured in
193+ * degrees.
194+ */
195+ @ JsonCreator
196+ public AngularSnapPoint (@ JsonProperty (required = true ) double snapTo ,
197+ @ JsonProperty (required = true ) double upperBound ,
198+ @ JsonProperty (required = true ) double lowerBound ) {
199+ this .snapTo = snapTo ;
200+ this .upperBound = upperBound ;
201+ this .lowerBound = lowerBound ;
202+ }
203+
204+ /**
205+ * @return The angle to snap the setpoint to, in degrees.
206+ */
207+ public double getSnapTo () {
208+ return snapTo ;
209+ }
210+
211+ /**
212+ * @return The upper bound, below which all angles above snapTo are changed to snapTo. Measured in degrees.
213+ */
214+ public double getUpperBound () {
215+ return upperBound ;
216+ }
217+
218+ /**
219+ * @return The lower bound, above which all angles below snapTo are changed to snapTo. Measured in degrees.
220+ */
221+ public double getLowerBound () {
222+ return lowerBound ;
223+ }
224+ }
225+ }
0 commit comments