001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007
008 package edu.wpi.first.wpilibj.command;
009
010 import edu.wpi.first.wpilibj.PIDController;
011 import edu.wpi.first.wpilibj.PIDOutput;
012 import edu.wpi.first.wpilibj.PIDSource;
013 import edu.wpi.first.wpilibj.Sendable;
014 import edu.wpi.first.wpilibj.tables.ITable;
015
016 /**
017 * This class defines a {@link Command} which interacts heavily with a PID loop.
018 *
019 * <p>It provides some convenience methods to run an internal {@link PIDController}.
020 * It will also start and stop said {@link PIDController} when the {@link PIDCommand} is
021 * first initialized and ended/interrupted.</p>
022 *
023 * @author Joe Grinstead
024 */
025 public abstract class PIDCommand extends Command implements Sendable{
026
027 /** The internal {@link PIDController} */
028 private PIDController controller;
029 /** An output which calls {@link PIDCommand#usePIDOutput(double)} */
030 private PIDOutput output = new PIDOutput() {
031
032 public void pidWrite(double output) {
033 usePIDOutput(output);
034 }
035 };
036 /** A source which calls {@link PIDCommand#returnPIDInput()} */
037 private PIDSource source = new PIDSource() {
038
039 public double pidGet() {
040 return returnPIDInput();
041 }
042 };
043
044 /**
045 * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
046 * @param name the name of the command
047 * @param p the proportional value
048 * @param i the integral value
049 * @param d the derivative value
050 */
051 public PIDCommand(String name, double p, double i, double d) {
052 super(name);
053 controller = new PIDController(p, i, d, source, output);
054 }
055
056 /**
057 * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space the time
058 * between PID loop calculations to be equal to the given period.
059 * @param name the name
060 * @param p the proportional value
061 * @param i the integral value
062 * @param d the derivative value
063 * @param period the time (in seconds) between calculations
064 */
065 public PIDCommand(String name, double p, double i, double d, double period) {
066 super(name);
067 controller = new PIDController(p, i, d, source, output, period);
068 }
069
070 /**
071 * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
072 * It will use the class name as its name.
073 * @param p the proportional value
074 * @param i the integral value
075 * @param d the derivative value
076 */
077 public PIDCommand(double p, double i, double d) {
078 controller = new PIDController(p, i, d, source, output);
079 }
080
081 /**
082 * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
083 * It will use the class name as its name..
084 * It will also space the time
085 * between PID loop calculations to be equal to the given period.
086 * @param p the proportional value
087 * @param i the integral value
088 * @param d the derivative value
089 * @param period the time (in seconds) between calculations
090 */
091 public PIDCommand(double p, double i, double d, double period) {
092 controller = new PIDController(p, i, d, source, output, period);
093 }
094
095 /**
096 * Returns the {@link PIDController} used by this {@link PIDCommand}.
097 * Use this if you would like to fine tune the pid loop.
098 *
099 * <p>Notice that calling {@link PIDController#setSetpoint(double) setSetpoint(...)} on the controller
100 * will not result in the setpoint being trimmed to be in
101 * the range defined by {@link PIDCommand#setSetpointRange(double, double) setSetpointRange(...)}.</p>
102 *
103 * @return the {@link PIDController} used by this {@link PIDCommand}
104 */
105 protected PIDController getPIDController() {
106 return controller;
107 }
108
109 void _initialize() {
110 controller.enable();
111 }
112
113 void _end() {
114 controller.disable();
115 }
116
117 void _interrupted() {
118 _end();
119 }
120
121 /**
122 * Adds the given value to the setpoint.
123 * If {@link PIDCommand#setRange(double, double) setRange(...)} was used,
124 * then the bounds will still be honored by this method.
125 * @param deltaSetpoint the change in the setpoint
126 */
127 public void setSetpointRelative(double deltaSetpoint) {
128 setSetpoint(getSetpoint() + deltaSetpoint);
129 }
130
131 /**
132 * Sets the setpoint to the given value. If {@link PIDCommand#setRange(double, double) setRange(...)}
133 * was called,
134 * then the given setpoint
135 * will be trimmed to fit within the range.
136 * @param setpoint the new setpoint
137 */
138 protected void setSetpoint(double setpoint) {
139 controller.setSetpoint(setpoint);
140 }
141
142 /**
143 * Returns the setpoint.
144 * @return the setpoint
145 */
146 protected double getSetpoint() {
147 return controller.getSetpoint();
148 }
149
150 /**
151 * Returns the current position
152 * @return the current position
153 */
154 protected double getPosition() {
155 return returnPIDInput();
156 }
157
158 /**
159 * Returns the input for the pid loop.
160 *
161 * <p>It returns the input for the pid loop, so if this command was based
162 * off of a gyro, then it should return the angle of the gyro</p>
163 *
164 * <p>All subclasses of {@link PIDCommand} must override this method.</p>
165 *
166 * <p>This method will be called in a different thread then the {@link Scheduler} thread.</p>
167 *
168 * @return the value the pid loop should use as input
169 */
170 protected abstract double returnPIDInput();
171
172 /**
173 * Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
174 * This method is a good time to set motor values, maybe something along the lines of <code>driveline.tankDrive(output, -output)</code>
175 *
176 * <p>All subclasses of {@link PIDCommand} must override this method.</p>
177 *
178 * <p>This method will be called in a different thread then the {@link Scheduler} thread.</p>
179 *
180 * @param output the value the pid loop calculated
181 */
182 protected abstract void usePIDOutput(double output);
183
184 public String getSmartDashboardType(){
185 return "PIDCommand";
186 }
187 public void initTable(ITable table){
188 controller.initTable(table);
189 super.initTable(table);
190 }
191 }