diff --git a/src/main/java/com/thegongoliers/commands/RotateToAngle.java b/src/main/java/com/thegongoliers/commands/RotateToAngle.java deleted file mode 100644 index 60818a7..0000000 --- a/src/main/java/com/thegongoliers/commands/RotateToAngle.java +++ /dev/null @@ -1,65 +0,0 @@ -package com.thegongoliers.commands; - -import com.thegongoliers.annotations.UsedInCompetition; -import com.thegongoliers.output.interfaces.Drivetrain; -import com.thegongoliers.pathFollowing.controllers.MotionController; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.interfaces.Gyro; - -/** - * A command to rotate the robot by an angle - * - */ -@UsedInCompetition(team = "5112", year = "2019") -public class RotateToAngle extends Command { - - private double targetAngle; - private Drivetrain drivetrain; - private Gyro gyro; - private MotionController controller; - - /** - * Default constructor - * - * @param subsystem the subsystem to require - * @param drivetrain the drivetrain - * @param gyro the gyroscope - * @param controller the motion controller which determines percent power given a target angle - * @param targetAngle the target angle of rotation, relative to the current position - */ - public RotateToAngle(Subsystem subsystem, Drivetrain drivetrain, Gyro gyro, MotionController controller, double targetAngle) { - requires(subsystem); - this.targetAngle = targetAngle; - this.drivetrain = drivetrain; - this.gyro = gyro; - this.controller = controller; - } - - @Override - protected void initialize() { - targetAngle += gyro.getAngle(); - } - - @Override - protected void execute() { - double pwm = controller.calculate(gyro.getAngle(), targetAngle); - drivetrain.arcade(0, pwm); - } - - @Override - protected boolean isFinished() { - return controller.isOnTarget(gyro.getAngle(), targetAngle); - } - - @Override - protected void end() { - drivetrain.stop(); - } - - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/main/java/com/thegongoliers/pathFollowing/FollowPathCommand.java b/src/main/java/com/thegongoliers/pathFollowing/FollowPathCommand.java deleted file mode 100644 index 3ab9b2d..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/FollowPathCommand.java +++ /dev/null @@ -1,18 +0,0 @@ - package com.thegongoliers.pathFollowing; - - import com.thegongoliers.annotations.UsedInCompetition; - import edu.wpi.first.wpilibj.command.CommandGroup; - - @UsedInCompetition(team = "5112", year = "2018") - public class FollowPathCommand extends CommandGroup { - - /** - * Creates a command to follow a path. - * @param path The path to follow. - */ - public FollowPathCommand(Path path){ - for (PathTaskCommand command: path){ - addSequential(command); - } - } - } diff --git a/src/main/java/com/thegongoliers/pathFollowing/Path.java b/src/main/java/com/thegongoliers/pathFollowing/Path.java deleted file mode 100644 index b915da2..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/Path.java +++ /dev/null @@ -1,70 +0,0 @@ - package com.thegongoliers.pathFollowing; - - import com.thegongoliers.annotations.UsedInCompetition; - import com.thegongoliers.input.odometry.DistanceSensor; - import com.thegongoliers.output.interfaces.Drivetrain; - import com.thegongoliers.pathFollowing.controllers.MotionController; - import edu.wpi.first.wpilibj.command.Subsystem; - import edu.wpi.first.wpilibj.interfaces.Gyro; - - import java.util.Iterator; - import java.util.LinkedList; - import java.util.List; - import java.util.Objects; - - @UsedInCompetition(team = "5112", year = "2018") - public class Path implements Iterable { - private List path; - private Subsystem subsystem; - private Drivetrain drivetrain; - private Gyro gyro; - private DistanceSensor encoder; - private MotionController turnController, straightController; - - public Path(Subsystem subsystem, Drivetrain drivetrain, Gyro gyro, DistanceSensor encoder, MotionController turnController, MotionController straightController) { - path = new LinkedList<>(); - this.subsystem = subsystem; - this.drivetrain = drivetrain; - this.gyro = gyro; - this.encoder = encoder; - this.turnController = turnController; - this.straightController = straightController; - } - - public void addStraightAway(double distance) { - path.add(new PathStraightAwayCommand(subsystem, drivetrain, encoder, straightController, distance)); - } - - public void addRotation(double degrees) { - path.add(new PathRotateCommand(subsystem, drivetrain, gyro, turnController, degrees)); - } - - public void addStraightAway(double distance, double timeout) { - path.add(new PathStraightAwayCommand(subsystem, drivetrain, encoder, straightController, distance, timeout)); - } - - public void addRotation(double degrees, double timeout) { - path.add(new PathRotateCommand(subsystem, drivetrain, gyro, turnController, degrees, timeout)); - } - - @Override - public Iterator iterator() { - return path.iterator(); - } - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - Path that = (Path) o; - return Objects.equals(path, that.path) && - Objects.equals(drivetrain, that.drivetrain); - } - - @Override - public String toString() { - return "Path{" + - "path=" + path + - '}'; - } - } diff --git a/src/main/java/com/thegongoliers/pathFollowing/PathRotateCommand.java b/src/main/java/com/thegongoliers/pathFollowing/PathRotateCommand.java deleted file mode 100644 index 814cf99..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/PathRotateCommand.java +++ /dev/null @@ -1,61 +0,0 @@ - package com.thegongoliers.pathFollowing; - - import com.thegongoliers.annotations.UsedInCompetition; - import com.thegongoliers.output.interfaces.Drivetrain; - import com.thegongoliers.pathFollowing.controllers.MotionController; - import edu.wpi.first.wpilibj.command.Subsystem; - import edu.wpi.first.wpilibj.interfaces.Gyro; - - @UsedInCompetition(team = "5112", year = "2018") - public class PathRotateCommand extends PathTaskCommand { - - private double angleDegrees; - private Gyro gyro; - private MotionController motionController; - - public PathRotateCommand(Subsystem subsystem, Drivetrain drivetrain, Gyro gyro, MotionController controller, double angleDegrees) { - super(subsystem, drivetrain); - this.angleDegrees = angleDegrees; - this.gyro = gyro; - motionController = controller; - } - - public PathRotateCommand(Subsystem subsystem, Drivetrain drivetrain, Gyro gyro, MotionController controller, double angleDegrees, double timeout) { - super(subsystem, drivetrain, timeout); - this.angleDegrees = angleDegrees; - this.gyro = gyro; - motionController = controller; - } - - @Override - protected void initialize() { - angleDegrees += gyro.getAngle(); - } - - @Override - protected void execute() { - double currentAngle = gyro.getAngle(); - double pwm = motionController.calculate(currentAngle, angleDegrees); - drivetrain.arcade(0, pwm); - } - - @Override - protected boolean isFinished() { - return motionController.isOnTarget(gyro.getAngle(), angleDegrees); - } - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - PathRotateCommand that = (PathRotateCommand) o; - return Double.compare(that.angleDegrees, angleDegrees) == 0; - } - - @Override - public String toString() { - return "PathRotateCommand{" + - "angleDegrees=" + angleDegrees + - '}'; - } - } diff --git a/src/main/java/com/thegongoliers/pathFollowing/PathStraightAwayCommand.java b/src/main/java/com/thegongoliers/pathFollowing/PathStraightAwayCommand.java deleted file mode 100644 index 4ba2625..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/PathStraightAwayCommand.java +++ /dev/null @@ -1,62 +0,0 @@ - package com.thegongoliers.pathFollowing; - - import com.thegongoliers.annotations.UsedInCompetition; - import com.thegongoliers.input.odometry.DistanceSensor; - import com.thegongoliers.output.interfaces.Drivetrain; - import com.thegongoliers.pathFollowing.controllers.MotionController; - import edu.wpi.first.wpilibj.command.Subsystem; - - - @UsedInCompetition(team = "5112", year = "2018") - public class PathStraightAwayCommand extends PathTaskCommand { - - private double distance; - private DistanceSensor encoder; - private MotionController motionController; - - public PathStraightAwayCommand(Subsystem subsystem, Drivetrain drivetrain, DistanceSensor encoder, MotionController controller, double distance) { - super(subsystem, drivetrain); - this.distance = distance; - this.encoder = encoder; - motionController = controller; - } - - public PathStraightAwayCommand(Subsystem subsystem, Drivetrain drivetrain, DistanceSensor encoder, MotionController controller, double distance, double timeout) { - super(subsystem, drivetrain, timeout); - this.distance = distance; - this.encoder = encoder; - motionController = controller; - } - - @Override - protected void initialize() { - distance += encoder.getDistance(); - } - - @Override - protected void execute() { - double currentDistance = encoder.getDistance(); - double pwm = motionController.calculate(currentDistance, distance); - drivetrain.arcade(pwm, 0); - } - - @Override - protected boolean isFinished() { - return motionController.isOnTarget(encoder.getDistance(), distance); - } - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - PathStraightAwayCommand that = (PathStraightAwayCommand) o; - return Double.compare(that.distance, distance) == 0; - } - - @Override - public String toString() { - return "PathStraightAwayCommand{" + - "distance=" + distance + - '}'; - } - } diff --git a/src/main/java/com/thegongoliers/pathFollowing/PathTaskCommand.java b/src/main/java/com/thegongoliers/pathFollowing/PathTaskCommand.java deleted file mode 100644 index 45e0d47..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/PathTaskCommand.java +++ /dev/null @@ -1,39 +0,0 @@ - package com.thegongoliers.pathFollowing; - - import com.thegongoliers.annotations.UsedInCompetition; - import com.thegongoliers.output.interfaces.Drivetrain; - import edu.wpi.first.wpilibj.command.Command; - import edu.wpi.first.wpilibj.command.Subsystem; - - @UsedInCompetition(team = "5112", year = "2018") - public abstract class PathTaskCommand extends Command { - - protected Drivetrain drivetrain; - - public PathTaskCommand(Subsystem subsystem, Drivetrain drivetrain){ - requires(subsystem); - this.drivetrain = drivetrain; - } - - public PathTaskCommand(Subsystem subsystem, Drivetrain drivetrain, double timeout){ - requires(subsystem); - this.drivetrain = drivetrain; - setTimeout(timeout); - } - - @Override - abstract protected void execute(); - - @Override - abstract protected boolean isFinished(); - - @Override - protected void end() { - drivetrain.stop(); - } - - @Override - protected void interrupted() { - end(); - } - } diff --git a/src/main/java/com/thegongoliers/pathFollowing/controllers/MotionController.java b/src/main/java/com/thegongoliers/pathFollowing/controllers/MotionController.java deleted file mode 100644 index ae72970..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/controllers/MotionController.java +++ /dev/null @@ -1,21 +0,0 @@ -package com.thegongoliers.pathFollowing.controllers; - -public interface MotionController { - - /** - * Calculate the power to move to the target position - * @param current the current position - * @param target the target position - * @return the power to move to the target position - */ - double calculate(double current, double target); - - /** - * Determines if the mechanism is on target - * @param current the current position - * @param target the target position - * @return true if the mechanism is on target otherwise false - */ - boolean isOnTarget(double current, double target); - -} diff --git a/src/main/java/com/thegongoliers/pathFollowing/controllers/MotionProfileController.java b/src/main/java/com/thegongoliers/pathFollowing/controllers/MotionProfileController.java deleted file mode 100644 index 5ea6d4a..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/controllers/MotionProfileController.java +++ /dev/null @@ -1,212 +0,0 @@ -package com.thegongoliers.pathFollowing.controllers; - -import com.thegongoliers.input.time.Clock; -import com.thegongoliers.input.time.RobotClock; -import com.thegongoliers.math.GMath; - -public class MotionProfileController implements MotionController { - - private final double kp; - private final double ki; - private final double kd; - private final double kffv; - private final double kffa; - private final double tolerance; - - private double prevError; - - private double iState; - private double maxOutput = 1; - private double minOutput = -1; - private double maxI = maxOutput; - private double minI = minOutput; - - private Clock clock; - - private double lastTime = -1; - - /** - * Generates a simple motion profile controller - * @param kp The proportional feedback constant. - * @param ki The integral feedback constant. - * @param kd The derivative feedback constant. - * @param kffv The velocity feed forward constant. Typically 1 / max velocity. - * @param kffa The acceleration feed forward constant. - * @param tolerance The absolute tolerance. - */ - public MotionProfileController(double kp, double ki, double kd, double kffv, double kffa, double tolerance) { - this(kp, ki, kd, kffv, kffa, tolerance, new RobotClock()); - } - - /** - * Generates a simple motion profile controller - * @param kp The proportional feedback constant. - * @param ki The integral feedback constant. - * @param kd The derivative feedback constant. - * @param kffv The velocity feed forward constant. Typically 1 / max velocity. - * @param kffa The acceleration feed forward constant. - * @param tolerance The absolute tolerance. - * @param clock The clock to use. - */ - public MotionProfileController(double kp, double ki, double kd, double kffv, double kffa, double tolerance, Clock clock) { - this.kp = kp; - this.ki = ki; - this.kd = kd; - this.kffv = kffv; - this.kffa = kffa; - this.tolerance = tolerance; - this.clock = clock; - } - - /** - * Gets the maximum output of the controller. - * @return The maximum output. - */ - public double getMaxOutput() { - return maxOutput; - } - - /** - * Sets the maximum output of the controller. - * @param maxOutput The maximum output of the controller. - */ - public void setMaxOutput(double maxOutput) { - this.maxOutput = maxOutput; - } - - /** - * Gets the minimum output of the controller. - * @return The minimum output. - */ - public double getMinOutput() { - return minOutput; - } - - /** - * Sets the minimum output of the controller. - * @param minOutput The minimum output of the controller. - */ - public void setMinOutput(double minOutput) { - this.minOutput = minOutput; - } - - /** - * Get the maximum allowed integral error. - * @return The maximum integral error. - */ - public double getMaxIntegralError() { - return maxI; - } - - /** - * Sets the maximum allowed integral error. - * @param maxIntegralError The maximum integral error. - */ - public void setMaxIntegralError(double maxIntegralError) { - this.maxI = maxIntegralError; - } - - /** - * Gets the minimum allowed integral error. - * @return The minimum allowed integral error. - */ - public double getMinIntegralError() { - return minI; - } - - /** - * Sets the minimum allowed integral error. - * @param minIntegralError The minimum allowed integral error. - */ - public void setMinIntegralError(double minIntegralError) { - this.minI = minIntegralError; - } - - /** - * Gets the clock being used. - * @return The clock. - */ - public Clock getClock() { - return clock; - } - - /** - * Sets the clock to be used. - * @param clock The clock. - */ - public void setClock(Clock clock) { - this.clock = clock; - } - - /** - * Resets the controller's state. Use this before using the controller another time. - */ - public void reset(){ - iState = 0; - prevError = 0; - lastTime = -1; - } - - /** - * @see MotionController#isOnTarget(double, double) - */ - public boolean isOnTarget(double currentPosition, double targetPosition){ - double error = targetPosition - currentPosition; - return Math.abs(error) <= tolerance; - } - - /** - * Calculate the output value of the controller. - * @param currentPosition The current position of the system. - * @param targetPosition The target position of the system. - * @param targetVelocity The target velocity of the system. - * @param targetAcceleration The target acceleration of the system. - * @return The output of the controller. - */ - public double calculate(double currentPosition, double targetPosition, double targetVelocity, double targetAcceleration){ - double error = targetPosition - currentPosition; - - double pTerm = kp * error; - double dTerm = 0; - double vTerm = kffv * targetVelocity; - double aTerm = kffa * targetAcceleration; - - iState += error; - iState = GMath.clamp(iState, minI, maxI); - double iTerm = ki * iState; - - double currentTime = clock.getTime(); - - if(lastTime != -1){ - dTerm = kd * ((error - prevError) / (currentTime - lastTime) - targetVelocity); - } - - - - double value = pTerm + iTerm + dTerm + vTerm + aTerm; - - lastTime = currentTime; - prevError = error; - - return GMath.clamp(value, minOutput, maxOutput); - } - - /** - * Calculate the output value of the controller. - * @param currentPosition The current position of the system. - * @param targetPosition The target position of the system. - * @param targetVelocity The target velocity of the system. - * @return The output of the controller. - */ - public double calculate(double currentPosition, double targetPosition, double targetVelocity){ - return calculate(currentPosition, targetPosition, targetVelocity, 0); - } - - /** - * @see MotionController#calculate(double, double) - */ - public double calculate(double currentPosition, double targetPosition){ - return calculate(currentPosition, targetPosition, 0, 0); - } - -} diff --git a/src/main/java/com/thegongoliers/pathFollowing/controllers/PID.java b/src/main/java/com/thegongoliers/pathFollowing/controllers/PID.java deleted file mode 100644 index 9a32dc2..0000000 --- a/src/main/java/com/thegongoliers/pathFollowing/controllers/PID.java +++ /dev/null @@ -1,124 +0,0 @@ -package com.thegongoliers.pathFollowing.controllers; - -import com.thegongoliers.annotations.UsedInCompetition; -import com.thegongoliers.math.GMath; - -@UsedInCompetition(team = "5112", year = "2018") -public class PID implements MotionController { - - private final double kp; - private final double ki; - private final double kd; - - private double iState = 0; - private double dState; - - private final double threshold; - private boolean first = true; - private boolean continuous = false; - private final double minInput = 0; - private final double maxInput = 1; - private final double maxOutput = 1; - private final double minOutput = -1; - private final double maxI = maxOutput; - private final double minI = minOutput; - - /** - * Creates a simple PID calculator. - * - * @param kp - * The proportional constant. - * @param ki - * The integral constant. - * @param kd - * The differential constant. - * @param threshold - * The threshold at which the sensor is considered on target. - * @param continuous - * True if the device is continuous (for example if rotating 360 - * degrees from 0 you end up at 0 it is continuous) - */ - public PID(double kp, double ki, double kd, double threshold, boolean continuous) { - this.kp = kp; - this.ki = ki; - this.kd = kd; - this.threshold = threshold; - this.continuous = continuous; - } - - /** - * Creates a simple PID calculator. - * - * @param kp - * The proportional constant. - * @param ki - * The integral constant. - * @param kd - * The differential constant. - * @param threshold - * The threshold at which the sensor is considered on target. - */ - public PID(double kp, double ki, double kd, double threshold) { - this.kp = kp; - this.ki = ki; - this.kd = kd; - this.threshold = threshold; - } - - private double getContinuousError(double error) { - if (continuous) { - if (Math.abs(error) > (maxInput - minInput) / 2) { - if (error > 0) { - return error - (maxInput - minInput); - } else { - return error + (maxInput - minInput); - } - } - } - return error; - } - - /** - * Calculates the output of the PID. - * - * @param currentPosition - * The current position. - * @param targetPosition - * The target position. - * @return The output of the PID which calculates how to get to the target - * position from the current position. - */ - public double calculate(double currentPosition, double targetPosition) { - double error = getContinuousError(targetPosition - currentPosition); - if (first) { - dState = currentPosition; - first = false; - } - iState += error; - iState = GMath.clamp(iState, minI, maxI); - - double pTerm = kp * error; - double iTerm = ki * iState; - double dTerm = (dState - currentPosition) * kd; - - dState = currentPosition; - - double pidOutput = pTerm + iTerm + dTerm; - - return GMath.clamp(pidOutput, minOutput, maxOutput); - } - - /** - * Determines if the current position is close enough to the target position - * using the threshold. - * - * @param currentPosition - * The current position. - * @param targetPosition - * The target position. - * @return True if it is at the target position (within the threshold). - */ - public boolean isOnTarget(double currentPosition, double targetPosition) { - return Math.abs(currentPosition - targetPosition) <= threshold; - } -} diff --git a/src/test/java/com/thegongoliers/pathFollowing/controllers/MotionProfileControllerTest.java b/src/test/java/com/thegongoliers/pathFollowing/controllers/MotionProfileControllerTest.java deleted file mode 100644 index e7ecb3d..0000000 --- a/src/test/java/com/thegongoliers/pathFollowing/controllers/MotionProfileControllerTest.java +++ /dev/null @@ -1,26 +0,0 @@ -package com.thegongoliers.pathFollowing.controllers; - -import com.thegongoliers.mockHardware.input.MockClock; -import com.thegongoliers.pathFollowing.controllers.MotionProfileController; -import org.junit.Test; - -import static org.junit.Assert.*; - -public class MotionProfileControllerTest { - - @Test - public void test(){ - - MockClock mockClock = new MockClock(); - - MotionProfileController controller = new MotionProfileController(0.01, 0.0001, 0.01, 0.1, 0.02, 0.1, mockClock); - - assertEquals(controller.calculate(0, 1, 8, 4), 0.8901, 0.00001); - mockClock.setTime(1); - assertEquals(controller.calculate(1, 2, 6, -2), 0.5101, 0.00001); - - assertTrue(controller.isOnTarget(1, 1.01)); - assertFalse(controller.isOnTarget(0.9, 1.01)); - } - -} \ No newline at end of file