From 49721ccea6b71d17e3bc3cdb8ae3169a03609d69 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Tue, 9 Jan 2024 01:05:26 -0800 Subject: [PATCH 1/3] add gatorade code to 2024 contains some light changes for WPILib 2024, around DriverStation.getAlliance(). still needs some work before this can compile, especially around Odometry which contains some robot-specific code. --- .../java/com/team766/odometry/Odometry.java | 201 ++++++++++++ src/main/java/com/team766/odometry/Point.java | 93 ++++++ .../java/com/team766/odometry/PointDir.java | 82 +++++ .../robot/gatorade/AutonomousModes.java | 31 ++ .../java/com/team766/robot/gatorade/OI.java | 306 ++++++++++++++++++ .../robot/gatorade/PlacementPosition.java | 9 + .../com/team766/robot/gatorade/Robot.java | 23 ++ .../team766/robot/gatorade/RobotTargets.java | 95 ++++++ .../gatorade/constants/ChargeConstants.java | 29 ++ .../gatorade/constants/ConfigConstants.java | 41 +++ .../constants/FollowPointsInputConstants.java | 12 + .../gatorade/constants/InputConstants.java | 50 +++ .../constants/OdometryInputConstants.java | 22 ++ .../constants/SwerveDriveConstants.java | 21 ++ .../robot/gatorade/mechanisms/Drive.java | 238 ++++++++++++++ .../robot/gatorade/mechanisms/Elevator.java | 198 ++++++++++++ .../gatorade/mechanisms/EncoderUtils.java | 74 +++++ .../robot/gatorade/mechanisms/Gyro.java | 63 ++++ .../robot/gatorade/mechanisms/Intake.java | 133 ++++++++ .../robot/gatorade/mechanisms/Lights.java | 55 ++++ .../robot/gatorade/mechanisms/Wrist.java | 186 +++++++++++ .../procedures/ChargeStationPathFinder.java | 84 +++++ .../procedures/ExtendWristvatorToHigh.java | 11 + .../procedures/ExtendWristvatorToHuman.java | 15 + .../procedures/ExtendWristvatorToLow.java | 11 + .../procedures/ExtendWristvatorToMid.java | 11 + .../robot/gatorade/procedures/GoForCones.java | 16 + .../robot/gatorade/procedures/GoForCubes.java | 16 + .../gatorade/procedures/GyroBalance.java | 178 ++++++++++ .../robot/gatorade/procedures/IntakeIdle.java | 13 + .../robot/gatorade/procedures/IntakeIn.java | 12 + .../robot/gatorade/procedures/IntakeOut.java | 12 + .../robot/gatorade/procedures/IntakeStop.java | 12 + .../gatorade/procedures/MoveWristvator.java | 36 +++ .../robot/gatorade/procedures/OPECHelper.java | 23 ++ .../gatorade/procedures/OnePieceBalance.java | 31 ++ .../procedures/OnePieceExitCommunity.java | 30 ++ .../OnePieceExitCommunityBalance.java | 33 ++ .../procedures/RetractWristvator.java | 11 + .../robot/gatorade/procedures/SetCross.java | 15 + .../proto/com/team766/logging/logging.proto | 4 +- 41 files changed, 2535 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/team766/odometry/Odometry.java create mode 100644 src/main/java/com/team766/odometry/Point.java create mode 100644 src/main/java/com/team766/odometry/PointDir.java create mode 100644 src/main/java/com/team766/robot/gatorade/AutonomousModes.java create mode 100644 src/main/java/com/team766/robot/gatorade/OI.java create mode 100644 src/main/java/com/team766/robot/gatorade/PlacementPosition.java create mode 100644 src/main/java/com/team766/robot/gatorade/Robot.java create mode 100644 src/main/java/com/team766/robot/gatorade/RobotTargets.java create mode 100644 src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java create mode 100644 src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java create mode 100644 src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java create mode 100644 src/main/java/com/team766/robot/gatorade/constants/InputConstants.java create mode 100644 src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java create mode 100644 src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/SetCross.java diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java new file mode 100644 index 000000000..26f1b1343 --- /dev/null +++ b/src/main/java/com/team766/odometry/Odometry.java @@ -0,0 +1,201 @@ +package com.team766.odometry; + +import com.team766.framework.LoggingBase; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import com.ctre.phoenix.sensors.CANCoder; +import com.team766.logging.Category; +import com.team766.robot.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** + * Method which calculates the position of the robot based on wheel positions. + */ +public class Odometry extends LoggingBase { + + private RateLimiter odometryLimiter; + + private MotorController[] motorList; + //The order of CANCoders should be the same as in motorList + private CANCoder[] CANCoderList; + private int motorCount; + + private PointDir[] prevPositions; + private PointDir[] currPositions; + private double[] prevEncoderValues; + private double[] currEncoderValues; + private double gyroPosition; + + private PointDir currentPosition; + + //In Meters + private static double WHEEL_CIRCUMFERENCE; + public static double GEAR_RATIO; + public static int ENCODER_TO_REVOLUTION_CONSTANT; + + //In the same order as motorList, relative to the center of the robot + private Point[] wheelPositions; + + /** + * Constructor for Odometry, taking in several defines for the robot. + * @param motors A list of every wheel-controlling motor on the robot. + * @param CANCoders A list of the CANCoders corresponding to each wheel, in the same order as motors. + * @param wheelLocations A list of the locations of each wheel, in the same order as motors. + * @param wheelCircumference The circumfrence of the wheels, including treads. + * @param gearRatio The gear ratio of the wheels. + * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels. + * @param rateLimiterTime How often odometry should run. + */ + public Odometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime) { + loggerCategory = Category.ODOMETRY; + + odometryLimiter = new RateLimiter(rateLimiterTime); + motorList = motors; + CANCoderList = CANCoders; + motorCount = motorList.length; + log("Motor count " + motorCount); + prevPositions = new PointDir[motorCount]; + currPositions = new PointDir[motorCount]; + prevEncoderValues = new double[motorCount]; + currEncoderValues = new double[motorCount]; + + wheelPositions = wheelLocations; + WHEEL_CIRCUMFERENCE = wheelCircumference; + GEAR_RATIO = gearRatio; + ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant; + + currentPosition = new PointDir(0, 0, 0); + for (int i = 0; i < motorCount; i++) { + prevPositions[i] = new PointDir(0,0, 0); + currPositions[i] = new PointDir(0,0, 0); + prevEncoderValues[i] = 0; + currEncoderValues[i] = 0; + } + } + + public String getName() { + return "Odometry"; + } + + /** + * Sets the current position of the robot to Point P + * @param P The point to set the current robot position to + */ + public void setCurrentPosition(Point P) { + currentPosition.set(P); + log("Set Current Position to: " + P.toString()); + for (int i = 0; i < motorCount; i++) { + prevPositions[i].set(currentPosition.add(wheelPositions[i])); + currPositions[i].set(currentPosition.add(wheelPositions[i])); + } + log("Current Position: " + currentPosition.toString()); + } + + + /** + * Updates the odometry encoder values to the robot encoder values. + */ + private void setCurrentEncoderValues() { + for (int i = 0; i < motorCount; i++) { + prevEncoderValues[i] = currEncoderValues[i]; + currEncoderValues[i] = motorList[i].getSensorPosition(); + currEncoderValues[i] *= (DriverStation.getAlliance() == Alliance.Blue ? 1 : -1); + } + } + + private static Vector2D rotate(Vector2D v, double angle) { + return new Vector2D( + v.getX() * Math.cos(angle) - Math.sin(angle) * v.getY(), + v.getY() * Math.cos(angle) + v.getX() * Math.sin(angle)); + } + + /** + * Updates the position of each wheel of the robot by assuming each wheel moved in an arc. + */ + private void updateCurrentPositions() { + double angleChange; + double radius; + double deltaX; + double deltaY; + gyroPosition = -Robot.gyro.getGyroYaw(); + + /* + Point slopeFactor = new Point(Math.sqrt(Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))), + Math.sqrt(Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())))); + */ + + for (int i = 0; i < motorCount; i++) { + //prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currPositions[i].getHeading()); + //This following line only works if the average of wheel positions is (0,0) + prevPositions[i].set(currentPosition.add(wheelPositions[i]), currPositions[i].getHeading()); + currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition); + angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading(); + + double yaw = -Math.toRadians(Robot.gyro.getGyroYaw()); + double roll = Math.toRadians(Robot.gyro.getGyroRoll()); + double pitch = Math.toRadians(Robot.gyro.getGyroPitch()); + + double w = Math.toRadians(CANCoderList[i].getAbsolutePosition()); + Vector2D u = new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch)); + Vector2D v = new Vector2D(Math.cos(yaw) * Math.sin(pitch) * Math.sin(roll) - Math.sin(yaw) * Math.cos(roll), + Math.sin(yaw) * Math.sin(pitch) * Math.sin(roll) + Math.cos(yaw) * Math.cos(roll)); + Vector2D a = u.scalarMultiply(Math.cos(w)).add(v.scalarMultiply(Math.sin(w))); + Vector2D b = u.scalarMultiply(-Math.sin(w)).add(v.scalarMultiply(Math.cos(w))); + Vector2D wheelMotion; + + //log("u: " + u + " v: " + v + " a: " + a + " b: " + b); + + //double oldWheelX; + //double oldWheelY; + + if (angleChange != 0) { + radius = 180 * (currEncoderValues[i] - prevEncoderValues[i]) / (Math.PI * angleChange); + deltaX = radius * Math.sin(Math.toRadians(angleChange)); + deltaY = radius * (1 - Math.cos(Math.toRadians(angleChange))); + + wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY)); + + //oldWheelX = ((Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX - Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + //oldWheelY = ((Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX + Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + + } else { + wheelMotion = a.scalarMultiply((currEncoderValues[i] - prevEncoderValues[i])); + + //oldWheelX = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + //oldWheelY = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + } + wheelMotion = wheelMotion.scalarMultiply(WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + //wheelMotion = rotate(wheelMotion, Math.toRadians(gyroPosition)); + //log("Difference: " + (oldWheelX - wheelMotion.getX()) + ", " + (oldWheelY - wheelMotion.getY()) + "Old Method: " + oldWheelX + ", " + oldWheelY + "Current Method: " + wheelMotion.getX() + ", " + wheelMotion.getY()); + //log("Current: " + currPositions[i] + " Motion: " + wheelMotion + " New: " + currPositions[i].add(wheelMotion)); + currPositions[i].set(currPositions[i].subtract(wheelMotion)); + } + } + + /** + * Calculates the position of the robot by finding the average of the wheel positions. + */ + private void findRobotPosition() { + double sumX = 0; + double sumY = 0; + for (int i = 0; i < motorCount; i++) { + sumX += currPositions[i].getX(); + sumY += currPositions[i].getY(); + //log("sumX: " + sumX + " Motor Count: " + motorCount + " CurrentPosition: " + currPositions[i]); + } + currentPosition.set(sumX / motorCount, sumY / motorCount, gyroPosition); + } + + //Intended to be placed inside Robot.drive.run() + public PointDir run() { + if (odometryLimiter.next()) { + setCurrentEncoderValues(); + updateCurrentPositions(); + findRobotPosition(); + log(currentPosition.toString()); + } + return currentPosition; + } +} diff --git a/src/main/java/com/team766/odometry/Point.java b/src/main/java/com/team766/odometry/Point.java new file mode 100644 index 000000000..5876c6a55 --- /dev/null +++ b/src/main/java/com/team766/odometry/Point.java @@ -0,0 +1,93 @@ +package com.team766.odometry; + +import com.team766.framework.LoggingBase; +import java.lang.Math; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +/** + * Class of two-coordinate objects, an x-coordinate and a y-coordinate. + */ +public class Point extends LoggingBase { + private double x; + private double y; + + public String getName() { + return "Points"; + } + + public Point(double x, double y){ + this.x = x; + this.y = y; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public void set(double x, double y) { + this.x = x; + this.y = y; + } + + public void set(Point P) { + this.x = P.getX(); + this.y = P.getY(); + } + + public void setX(double x) { + this.x = x; + } + + public void setY(double y) { + this.y = y; + } + + public double distance(Point a) { + return Math.sqrt(Math.pow(a.getX() - getX(), 2.0) + Math.pow(a.getY() - getY(), 2.0)); + } + + public double slope(Point a) { + double s; + final int MAX = 1000; + if (a.getX() == getX()) { + //If the points are on top of each other, returns positive or negative MAX. + if (a.getY() < getY()) { + s = -MAX; + } else { + s = MAX; + } + } else { + s = (getY() - a.getY()) / (getX() - a.getX()); + } + if (Math.abs(s) > MAX) { + s = Math.signum(s) * MAX; + } + return s; + } + + //Gets a vector with length scale between the point and another point. Used for swerve drive method input. + public Point scaleVector(Point inputPoint, double scale) { + double d = distance(inputPoint); + return new Point((inputPoint.getX() - getX()) * scale / d, (inputPoint.getY() - getY()) * scale / d); + } + + public Point add(Point p) { + return new Point(getX() + p.getX(), getY() + p.getY()); + } + + public Point add(Vector2D v) { + return new Point(getX() + v.getX(), getY() + v.getY()); + } + + public Point subtract(Vector2D v) { + return new Point(getX() - v.getX(), getY() - v.getY()); + } + + public String toString() { + return "X: " + getX() + " Y: " + getY(); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/odometry/PointDir.java b/src/main/java/com/team766/odometry/PointDir.java new file mode 100644 index 000000000..2f00c413c --- /dev/null +++ b/src/main/java/com/team766/odometry/PointDir.java @@ -0,0 +1,82 @@ +package com.team766.odometry; + +import com.team766.logging.Category; +import java.lang.Math; + +public class PointDir extends Point { + private double heading; + + public PointDir(double x, double y, double h){ + super(x, y); + heading = h; + loggerCategory = Category.DRIVE; + } + + public PointDir(double x, double y){ + super(x, y); + loggerCategory = Category.DRIVE; + } + + public PointDir(Point P){ + super(P.getX(), P.getY()); + loggerCategory = Category.DRIVE; + } + + public PointDir(Point P, double h) { + super(P.getX(), P.getY()); + heading = h; + loggerCategory = Category.DRIVE; + } + + public double getHeading() { + return heading; + } + + public void set(double x, double y, double h) { + super.set(x, y); + heading = h; + } + + public void set(Point P, double h) { + super.set(P); + heading = h; + } + + public void setHeading(double h) { + heading = h; + } + + public double getAngleDifference(Point a) { + //Returns a number between -1 and 1 to represent the number of rotations between the two angles. + PointDir unitVector; + if (distance(a) == 0) { + unitVector = new PointDir(1, 0, 0); + } else { + unitVector = new PointDir((a.getX() - getX()) / distance(a), (a.getY() - getY()) / distance(a), Math.toDegrees(Math.atan2((a.getY() - getY()) / distance(a), (a.getX() - getX()) / distance(a)))); + } + double headingAngle = getHeading() % 360; + if (headingAngle < 0) { + headingAngle += 360; + } + if (unitVector.getHeading() < 0) { + unitVector.setHeading(unitVector.getHeading() + 360); + } + double diff = headingAngle - unitVector.getHeading(); + if (diff < -180) { + diff += 360; + } else if (diff > 180) { + diff -= 360; + } + return diff / 180; + } + + @Override + public String toString() { + return super.toString() + " Heading: " + getHeading(); + } + + @Override + public PointDir clone() { + return new PointDir(getX(), getY(), getHeading()); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java new file mode 100644 index 000000000..0cd78ebf7 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java @@ -0,0 +1,31 @@ +package com.team766.robot.gatorade; + +import com.team766.framework.AutonomousMode; +import com.team766.robot.gatorade.procedures.*; + +import edu.wpi.first.wpilibj.DriverStation; + +import com.team766.odometry.Point; +import com.team766.odometry.PointDir; +import com.team766.framework.Procedure; + +public class AutonomousModes { + + public static final AutonomousMode[] AUTONOMOUS_MODES = new AutonomousMode[] { + // Add autonomous modes here like this: + // new AutonomousMode("NameOfAutonomousMode", () -> new MyAutonomousProcedure()), + // + // If your autonomous procedure has constructor arguments, you can + // define one or more different autonomous modes with it like this: + // new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)), + // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), + // new AutonomousMode("FollowPoints", () -> new FollowPoints()), + // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), + // new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), + new AutonomousMode("OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), + // AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), + // new AutonomousMode("FollowPointsFile", () -> new FollowPoints("FollowPoints.json")), + // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new PointDir(2, 1), new PointDir(0, 1)})), + // new AutonomousMode("DoNothing", () -> new DoNothing()), + }; +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java new file mode 100644 index 000000000..debd435f9 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/OI.java @@ -0,0 +1,306 @@ +package com.team766.robot.gatorade; + +import com.team766.framework.Procedure; +import com.team766.framework.Context; +import com.team766.hal.JoystickReader; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.logging.Category; +import com.team766.logging.Severity; +import com.team766.robot.gatorade.constants.InputConstants; +import com.team766.robot.gatorade.procedures.*; +import com.team766.simulator.interfaces.ElectricalDevice.Input; +import com.team766.robot.gatorade.mechanisms.Drive; +import com.team766.robot.gatorade.mechanisms.Intake; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This class is the glue that binds the controls on the physical operator + * interface to the code that allow control of the robot. + */ +public class OI extends Procedure { + + private JoystickReader leftJoystick; + private JoystickReader rightJoystick; + private JoystickReader boxopGamepad; + private double rightJoystickX = 0; + private double leftJoystickX = 0; + private double leftJoystickY = 0; + private boolean isCross = false; + + private static final double FINE_DRIVING_COEFFICIENT = 0.25; + double turningValue = 0; + boolean manualControl = true; + PlacementPosition placementPosition = PlacementPosition.NONE; + + private RateLimiter lightsRateLimit = new RateLimiter(1.3); + + public OI() { + loggerCategory = Category.OPERATOR_INTERFACE; + + leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); + rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); + boxopGamepad = RobotProvider.instance.getJoystick(InputConstants.BOXOP_GAMEPAD); + } + + public void run(Context context) { + context.takeOwnership(Robot.drive); + context.takeOwnership(Robot.gyro); + context.takeOwnership(Robot.lights); + + boolean elevatorManual = false; + boolean wristManual = false; + + while (true) { + context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); + RobotProvider.instance.refreshDriverStationData(); + + SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); + + // Add driver controls here - make sure to take/release ownership + // of mechanisms when appropriate. + + leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + //Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); + + if (leftJoystick.getButtonPressed(InputConstants.INTAKE_OUT)) { + new IntakeOut().run(context); + } else if (leftJoystick.getButtonReleased(InputConstants.INTAKE_OUT)) { + new IntakeStop().run(context); + } + + if (leftJoystick.getButtonPressed(InputConstants.RESET_GYRO)) { + Robot.gyro.resetGyro(); + } + + if (leftJoystick.getButtonPressed(InputConstants.RESET_POS)) { + Robot.drive.resetCurrentPosition(); + } + + if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT) / 2; + } else { + rightJoystickX = 0; + } + + if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { + leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + } else { + leftJoystickY = 0; + } + if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + } else { + leftJoystickX = 0; + } + + // Sets the wheels to the cross position if the cross button is pressed + if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { + if (!isCross) { + context.startAsync(new SetCross()); + } + isCross = !isCross; + } + + // Moves the robot if there are joystick inputs + if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) { + context.takeOwnership(Robot.drive); + // If a button is pressed, drive is just fine adjustment + if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { + Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); + } else { + // On deafault, controls the robot field oriented + Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX), (leftJoystickY), (rightJoystickX)); + } + } else { + Robot.drive.stopDrive(); + } + + // Respond to boxop commands + + // first, check if the boxop is making a cone or cube selection + if (boxopGamepad.getPOV() == InputConstants.POV_UP) { + new GoForCones().run(context); + setLightsForGamePiece(); + } else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) { + new GoForCubes().run(context); + setLightsForGamePiece(); + } + + // look for button presses to queue placement of intake/wrist/elevator superstructure + if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { + placementPosition = PlacementPosition.LOW_NODE; + setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) { + placementPosition = PlacementPosition.MID_NODE; + setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) { + placementPosition = PlacementPosition.HIGH_NODE; + setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) { + placementPosition = PlacementPosition.HUMAN_PLAYER; + setLightsForPlacement(); + } + + // look for button hold to start intake, release to idle intake + if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_INTAKE_IN)) { + new IntakeIn().run(context); + } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) { + new IntakeIdle().run(context); + } + + // see if we should reset the button states + if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) { + // stop the intake + new IntakeStop().run(context); + // reset the placement position + placementPosition = PlacementPosition.NONE; + // reset the cone/cube selection to cones + new GoForCones().run(context); + } + + // look for button hold to extend intake/wrist/elevator superstructure, + // release to retract + if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { + switch (placementPosition) { + case NONE: + break; + case LOW_NODE: + context.startAsync(new ExtendWristvatorToLow()); + break; + case MID_NODE: + context.startAsync(new ExtendWristvatorToMid()); + break; + case HIGH_NODE: + context.startAsync(new ExtendWristvatorToHigh()); + break; + case HUMAN_PLAYER: + context.startAsync(new ExtendWristvatorToHuman(Robot.intake.getGamePieceType())); + break; + default: + // warn, ignore + log(Severity.WARNING, "Unexpected placement position: " + placementPosition.toString()); + break; + } + } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { + context.startAsync(new RetractWristvator()); + } + + // TODO: refactor this code. it's getting gnarly. + // look for manual nudges + // we only allow these if the extend elevator trigger is extended + if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { + // the y axis is flipped from what we expect. invert so up is positive, down is negative. + double elevatorNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT); + double wristNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); + + if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { + // bypass PID + if (Math.abs(elevatorNudgeAxis) > 0.05) { + elevatorManual = true; + context.takeOwnership(Robot.elevator); + Robot.elevator.nudgeNoPID(elevatorNudgeAxis); + context.releaseOwnership(Robot.elevator); + } else if (elevatorManual) { + context.takeOwnership(Robot.elevator); + Robot.elevator.stopElevator(); + context.releaseOwnership(Robot.elevator); + elevatorManual = false; + } + + if ((Math.abs(wristNudgeAxis) > 0.05)) { + wristManual = true; + context.takeOwnership(Robot.wrist); + Robot.wrist.nudgeNoPID(wristNudgeAxis); + context.releaseOwnership(Robot.wrist); + } else if (wristManual) { + context.takeOwnership(Robot.wrist); + Robot.wrist.stopWrist(); + context.releaseOwnership(Robot.wrist); + elevatorManual = false; + } + } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { + context.takeOwnership(Robot.wrist); + context.takeOwnership(Robot.elevator); + Robot.wrist.resetEncoder(); + Robot.elevator.resetEncoder(); + context.releaseOwnership(Robot.wrist); + context.releaseOwnership(Robot.elevator); + } else { + // look for elevator nudges + if (Math.abs(elevatorNudgeAxis) > 0.05) { + context.takeOwnership(Robot.elevator); + if (elevatorNudgeAxis > 0) { + Robot.elevator.nudgeUp(); + } else { + Robot.elevator.nudgeDown(); + } + context.releaseOwnership(Robot.elevator); + } + + // look for wrist nudges + if (Math.abs(wristNudgeAxis) > 0.05) { + context.takeOwnership(Robot.wrist); + if (wristNudgeAxis > 0) { + Robot.wrist.nudgeUp(); + } else { + Robot.wrist.nudgeDown(); + } + context.releaseOwnership(Robot.wrist); + } + } + } + + if (lightsRateLimit.next()) { + if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) { + Robot.lights.rainbow(); + } else { + setLightsForPlacement(); + } + } + } + } + + private void setLightsForPlacement() { + switch (placementPosition) { + case NONE: + Robot.lights.white(); + break; + case LOW_NODE: + Robot.lights.green(); + break; + case MID_NODE: + Robot.lights.red(); + break; + case HIGH_NODE: + Robot.lights.orange(); + break; + case HUMAN_PLAYER: + setLightsForGamePiece(); + break; + default: + // warn, ignore + log(Severity.WARNING, "Unexpected placement position: " + placementPosition.toString()); + break; + } + + lightsRateLimit.reset(); + lightsRateLimit.next(); + } + + private void setLightsForGamePiece() { + if (Robot.intake.getGamePieceType() == GamePieceType.CUBE) { + Robot.lights.purple(); + } else { + Robot.lights.yellow(); + } + + lightsRateLimit.reset(); + lightsRateLimit.next(); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/PlacementPosition.java b/src/main/java/com/team766/robot/gatorade/PlacementPosition.java new file mode 100644 index 000000000..1a1118c60 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/PlacementPosition.java @@ -0,0 +1,9 @@ +package com.team766.robot.gatorade; + +public enum PlacementPosition { + NONE, + LOW_NODE, + MID_NODE, + HIGH_NODE, + HUMAN_PLAYER +} diff --git a/src/main/java/com/team766/robot/gatorade/Robot.java b/src/main/java/com/team766/robot/gatorade/Robot.java new file mode 100644 index 000000000..891244ce9 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/Robot.java @@ -0,0 +1,23 @@ +package com.team766.robot.gatorade; + +import com.team766.robot.gatorade.mechanisms.*; + +public class Robot { + // Declare mechanisms here + public static Intake intake; + public static Wrist wrist; + public static Elevator elevator; + public static Drive drive; + public static Gyro gyro; + public static Lights lights; + + public static void robotInit() { + // Initialize mechanisms here + intake = new Intake(); + wrist = new Wrist(); + elevator = new Elevator(); + drive = new Drive(); + gyro = new Gyro(); + lights = new Lights(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/RobotTargets.java b/src/main/java/com/team766/robot/gatorade/RobotTargets.java new file mode 100644 index 000000000..c52270b3a --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/RobotTargets.java @@ -0,0 +1,95 @@ +package com.team766.robot.gatorade; + +import com.team766.odometry.Point; + +/** + * Constants defining points the robot may want to travel to + */ +public final class RobotTargets { + + private static final double BLUE_Y = 1.88595; + private static final double RED_Y = 14.6558; + + private static final double ROW_1 = 0.508; + private static final double ROW_2 = 1.071626; + private static final double ROW_3 = 1.6256; + private static final double ROW_4 = 2.1844; + private static final double ROW_5 = 2.748026; + private static final double ROW_6 = 3.302; + private static final double ROW_7 = 3.8608; + private static final double ROW_8 = 4.424426; + private static final double ROW_9 = 4.9784; + + //Points corresponding to nodes, in the format: NODES_COLOR_GRID_COLUMN + //This is from the perspective of the driver + public static final Point NODES_BLUE_RIGHT_RIGHT = new Point(BLUE_Y, ROW_1); + public static final Point NODES_BLUE_RIGHT_CENTER = new Point(BLUE_Y, ROW_2); + public static final Point NODES_BLUE_RIGHT_LEFT = new Point(BLUE_Y, ROW_3); + public static final Point NODES_BLUE_CENTER_RIGHT = new Point(BLUE_Y, ROW_4); + public static final Point NODES_BLUE_CENTER_CENTER = new Point(BLUE_Y, ROW_5); + public static final Point NODES_BLUE_CENTER_LEFT = new Point(BLUE_Y, ROW_6); + public static final Point NODES_BLUE_LEFT_RIGHT = new Point(BLUE_Y, ROW_7); + public static final Point NODES_BLUE_LEFT_CENTER = new Point(BLUE_Y, ROW_8); + public static final Point NODES_BLUE_LEFT_LEFT = new Point(BLUE_Y, ROW_9); + + public static final Point NODES_RED_LEFT_LEFT = new Point(RED_Y, ROW_1); + public static final Point NODES_RED_LEFT_CENTER = new Point(RED_Y, ROW_2); + public static final Point NODES_RED_LEFT_RIGHT = new Point(RED_Y, ROW_3); + public static final Point NODES_RED_CENTER_LEFT = new Point(RED_Y, ROW_4); + public static final Point NODES_RED_CENTER_CENTER = new Point(RED_Y, ROW_5); + public static final Point NODES_RED_CENTER_RIGHT = new Point(RED_Y, ROW_6); + public static final Point NODES_RED_RIGHT_LEFT = new Point(RED_Y, ROW_7); + public static final Point NODES_RED_RIGHT_CENTER = new Point(RED_Y, ROW_8); + public static final Point NODES_RED_RIGHT_RIGHT = new Point(RED_Y, ROW_9); + + public static final Point[] NODES = { + NODES_BLUE_RIGHT_RIGHT, + NODES_BLUE_RIGHT_CENTER, + NODES_BLUE_RIGHT_LEFT, + NODES_BLUE_CENTER_RIGHT, + NODES_BLUE_CENTER_CENTER, + NODES_BLUE_CENTER_LEFT, + NODES_BLUE_LEFT_RIGHT, + NODES_BLUE_LEFT_CENTER, + NODES_BLUE_LEFT_LEFT, + NODES_RED_RIGHT_RIGHT, + NODES_RED_RIGHT_CENTER, + NODES_RED_RIGHT_LEFT, + NODES_RED_CENTER_RIGHT, + NODES_RED_CENTER_CENTER, + NODES_RED_CENTER_LEFT, + NODES_RED_LEFT_RIGHT, + NODES_RED_LEFT_CENTER, + NODES_RED_LEFT_LEFT + }; + + public static final Point[] CONE_NODES = { + NODES_BLUE_RIGHT_RIGHT, + NODES_BLUE_RIGHT_LEFT, + NODES_BLUE_CENTER_RIGHT, + NODES_BLUE_CENTER_LEFT, + NODES_BLUE_LEFT_RIGHT, + NODES_BLUE_LEFT_LEFT, + NODES_RED_RIGHT_RIGHT, + NODES_RED_RIGHT_LEFT, + NODES_RED_CENTER_RIGHT, + NODES_RED_CENTER_LEFT, + NODES_RED_LEFT_RIGHT, + NODES_RED_LEFT_LEFT + }; + + public static final Point[] CUBE_NODES = { + NODES_BLUE_RIGHT_CENTER, + NODES_BLUE_CENTER_CENTER, + NODES_BLUE_LEFT_CENTER, + NODES_RED_RIGHT_CENTER, + NODES_RED_CENTER_CENTER, + NODES_RED_LEFT_CENTER, + }; + + public static final double[] CUBE_ROWS = { + ROW_2, + ROW_5, + ROW_8 + }; +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java b/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java new file mode 100644 index 000000000..12023da4f --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java @@ -0,0 +1,29 @@ +package com.team766.robot.gatorade.constants; + +/** + * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. + * + * TODO: consider moving this into a config file. + */ +public final class ChargeConstants { + + public static final double BLUE_BALANCE_TARGET_X = 3.9417625; + public static final double BLUE_BALANCE_LEFT_EDGE = 2.974975; + public static final double BLUE_BALANCE_RIGHT_EDGE = 4.90855; + + public static final double RED_BALANCE_TARGET_X = 12.5999875; + public static final double RED_BALANCE_LEFT_EDGE = 11.6332; + public static final double RED_BALANCE_RIGHT_EDGE = 13.566775; + + public static final double X_ALIGNMENT_THRESHOLD = 0.5; + public static final double Y_ALIGNMENT_THRESHOLD = 0.5; + + public static final double BLUE_LEFT_PT = BLUE_BALANCE_LEFT_EDGE - X_ALIGNMENT_THRESHOLD; + public static final double BLUE_RIGHT_PT = BLUE_BALANCE_RIGHT_EDGE + X_ALIGNMENT_THRESHOLD; + public static final double RED_LEFT_PT = RED_BALANCE_LEFT_EDGE - X_ALIGNMENT_THRESHOLD; + public static final double RED_RIGHT_PT = RED_BALANCE_RIGHT_EDGE + X_ALIGNMENT_THRESHOLD; + + public static final double CHARGE_TOP_EDGE = 3.96875; + public static final double CHARGE_BOTTOM_EDGE = 1.4986; + public static final double MIDDLE = 2.733675; +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java new file mode 100644 index 000000000..85ad22bb2 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java @@ -0,0 +1,41 @@ +package com.team766.robot.gatorade.constants; + +/** Constants used for reading values from the config file. */ +public final class ConfigConstants { + // utility class + private ConfigConstants() {} + + // drive config values + public static final String DRIVE_DRIVE_FRONT_RIGHT = "drive.DriveFrontRight"; + public static final String DRIVE_DRIVE_FRONT_LEFT = "drive.DriveFrontLeft"; + public static final String DRIVE_DRIVE_BACK_RIGHT = "drive.DriveBackRight"; + public static final String DRIVE_DRIVE_BACK_LEFT = "drive.DriveBackLeft"; + + public static final String DRIVE_STEER_FRONT_RIGHT = "drive.SteerFrontRight"; + public static final String DRIVE_STEER_FRONT_LEFT = "drive.SteerFrontLeft"; + public static final String DRIVE_STEER_BACK_RIGHT = "drive.SteerBackRight"; + public static final String DRIVE_STEER_BACK_LEFT = "drive.SteerBackLeft"; + + // intake config values + public static final String INTAKE_MOTOR = "intake.motor"; + + // wrist config values + public static final String WRIST_MOTOR = "wrist.motor"; + public static final String WRIST_PGAIN = "wrist.sparkPID.pGain"; + public static final String WRIST_IGAIN = "wrist.sparkPID.iGain"; + public static final String WRIST_DGAIN = "wrist.sparkPID.dGain"; + public static final String WRIST_FFGAIN = "wrist.sparkPID.ffGain"; + + // elevator config values + public static final String ELEVATOR_LEFT_MOTOR = "elevator.leftMotor"; + public static final String ELEVATOR_RIGHT_MOTOR = "elevator.rightMotor"; + public static final String ELEVATOR_PGAIN = "elevator.sparkPID.pGain"; + public static final String ELEVATOR_IGAIN = "elevator.sparkPID.iGain"; + public static final String ELEVATOR_DGAIN = "elevator.sparkPID.dGain"; + public static final String ELEVATOR_FFGAIN = "elevator.sparkPID.ffGain"; + public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity"; + public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity"; + public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel"; +} + + diff --git a/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java new file mode 100644 index 000000000..35af105a9 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java @@ -0,0 +1,12 @@ +package com.team766.robot.gatorade.constants; + +public final class FollowPointsInputConstants { + + public static double RADIUS = 0.5; + public static double SPEED = 0.25; + public static double RATE_LIMITER_TIME = 0.01; + public static double MAX_ROTATION_SPEED = 0.4; + public static double ANGLE_DISTANCE_FOR_MAX_SPEED = 90; + + +} diff --git a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java new file mode 100644 index 000000000..f333af27b --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java @@ -0,0 +1,50 @@ +package com.team766.robot.gatorade.constants; + +/** + * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. + */ +public final class InputConstants { + + // Joysticks + public static final int LEFT_JOYSTICK = 0; + public static final int RIGHT_JOYSTICK = 1; + public static final int BOXOP_GAMEPAD = 2; // should be in Logitech Mode + + // Navigation + public static final int AXIS_LEFT_RIGHT = 0; + public static final int AXIS_FORWARD_BACKWARD = 1; + public static final int AXIS_TWIST = 3; + + // Joystick buttons + public static final int INTAKE_OUT = 1; + public static final int FINE_DRIVING = 1; + public static final int CROSS_WHEELS = 3; + public static final int RESET_GYRO = 9; + public static final int RESET_POS = 15; + + // Boxop Gamepad Buttons + + // LT + public static final int BUTTON_INTAKE_IN = 7; + // RT + public static final int BUTTON_EXTEND_WRISTVATOR = 8; + // Start + public static final int BUTTON_RESET_STATE = 10; + + // left axis + public static final int AXIS_WRIST_MOVEMENT = 1; + // right axis + public static final int AXIS_ELEVATOR_MOVEMENT = 3; + + // pov + public static final int POV_UP = 0; + public static final int POV_DOWN = 180; + + // LB + public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5; + // X/A/B/Y + public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X + public static final int BUTTON_PLACEMENT_HIGH = 4; // Y + public static final int BUTTON_PLACEMENT_MID = 3; // B + public static final int BUTTON_PLACEMENT_LOW = 2; // A +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java new file mode 100644 index 000000000..75c0c12bf --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java @@ -0,0 +1,22 @@ +package com.team766.robot.gatorade.constants; + +import com.team766.hal.MotorController; + +/** + * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. + * + */ +public final class OdometryInputConstants { + + //Circumference of the wheels. It was measured to be 30.5cm, then experimentally this value had an error of 2.888%. This was then converted to meters. + public static final double WHEEL_CIRCUMFERENCE = 30.5 * 1.02888 / 100; + //Unique to the type of swerve module we have. This is the factor converting motor revolutions to wheel revolutions. + public static final double GEAR_RATIO = 6.75; + //Unique to the type of swerve module we have. For every revolution of the wheel, the encoders will increase by 2048. + public static final int ENCODER_TO_REVOLUTION_CONSTANT = 2048; + //The distance between the centers of the wheels on each side of the robot. This was measured as 20.5 inches, then converted to meters. + public static final double DISTANCE_BETWEEN_WHEELS = 20.5 * 2.54 / 100; + //How often odometry updates, in seconds. + public static final double RATE_LIMITER_TIME = 0.05; + +} diff --git a/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java b/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java new file mode 100644 index 000000000..31d09b766 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java @@ -0,0 +1,21 @@ +package com.team766.robot.gatorade.constants; + +public final class SwerveDriveConstants { + + // defines where the wheels are in relation to the center of the robot + // allows swerve drive code to calculate how to turn + public static final double FL_X = -1; + public static final double FL_Y = 1; + public static final double FR_X = 1; + public static final double FR_Y = 1; + public static final double BL_X = -1; + public static final double BL_Y = -1; + public static final double BR_X = 1; + public static final double BR_Y = -1; + + public static final String SWERVE_CANBUS = "Swervavore"; + + public static final double DRIVE_MOTOR_CURRENT_LIMIT = 35; + public static final double STEER_MOTOR_CURRENT_LIMIT = 30; + +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java new file mode 100644 index 000000000..4735ab66d --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java @@ -0,0 +1,238 @@ +package com.team766.robot.gatorade.mechanisms; + +import com.ctre.phoenix.sensors.CANCoder; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.hal.MotorController.ControlMode; +import com.team766.logging.Category; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; +import com.team766.robot.gatorade.constants.OdometryInputConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import com.team766.odometry.Odometry; +import com.team766.odometry.Point; +import com.team766.odometry.PointDir; + +public class Drive extends Mechanism { + + // Drive motors + private MotorController m_DriveFR; + private MotorController m_DriveFL; + private MotorController m_DriveBR; + private MotorController m_DriveBL; + + // Motors to turn each wheel in place + private MotorController m_SteerFR; + private MotorController m_SteerFL; + private MotorController m_SteerBR; + private MotorController m_SteerBL; + + // Absolute encoders (cancoders) + private CANCoder e_FrontRight; + private CANCoder e_FrontLeft; + private CANCoder e_BackRight; + private CANCoder e_BackLeft; + + /* + * Specific offsets to align each wheel + * These are assigned upon startup with setEncoderOffset() + */ + private double offsetFR; + private double offsetFL; + private double offsetBR; + private double offsetBL; + + /* + * Factor that converts between motor units and degrees + * Multiply to convert from degrees to motor units + * Divide to convert from motor units to degrees + */ + private final double ENCODER_CONVERSION_FACTOR = (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + + // TODO: rework odometry so it doesn't have to go through drive + + // declaration of odometry object + private Odometry swerveOdometry; + // variable representing current position + private static PointDir currentPosition; + + // other variables to set up odometry + private MotorController[] motorList; + private CANCoder[] CANCoderList; + private Point[] wheelPositions; + + public Drive() { + + loggerCategory = Category.DRIVE; + // Initializations of motors + + // Initialize the drive motors + m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); + + // Initialize the steering motors + m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); + + // Initialize the encoders + e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); + + // Current limit for motors to avoid breaker problems + m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_DriveFL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_DriveBR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_DriveBL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_SteerFR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + m_SteerFL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + m_SteerBR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + m_SteerBL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + + // Sets up odometry + currentPosition = new PointDir(0, 0, 0); + motorList = new MotorController[] {m_DriveFR, m_DriveFL, m_DriveBL, + m_DriveBR}; + CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; + wheelPositions = + new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; + log("MotorList Length: " + motorList.length); + log("CANCoderList Length: " + CANCoderList.length); + swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); + + // Sets the offset value for each steering motor so that each is aligned + setEncoderOffset(); + } + + /** + * Sets just the steer for a specific module + * Can be used to turn the wheels without moving + * @param steerMotor the steer motor of this module + * @param vector the vector specifying the module's motion + * @param offset the offset for this module + */ + public void controlModuleSteer(MotorController steerMotor, Vector2D vector, double offset) { + + // Calculates the angle of the vector from -180° to 180° + final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); + + // Add 360 * number of full rotations to vectorTheta, then add offset + final double angleDegrees = vectorTheta + 360*(Math.round((steerMotor.getSensorPosition()/ENCODER_CONVERSION_FACTOR - offset - vectorTheta)/360)) + offset; + + // Sets the degree of the steer wheel + // Needs to multiply by encoderconversionfactor to translate into a unit the motor understands + steerMotor.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR*angleDegrees); + + SmartDashboard.putNumber("Angle", angleDegrees); + } + + /** + * Sets the motion for a specific module + * @param driveMotor the drive motor of this module + * @param steerMotor the steer motor of this module + * @param vector the vector specifying the module's motion + * @param offset the offset for this module + */ + public void controlModuleSteerAndPower(MotorController driveMotor, MotorController steerMotor, Vector2D vector, double offset) { + checkContextOwnership(); + + controlModuleSteer(steerMotor, vector, offset); + + // Sets the power to the magnitude of the vector + driveMotor.set(vector.getNorm()); + + } + + /** + * Maps parameters to robot oriented swerve movement + * @param x the x value for the position joystick + * @param y the y value for the position joystick + * @param turn the turn value from the rotation joystick + */ + public void controlRobotOriented(double x, double y, double turn) { + // Finds the vectors for turning and for translation of each module, and adds them + // Applies this for each module + controlModuleSteerAndPower(m_DriveFL, m_SteerFL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X).normalize()), offsetFL); + controlModuleSteerAndPower(m_DriveFR, m_SteerFR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X).normalize()), offsetFR); + controlModuleSteerAndPower(m_DriveBR, m_SteerBR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X).normalize()), offsetBR); + controlModuleSteerAndPower(m_DriveBL, m_SteerBL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X).normalize()), offsetBL); + } + + /** + * Uses controlRobotOriented() to control the robot relative to the field + * @param yawRad the robot gyro's current yaw value in radians + * @param x the x value for the position joystick + * @param y the y value for the position joystick + * @param turn the turn value from the rotation joystick + */ + public void controlFieldOriented(double yawRad, double x, double y, double turn) { + // Applies a rotational translation to controlRobotOriented + // Counteracts the forward direction changing when the robot turns + // TODO: change to inverse rotation matrix (rather than negative angle) + controlRobotOriented(Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, turn); + } + + /* + * Compares the absolute encoder to the motor encoder to find each motor's offset + * This helps each wheel to always be aligned + */ + public void setEncoderOffset() { + offsetFR = (m_SteerFR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_FrontRight.getAbsolutePosition(); + offsetFL = (m_SteerFL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_FrontLeft.getAbsolutePosition(); + offsetBR = (m_SteerBR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_BackRight.getAbsolutePosition(); + offsetBL = (m_SteerBL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_BackLeft.getAbsolutePosition(); + } + + /* + * Stops each drive motor + */ + public void stopDrive() { + checkContextOwnership(); + m_DriveFR.stopMotor(); + m_DriveFL.stopMotor(); + m_DriveBR.stopMotor(); + m_DriveBL.stopMotor(); + } + + /* + * Turns wheels in a cross formation to prevent robot from moving + */ + public void setCross() { + controlModuleSteer(m_SteerFL, new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X), offsetFL); + controlModuleSteer(m_SteerFR, new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X), offsetFR); + controlModuleSteer(m_SteerBL, new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X), offsetBL); + controlModuleSteer(m_SteerBR, new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X), offsetBR); + } + + // TODO: rework odometry so it doesn't have to go through drive + // TODO: figure out why odometry x and y are swapped + public PointDir getCurrentPosition() { + return currentPosition; + } + + public void setCurrentPosition(Point P) { + swerveOdometry.setCurrentPosition(P); + } + + public void resetCurrentPosition() { + swerveOdometry.setCurrentPosition(new Point(0, 0)); + } + + // Odometry + @Override + public void run() { + currentPosition = swerveOdometry.run(); + log(currentPosition.toString()); + SmartDashboard.putString("position", currentPosition.toString()); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java new file mode 100644 index 000000000..a6929e401 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java @@ -0,0 +1,198 @@ +package com.team766.robot.gatorade.mechanisms; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.SparkMaxPIDController.AccelStrategy; +import com.team766.config.ConfigFileReader; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + +public class Elevator extends Mechanism { + public enum Position { + + // TODO: do we need separate heights for cones vs cubes? + + /** Elevator is fully retracted. */ + RETRACTED(0), + /** Elevator is the appropriate height to place game pieces at the low node. */ + LOW(5), + /** Elevator is the appropriate height to place game pieces at the mid node. */ + MID(18), + /** Elevator is at appropriate height to place game pieces at the high node. */ + HIGH(40), + /** Elevator is at appropriate height to grab cubes from the human player. */ + HUMAN_CUBES(40), + /** Elevator is at appropriate height to grab cones from the human player. */ + HUMAN_CONES(40), + /** Elevator is fully extended. */ + EXTENDED(40); + + private final double height; + + Position(double position) { + this.height = position; + } + + private double getHeight() { + return height; + } + } + + private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.25; + + private static final double NEAR_THRESHOLD = 2.0; + + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final ValueProvider maxVelocity; + private final ValueProvider minOutputVelocity; + private final ValueProvider maxAccel; + + + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Contructs a new Elevator. + */ + public Elevator() { + MotorController halLeftMotor = RobotProvider.instance.getMotor(ELEVATOR_LEFT_MOTOR); + MotorController halRightMotor = RobotProvider.instance.getMotor(ELEVATOR_RIGHT_MOTOR); + + if (!((halLeftMotor instanceof CANSparkMax)&&(halRightMotor instanceof CANSparkMax))) { + log(Severity.ERROR, "Motors are not CANSparkMaxes!"); + throw new IllegalStateException("Motor are not CANSparkMaxes!"); + } + + leftMotor = (CANSparkMax) halLeftMotor; + rightMotor = (CANSparkMax) halRightMotor; + + rightMotor.follow(leftMotor, true); + + resetEncoder(); + + pidController = leftMotor.getPIDController(); + pidController.setFeedbackDevice(leftMotor.getEncoder()); + + pGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_FFGAIN); + maxVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_VELOCITY); + minOutputVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MIN_OUTPUT_VELOCITY); + maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL); + } + + public void resetEncoder() { + leftMotor.getEncoder().setPosition( + EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); + } + + public double getRotations() { + return leftMotor.getEncoder().getPosition(); + } + + /** + * Returns the current height of the elevator, in inches ('Murica). + */ + public double getHeight() { + return EncoderUtils.elevatorRotationsToHeight(leftMotor.getEncoder().getPosition()); + } + + public boolean isNearTo(Position position) { + return isNearTo(position.getHeight()); + } + + public boolean isNearTo(double position) { + return Math.abs(position - getHeight()) < NEAR_THRESHOLD; + } + + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + leftMotor.set(clampedValue); + } + + public void stopElevator() { + checkContextOwnership(); + leftMotor.set(0); + } + + public void nudgeUp() { + System.err.println("Nudging up."); + + double height = getHeight(); + // NOTE: this could artificially limit nudge range + double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight()); + System.err.println("Target: " + targetHeight); + + moveTo(targetHeight); + } + + public void nudgeDown() { + double height = getHeight(); + // NOTE: this could artificially limit nudge range + double targetHeight = Math.max(height - NUDGE_INCREMENT, Position.RETRACTED.getHeight()); + moveTo(targetHeight); + } + + /** + * Moves the elevator to a pre-set {@link Position}. + */ + public void moveTo(Position position) { + moveTo(position.getHeight()); + } + + /** + * Moves the elevator to a specific position (in inches). + */ + public void moveTo(double position) { + checkContextOwnership(); + + System.err.println("Setting target position to " + position); + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + // pidController.setFF(ffGain.get()); + double ff = ffGain.get(); + + + pidController.setOutputRange(-0.4, 0.4); + + // pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0); + // pidController.setSmartMotionMaxVelocity(maxVelocity.get(), 0); + // pidController.setSmartMotionMinOutputVelocity(minOutputVelocity.get(), 0); + // pidController.setSmartMotionMaxAccel(maxAccel.get(), 0); + + // convert the desired target degrees to encoder units + double rotations = EncoderUtils.elevatorHeightToRotations(position); + + // SmartDashboard.putNumber("[ELEVATOR] ff", ff); + SmartDashboard.putNumber("[ELEVATOR] reference", rotations); + + // set the reference point for the wrist + pidController.setReference(rotations, ControlType.kPosition, 0, ff); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("[ELEVATOR] Height", getHeight()); + SmartDashboard.putNumber("[ELEVATOR] Rotations", getRotations()); + } + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java new file mode 100644 index 000000000..f02571734 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java @@ -0,0 +1,74 @@ +package com.team766.robot.gatorade.mechanisms; + +/** + * Utility class to convert between encoder units and physical units we use for different + * mechanisms. + */ +public final class EncoderUtils { + + /** + * Utility class. + */ + private EncoderUtils() { + } + + /** + * Converts a target rotation (in degrees) to encoder units for the wrist motor. + */ + public static double wristDegreesToRotations(double angle) { + // angle * net gear ratio * (rotations / degrees) + // FIXME: replace 32 with actual # of teeth + return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.); + } + + /** + * Converts the wrist motor's rotations to degrees. + */ + public static double wristRotationsToDegrees(double rotations) { + // rotations * net gear ratio * (degrees / rotations) + // FIXME: replace 32 with actual # of teeth + return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.); + } + + /** + * Converts a desired height (in inches) to rotations for the elevator motors. + */ + public static double elevatorHeightToRotations(double height) { + // height * net gear ratio * (rotations / height) + return height * (36./12.) * (1./(1.641 * Math.PI)); + } + + /** + * Converts the elevator motor's rotations to a height (in inches). + */ + public static double elevatorRotationsToHeight(double rotations) { + // rotations * net gear ratio * (height / rotations) + // FIXME: everything + return rotations * (12./36.) * ((1.641 * Math.PI)/1.); + } + + /** + * Cosine law + * @param side1 + * @param side2 + * @param angle in degrees + * @return + */ + public static double lawOfCosines(double side1, double side2, double angle) { + double side3Squared = (Math.pow(side1,2.0)+Math.pow(side2,2.0)-2*side1*side2*Math.cos(Math.toRadians(angle))); + return Math.sqrt(side3Squared); + } + + public static double lawOfSines(double side1, double angle1, double side2) { + return Math.asin(side2*Math.sin(angle1)/side1); + } + + public static double clampValueToRange(double value, double min, double max) { + if (value > max){ + value = max; + } else if (value < min){ + value = min; + } + return value; + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java new file mode 100644 index 000000000..dafd427da --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java @@ -0,0 +1,63 @@ +package com.team766.robot.gatorade.mechanisms; +import com.ctre.phoenix.sensors.Pigeon2; +import com.team766.framework.Mechanism; +import com.team766.library.RateLimiter; +import com.team766.logging.Category; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Gyro extends Mechanism { + Pigeon2 g_gyro = new Pigeon2(0, SwerveDriveConstants.SWERVE_CANBUS); + double[] gyroArray = new double[3]; + private RateLimiter l_loggingRate = new RateLimiter(0.05); + public Gyro() { + loggerCategory = Category.GYRO; + } + public void resetGyro(){ + g_gyro.setYaw(0); + } + + public void resetGyro180() { + g_gyro.setYaw(180); + } + + public void setGyro(double angleDeg) { + g_gyro.setYaw(angleDeg); + } + + public double getGyroPitch() { + double angle = g_gyro.getPitch(); + return angle; + } + + public double getGyroYaw() { + double angle = -1* g_gyro.getYaw(); + return angle; + } + + public double getGyroRoll() { + double angle = g_gyro.getRoll(); + return angle; + } + + @Override + public void run() { + if (l_loggingRate.next()) { + gyroArray[0] = getGyroYaw(); + gyroArray[1] = getGyroPitch(); + gyroArray[2] = getGyroRoll(); + SmartDashboard.putNumber("Yaw", gyroArray[0]); + SmartDashboard.putNumber("Pitch", gyroArray[1]); + SmartDashboard.putNumber("Roll", gyroArray[2]); + g_gyro.getYawPitchRoll(gyroArray); + } + } + + /** + * @return combined pitch and roll values of gyro + */ + public double getAbsoluteTilt() { + return Math.toDegrees(Math.acos(Math.cos(Math.toRadians(getGyroRoll()))*Math.cos(Math.toRadians(getGyroPitch())))); + } + +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java new file mode 100644 index 000000000..b0aa3dc0c --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java @@ -0,0 +1,133 @@ +package com.team766.robot.gatorade.mechanisms; + +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + +/** + * Basic intake. Mounted on end of {@link Wrist}. The intake can be controlled to attempt to + * pull a game piece in via {@link #in}, release a contained game piece via {@link #out}, or stop + * moving via {@link #stop}. + * + * + * While the Intake does not maintain any state as to whether or not it contains a game piece, + * it does have different modes of operation based on what kind of game piece it is prepared to + * intake or outtake. This is because the motor must spin in opposite directions to intake cubes + * versus cones. + */ +public class Intake extends Mechanism { + + private static final double POWER_IN = 0.3; + private static final double POWER_OUT = 0.25; + private static final double POWER_IDLE = 0.02; + + /** + * The current type of game piece the Intake is preparing to hold or is holding. + */ + public enum GamePieceType { + CONE, + CUBE + } + + /** + * The current movement state for the intake. + */ + public enum State { + STOPPED, + IDLE, + IN, + OUT + } + + private MotorController motor; + private GamePieceType gamePieceType = GamePieceType.CONE; + private State state = State.IDLE; + private RateLimiter rateLimiter = new RateLimiter(5 /* seconds */); + + /** + * Constructs a new Intake. + */ + public Intake() { + motor = RobotProvider.instance.getMotor(INTAKE_MOTOR); + } + + /** + * Returns the type of game piece the Intake is preparing to hold or is holding. + * @return The current game piece type. + */ + public GamePieceType getGamePieceType() { + return gamePieceType; + } + + /** + * Sets the type of game piece type the Intake is preparing to hold or is holding. + */ + public void setGamePieceType(GamePieceType type) { + checkContextOwnership(); + + this.gamePieceType = type; + } + + /** + * Returns the current movement state of the intake. + * + * @return The current movement state of the intake. + */ + public State getState() { + return state; + } + + /** + * Turns the intake motor on in order to pull a game piece into the mechanism. + */ + public void in() { + checkContextOwnership(); + + double power = (gamePieceType == GamePieceType.CONE) ? POWER_IN : (-1 * POWER_IN); + motor.set(power); + state = State.IN; + } + + /** + * Turns the intake motor on in reverse direction, to release any contained game piece. + */ + public void out() { + checkContextOwnership(); + + double power = (gamePieceType == GamePieceType.CONE) ? (-1 * POWER_OUT) : POWER_OUT; + motor.set(power); + state = State.OUT; + } + + /** + * Turns off the intake motor. + */ + public void stop() { + checkContextOwnership(); + motor.set(0.0); + state = State.STOPPED; + } + + /** + * Turns the intake to idle - run at low power to keep the game piece contained. + */ + public void idle() { + checkContextOwnership(); + + double power = (gamePieceType == GamePieceType.CONE) ? POWER_IDLE : (-1 * POWER_IDLE); + motor.set(power); + state = State.IDLE; + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putString("[INTAKE] Game Piece", gamePieceType.toString()); + SmartDashboard.putString("[INTAKE] State", state.toString()); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java new file mode 100644 index 000000000..e3c86a83a --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java @@ -0,0 +1,55 @@ +package com.team766.robot.gatorade.mechanisms; +import com.ctre.phoenix.led.Animation; +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.RainbowAnimation; +import com.team766.framework.Mechanism; + + +public class Lights extends Mechanism{ + + private CANdle candle; + private static final int CANID = 5; + private static final int LED_COUNT = 90; + private Animation rainbowAnimation = new RainbowAnimation(1, 0.1, LED_COUNT); + + public Lights(){ + candle = new CANdle(CANID); + + } + + public void purple(){ + checkContextOwnership(); + candle.setLEDs(128, 0, 128); + } + + public void white(){ + checkContextOwnership(); + // NOTE: 255, 255, 255 trips the breaker. lol + candle.setLEDs(128, 128, 128); + } + + public void yellow(){ + checkContextOwnership(); + candle.setLEDs(255, 255, 0); + } + + public void red() { + checkContextOwnership(); + candle.setLEDs(255, 0, 0); + } + + public void green() { + checkContextOwnership(); + candle.setLEDs(0, 255, 0); + } + + public void orange() { + checkContextOwnership(); + candle.setLEDs(255, 64, 0); + } + + public void rainbow() { + checkContextOwnership(); + candle.animate(rainbowAnimation); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java new file mode 100644 index 000000000..3357153e9 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java @@ -0,0 +1,186 @@ +package com.team766.robot.gatorade.mechanisms; + +import com.team766.config.ConfigFileReader; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + +/** + * Basic wrist mechanism. Used in conjunction with the {@link Intake} and {@link Elevator}. + * Can be moved up and down as part of teleop or autonomous control to move the {@link Intake} + * (attached to the end of the Wrist) closer to a game piece or game element (eg node in the + * field, human player station), at which point the {@link Intake} can grab or release the game + * piece as appropriate. + */ +public class Wrist extends Mechanism { + + /** + * Pre-set positions for the wrist. + */ + public enum Position { + + // TODO: adjust these values. + + /** Wrist is in top position. Starting position. */ + TOP(-135), + /** Wrist is in the position for moving around the field. */ + RETRACTED(-120.0), + /** Wrist is level with ground. */ + LEVEL(0.0), + HIGH_NODE(-30), + MID_NODE(-25.5), + /** Wrist is fully down. */ + BOTTOM(60); + + private final double angle; + + Position(double angle) { + this.angle = angle; + } + + private double getAngle() { + return angle; + } + } + + private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.15; + + private static final double NEAR_THRESHOLD = 5.0; + + private final CANSparkMax motor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Contructs a new Wrist. + */ + public Wrist() { + MotorController halMotor = RobotProvider.instance.getMotor(WRIST_MOTOR); + if (!(halMotor instanceof CANSparkMax)) { + log(Severity.ERROR, "Motor is not a CANSparkMax!"); + throw new IllegalStateException("Motor is not a CANSparkMax!"); + } + motor = (CANSparkMax) halMotor; + + resetEncoder(); + + // stash the PIDController for convenience. will update the PID values to the latest from the config + // file each time we use the motor. + pidController = motor.getPIDController(); + pidController.setFeedbackDevice(motor.getEncoder()); + + // grab config values for PID. + pGain = ConfigFileReader.getInstance().getDouble(WRIST_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(WRIST_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(WRIST_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN); + } + + public void resetEncoder() { + motor.getEncoder() + .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); + } + + public double getRotations() { + return motor.getEncoder().getPosition(); + } + + /** + * Returns the current angle of the wrist. + */ + public double getAngle() { + return EncoderUtils.wristRotationsToDegrees(motor.getEncoder().getPosition()); + } + + public boolean isNearTo(Position position) { + return isNearTo(position.getAngle()); + } + + public boolean isNearTo(double angle) { + return Math.abs(angle - getAngle()) < NEAR_THRESHOLD; + } + + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + motor.set(clampedValue); + } + + public void stopWrist() { + checkContextOwnership(); + motor.set(0); + } + + public void nudgeUp() { + System.err.println("Nudging up."); + double angle = getAngle(); + double targetAngle = Math.max(angle - NUDGE_INCREMENT, Position.TOP.getAngle()); + System.err.println("Target: " + targetAngle); + + rotate(targetAngle); + } + + public void nudgeDown() { + System.err.println("Nudging down."); + double angle = getAngle(); + double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.BOTTOM.getAngle()); + rotate(targetAngle); + } + + /** + * Rotates the wrist to a pre-set {@link Position}. + */ + public void rotate(Position position) { + rotate(position.getAngle()); + } + + /** + * Starts rotating the wrist to the specified angle. + * NOTE: this method returns immediately. Check the current wrist position of the wrist + * with {@link #getAngle()}. + */ + public void rotate(double angle) { + checkContextOwnership(); + + System.err.println("Setting target angle to " + angle); + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + // pidController.setFF(ffGain.get()); + double ff = ffGain.get() * Math.cos(Math.toRadians(angle)); + SmartDashboard.putNumber("[WRIST] ff", ff); + SmartDashboard.putNumber("[WRIST] reference", angle); + + pidController.setOutputRange(-1, 1); + + // convert the desired target degrees to rotations + double rotations = EncoderUtils.wristDegreesToRotations(angle); + + // set the reference point for the wrist + pidController.setReference(rotations, ControlType.kPosition, 0, ff); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("[WRIST] Angle", getAngle()); + SmartDashboard.putNumber("[WRIST] Rotations", getRotations()); + } + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java b/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java new file mode 100644 index 000000000..4b48015da --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java @@ -0,0 +1,84 @@ +package com.team766.robot.gatorade.procedures; + +import java.util.ArrayList; +import java.util.List; +import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.odometry.PointDir; +import com.team766.robot.gatorade.constants.ChargeConstants; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +// Helper class for AlignCharger, see AlignCharger.md for more details +public class ChargeStationPathFinder { + + // Since this class does not extend Procedure, we need to instantiate logger + private static final Logger logger = Logger.get(Category.PROCEDURES); + private final Alliance alliance; + + /** + * Constructor which takes alliance + * @param alliance Alliance for choosing which charge station to align to + */ + public ChargeStationPathFinder(Alliance alliance) { + this.alliance = alliance; + } + + /** + * Calculates points that the robot needs to follow based on alliance and current position + * @param curPos the current robot position, represented by a pointDir + * @return PointDir[]: list of the calculated points that can be entered into FollowPoints + */ + public PointDir[] calculatePoints(PointDir curPos) { + + // Sets curX and curY variables based on curPos param + double curX = curPos.getX(); + double curY = curPos.getY(); + + // Creates ArrayList for points + List points = new ArrayList(); + switch (alliance) { + case Red: + // Calls addPoints method for red alliance coordinates + addPoints(points, curX, curY, ChargeConstants.RED_BALANCE_TARGET_X, ChargeConstants.RED_LEFT_PT, ChargeConstants.RED_RIGHT_PT, ChargeConstants.MIDDLE); + break; + + case Blue: + // Calls addPoints method for red alliance coordinates + addPoints(points, curX, curY, ChargeConstants.BLUE_BALANCE_TARGET_X, ChargeConstants.BLUE_LEFT_PT, ChargeConstants.BLUE_RIGHT_PT, ChargeConstants.MIDDLE); + break; + default: + logger.logRaw(null, "Invalid Alliance"); + } + + // Converts pointDir arrayList to array and returns it + return points.toArray(new PointDir[points.size()]); + } + + /** + * Adds a set of points to an arrayList based on passed charge station coordinates + * @param points ArrayList to add points to + * @param curX Current robot X value + * @param curY Current robot Y value + * @param target X value of center of target charge station + * @param left X value of left side of target charge station + * @param right X value of right side of target charge station + * @param height Y value of center of target charge station + */ + private void addPoints(List points, double curX, double curY, double target, double left, double right, double height) { + // If on the right side of the target, check if robot is blocked by the right side of the charge station, and add points accordingly + if (curX > target) { + if (curX < right) + points.add(new PointDir(right, curY)); + + points.add(new PointDir(right, height)); + + // Otherwise, it will be on the left side of the target, so check if robot is blocked by the left side of the charge station, and add points accordingly + } else { + if (curX > left) + points.add(new PointDir(left, curY)); + + points.add(new PointDir(left, height)); + } + } + +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java new file mode 100644 index 000000000..5ed5eb5a9 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java @@ -0,0 +1,11 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Wrist; + +public class ExtendWristvatorToHigh extends MoveWristvator { + + public ExtendWristvatorToHigh() { + super(Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java new file mode 100644 index 000000000..66b8f21f4 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java @@ -0,0 +1,15 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Wrist; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class ExtendWristvatorToHuman extends MoveWristvator { + + public ExtendWristvatorToHuman(GamePieceType gamePieceType) { + super(gamePieceType == GamePieceType.CONE + ? Elevator.Position.HUMAN_CONES + : Elevator.Position.HUMAN_CUBES, + Wrist.Position.LEVEL); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java new file mode 100644 index 000000000..c009207b7 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java @@ -0,0 +1,11 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Wrist; + +public class ExtendWristvatorToLow extends MoveWristvator { + + public ExtendWristvatorToLow() { + super(Elevator.Position.LOW, Wrist.Position.LEVEL); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java new file mode 100644 index 000000000..c8fd16188 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java @@ -0,0 +1,11 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Wrist; + +public class ExtendWristvatorToMid extends MoveWristvator { + + public ExtendWristvatorToMid() { + super(Elevator.Position.MID, Wrist.Position.MID_NODE); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java b/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java new file mode 100644 index 000000000..fd8cbdab5 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java @@ -0,0 +1,16 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class GoForCones extends Procedure { + + @Override + public void run(Context context) { + context.takeOwnership(Robot.intake); + + Robot.intake.setGamePieceType(GamePieceType.CONE); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java b/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java new file mode 100644 index 000000000..4b6673a26 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java @@ -0,0 +1,16 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class GoForCubes extends Procedure { + + @Override + public void run(Context context) { + context.takeOwnership(Robot.intake); + + Robot.intake.setGamePieceType(GamePieceType.CUBE); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java new file mode 100644 index 000000000..a7ffb2b23 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java @@ -0,0 +1,178 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.constants.ChargeConstants; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * {@link Procedure} to use the gyro to automatically balance on the charge station. + * See GyroBalance.md for more details + */ +public class GyroBalance extends Procedure { + + // State machine with 4 states for position on ramp + private enum State { + GROUND, + RAMP_TRANSITION, + RAMP_TILT, + RAMP_LEVEL + } + + // Direction determines which direction the robot moves + private enum Direction { + LEFT, + RIGHT, + STOP, + } + + // tilt is the overall combination of pitch and roll + private double tilt = Robot.gyro.getAbsoluteTilt(); + + // absSpeed is unsigned speed value + private double absSpeed; + private State prevState; + private State curState; + private Direction direction; + private final Alliance alliance; + + private final double TOP_TILT = 15.0; + private final double FLAP_TILT = 11; + + // Tweak these values to adjust how the robot balances + private final double LEVEL = 7; + private final double CORRECTION_DELAY = 0.7; + private final double SPEED_GROUND = .3; + private final double SPEED_TRANSITION = .25; + private final double SPEED_TILT = .18; + + /** + * Constructor to create a new GyroBalance instance + * @param alliance Alliance for setting direction towards charge station + */ + public GyroBalance(Alliance alliance) { + this.alliance = alliance; + } + + public void run(Context context) { + context.takeOwnership(Robot.gyro); + context.takeOwnership(Robot.drive); + + // curY is current robot y position + double curY = Robot.drive.getCurrentPosition().getY(); + + // driveSpeed is actual value of speed passed into the swerveDrive method + double driveSpeed = 1; + + // Sets movement direction ground state if on ground + setDir(curY); + + // sets starting state if not on ground + if (tilt < LEVEL && curState != State.GROUND) { + curState = State.RAMP_LEVEL; + } else if (tilt < TOP_TILT && tilt > FLAP_TILT) { + curState = State.RAMP_TILT; + } else if (tilt > LEVEL) { + curState = State.RAMP_TRANSITION; + } + + do { + // Sets prevState to the current state and calculates curState + prevState = curState; + curY = Robot.drive.getCurrentPosition().getY(); + tilt = Robot.gyro.getAbsoluteTilt(); + //log("curX:" + curX); + //log("direction: " + direction); + setState(); + + // Both being on Red alliance and needing to move right would make the movement direction negative, so this expression corrects for that + if ((alliance == Alliance.Red) ^ (direction == Direction.RIGHT)) { + driveSpeed = -absSpeed; + } else { + driveSpeed = absSpeed; + } + + // Drives the robot with the calculated speed and direction + Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), 0, driveSpeed, 0); + context.yield(); + } + // Loops until robot is level or until a call to the abort() method + while (!(curState == State.RAMP_LEVEL)); + + // After the robot is level, drives for correctionDelay seconds. + // Direction is opposite due to inversion of speed in setState() so it corrects for overshooting + context.waitForSeconds(CORRECTION_DELAY); + + // Locks wheels once balanced + context.startAsync(new SetCross()); + + context.releaseOwnership(Robot.drive); + context.releaseOwnership(Robot.gyro); + } + + // Sets state in state machine, see more details in GyroBalance.md + private void setState() { + if (prevState == State.GROUND && tilt > LEVEL) { + curState = State.RAMP_TRANSITION; + absSpeed = SPEED_TRANSITION; + log("Transition, prevState: " + prevState + ", curState: " + curState); + } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { + curState = State.RAMP_TILT; + absSpeed = SPEED_TILT; + log("Tilt, prevState: " + prevState + ", curState: " + curState); + } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { + curState = State.RAMP_LEVEL; + // If level, sets speed to negative to correct for overshooting + absSpeed = -absSpeed; + log("Level, prevState: " + prevState + ", curState: " + curState); + } + if (curState == State.GROUND) { + absSpeed = SPEED_GROUND; + } + } + + /** + * Sets direction towards desired charge station + * If robot is level and outside of charge station boundaries, sets state to ground + * @param curY current robot x position + */ + private void setDir(double curY) { + switch (alliance) { + case Red: + // If to the right of the charge station, go left + if (curY > ChargeConstants.RED_BALANCE_TARGET_X) { + // If level and outside of charge station boundaries, set state to ground + if (tilt < LEVEL && curY > ChargeConstants.RED_RIGHT_PT) { + curState = State.GROUND; + } + direction = Direction.LEFT; + // If to the left of the charge station, go right + } else { + if (tilt < LEVEL && curY < ChargeConstants.RED_LEFT_PT) { + curState = State.GROUND; + } + direction = Direction.RIGHT; + } + break; + case Blue: + // Same logic for blue alliance coordinates + if (curY > ChargeConstants.BLUE_BALANCE_TARGET_X) { + if (tilt < LEVEL && curY > ChargeConstants.BLUE_RIGHT_PT) { + curState = State.GROUND; + } + direction = Direction.LEFT; + } else { + if (tilt < LEVEL && curY < ChargeConstants.BLUE_LEFT_PT) { + curState = State.GROUND; + } + direction = Direction.RIGHT; + } + break; + default: + log("Invalid alliance"); + } + } + +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java new file mode 100644 index 000000000..ce045141b --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java @@ -0,0 +1,13 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; + +public class IntakeIdle extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.intake); + + Robot.intake.idle(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java new file mode 100644 index 000000000..e7da25948 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java @@ -0,0 +1,12 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; + +public class IntakeIn extends Procedure{ + public void run(Context context){ + context.takeOwnership(Robot.intake); + Robot.intake.in(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java new file mode 100644 index 000000000..aab4a8592 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java @@ -0,0 +1,12 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; + +public class IntakeOut extends Procedure{ + public void run(Context context){ + context.takeOwnership(Robot.intake); + Robot.intake.out(); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java new file mode 100644 index 000000000..e4b850c46 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java @@ -0,0 +1,12 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; + +public class IntakeStop extends Procedure{ + public void run(Context context){ + context.takeOwnership(Robot.intake); + Robot.intake.stop(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java new file mode 100644 index 000000000..38f440795 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java @@ -0,0 +1,36 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Wrist; + +public class MoveWristvator extends Procedure { + private final Elevator.Position elevatorSetpoint; + private final Wrist.Position wristSetpoint; + + public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) { + this.elevatorSetpoint = elevatorSetpoint_; + this.wristSetpoint = wristSetpoint_; + } + + @Override + public final void run(Context context) { + context.takeOwnership(Robot.wrist); + context.takeOwnership(Robot.elevator); + + // Always retract the wrist before moving the elevator. + // It might already be retracted, so it's possible that this step finishes instantaneously. + Robot.wrist.rotate(Wrist.Position.RETRACTED); + context.waitFor(() -> Robot.wrist.isNearTo(Wrist.Position.RETRACTED)); + + // Move the elevator. Wait until it gets near the target position. + Robot.elevator.moveTo(elevatorSetpoint); + context.waitFor(() -> Robot.elevator.isNearTo(elevatorSetpoint)); + + // Lastly, move the wrist. + Robot.wrist.rotate(wristSetpoint); + context.waitFor(() -> Robot.wrist.isNearTo(wristSetpoint)); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java b/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java new file mode 100644 index 000000000..8d3e4631c --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java @@ -0,0 +1,23 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.constants.FollowPointsInputConstants; + +public class OPECHelper extends Procedure { + + private static final double DIST = 4; + + public void run(Context context) { + context.takeOwnership(Robot.drive); + // context.takeOwnership(Robot.intake); + double startY = Robot.drive.getCurrentPosition().getY(); + // robot gyro is offset 90º from how we want, so we reset it to 90º to account for this + Robot.gyro.resetGyro(); + // new IntakeRelease().run(context); + Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0); + context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST); + Robot.drive.stopDrive(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java new file mode 100644 index 000000000..2f5628cf4 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java @@ -0,0 +1,31 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.odometry.PointDir; +import com.team766.robot.gatorade.Robot; + +import edu.wpi.first.wpilibj.DriverStation; + +public class OnePieceBalance extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.drive); + //context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.gyro); + Robot.gyro.resetGyro(); + switch (DriverStation.getAlliance()) { + case Blue: + Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); + break; + case Red: + Robot.drive.setCurrentPosition(new PointDir(2.7, 14.5)); + break; + default: + log("invalid alliance"); + return; + + } + // new IntakeRelease().run(context); + new GyroBalance(DriverStation.getAlliance()).run(context); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java new file mode 100644 index 000000000..9a9242621 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java @@ -0,0 +1,30 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.odometry.PointDir; +import com.team766.robot.gatorade.Robot; + +import edu.wpi.first.wpilibj.DriverStation; + +public class OnePieceExitCommunity extends Procedure { + public void run (Context context) { + context.takeOwnership(Robot.drive); + //context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.gyro); + Robot.gyro.resetGyro(); + switch (DriverStation.getAlliance()) { + case Blue: + Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); + break; + case Red: + Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); + break; + default: + log("invalid alliance"); + return; + } + log("exiting"); + new OPECHelper().run(context); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java new file mode 100644 index 000000000..8e81519a0 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java @@ -0,0 +1,33 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.odometry.PointDir; +import com.team766.robot.gatorade.Robot; + +import edu.wpi.first.wpilibj.DriverStation; + +public class OnePieceExitCommunityBalance extends Procedure { + public void run (Context context) { + context.takeOwnership(Robot.drive); + //context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.gyro); + Robot.gyro.resetGyro(); + switch (DriverStation.getAlliance()) { + case Blue: + Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); + break; + case Red: + Robot.drive.setCurrentPosition(new PointDir(2.7, 14.5)); + break; + default: + log("invalid alliance"); + return; + } + log("exiting"); + new OPECHelper().run(context); + log("Transitioning"); + new GyroBalance(DriverStation.getAlliance()).run(context); + } + +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java new file mode 100644 index 000000000..1feb332a8 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java @@ -0,0 +1,11 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Wrist; + +public class RetractWristvator extends MoveWristvator { + + public RetractWristvator() { + super(Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java b/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java new file mode 100644 index 000000000..34e4e9ad4 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java @@ -0,0 +1,15 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; + +public class SetCross extends Procedure { + + public void run(Context context) { + context.takeOwnership(Robot.drive); + Robot.drive.stopDrive(); + Robot.drive.setCross(); + } + +} diff --git a/src/main/proto/com/team766/logging/logging.proto b/src/main/proto/com/team766/logging/logging.proto index 40dc4b72f..46dca6654 100644 --- a/src/main/proto/com/team766/logging/logging.proto +++ b/src/main/proto/com/team766/logging/logging.proto @@ -37,6 +37,8 @@ enum Category { TRAJECTORY = 9; OPERATOR_INTERFACE = 10; DRIVE = 11; + GYRO = 12; + ODOMETRY = 13; } // `Value` represents a dynamically typed value which can be either a number, @@ -60,4 +62,4 @@ message LogValue { message LogListValue { repeated LogValue element = 1; -} \ No newline at end of file +} From 2a120649b83b489595a4f95bf7fddb11944d90f9 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Tue, 9 Jan 2024 01:20:55 -0800 Subject: [PATCH 2/3] temporarily tie odometry to gatorade code. will clean up in future cls as we iterate on odometry. --- .../java/com/team766/odometry/Odometry.java | 411 ++++++------ src/main/java/com/team766/odometry/Point.java | 169 ++--- .../java/com/team766/odometry/PointDir.java | 137 ++-- .../robot/gatorade/AutonomousModes.java | 47 +- .../java/com/team766/robot/gatorade/OI.java | 590 +++++++++--------- .../robot/gatorade/PlacementPosition.java | 10 +- .../com/team766/robot/gatorade/Robot.java | 32 +- .../team766/robot/gatorade/RobotTargets.java | 154 +++-- .../gatorade/constants/ChargeConstants.java | 34 +- .../gatorade/constants/ConfigConstants.java | 70 +-- .../constants/FollowPointsInputConstants.java | 12 +- .../gatorade/constants/InputConstants.java | 86 +-- .../constants/OdometryInputConstants.java | 29 +- .../constants/SwerveDriveConstants.java | 27 +- .../robot/gatorade/mechanisms/Drive.java | 536 +++++++++------- .../robot/gatorade/mechanisms/Elevator.java | 367 +++++------ .../gatorade/mechanisms/EncoderUtils.java | 120 ++-- .../robot/gatorade/mechanisms/Gyro.java | 113 ++-- .../robot/gatorade/mechanisms/Intake.java | 228 +++---- .../robot/gatorade/mechanisms/Lights.java | 95 ++- .../robot/gatorade/mechanisms/Wrist.java | 332 +++++----- .../procedures/ChargeStationPathFinder.java | 152 +++-- .../procedures/ExtendWristvatorToHigh.java | 6 +- .../procedures/ExtendWristvatorToHuman.java | 15 +- .../procedures/ExtendWristvatorToLow.java | 6 +- .../procedures/ExtendWristvatorToMid.java | 6 +- .../robot/gatorade/procedures/GoForCones.java | 10 +- .../robot/gatorade/procedures/GoForCubes.java | 14 +- .../gatorade/procedures/GyroBalance.java | 327 +++++----- .../robot/gatorade/procedures/IntakeIdle.java | 8 +- .../robot/gatorade/procedures/IntakeIn.java | 10 +- .../robot/gatorade/procedures/IntakeOut.java | 12 +- .../robot/gatorade/procedures/IntakeStop.java | 10 +- .../gatorade/procedures/MoveWristvator.java | 42 +- .../robot/gatorade/procedures/OPECHelper.java | 25 +- .../gatorade/procedures/OnePieceBalance.java | 47 +- .../procedures/OnePieceExitCommunity.java | 50 +- .../OnePieceExitCommunityBalance.java | 53 +- .../team766/robot/gatorade/procedures/README | 1 + .../procedures/RetractWristvator.java | 6 +- .../robot/gatorade/procedures/SetCross.java | 13 +- 41 files changed, 2303 insertions(+), 2109 deletions(-) create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/README diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java index 26f1b1343..9d5131a9f 100644 --- a/src/main/java/com/team766/odometry/Odometry.java +++ b/src/main/java/com/team766/odometry/Odometry.java @@ -1,201 +1,240 @@ package com.team766.odometry; +import com.ctre.phoenix.sensors.CANCoder; import com.team766.framework.LoggingBase; import com.team766.hal.MotorController; import com.team766.library.RateLimiter; -import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; -import com.ctre.phoenix.sensors.CANCoder; import com.team766.logging.Category; -import com.team766.robot.*; +import com.team766.robot.gatorade.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; /** * Method which calculates the position of the robot based on wheel positions. */ public class Odometry extends LoggingBase { - private RateLimiter odometryLimiter; - - private MotorController[] motorList; - //The order of CANCoders should be the same as in motorList - private CANCoder[] CANCoderList; - private int motorCount; - - private PointDir[] prevPositions; - private PointDir[] currPositions; - private double[] prevEncoderValues; - private double[] currEncoderValues; - private double gyroPosition; - - private PointDir currentPosition; - - //In Meters - private static double WHEEL_CIRCUMFERENCE; - public static double GEAR_RATIO; - public static int ENCODER_TO_REVOLUTION_CONSTANT; - - //In the same order as motorList, relative to the center of the robot - private Point[] wheelPositions; - - /** - * Constructor for Odometry, taking in several defines for the robot. - * @param motors A list of every wheel-controlling motor on the robot. - * @param CANCoders A list of the CANCoders corresponding to each wheel, in the same order as motors. - * @param wheelLocations A list of the locations of each wheel, in the same order as motors. - * @param wheelCircumference The circumfrence of the wheels, including treads. - * @param gearRatio The gear ratio of the wheels. - * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels. - * @param rateLimiterTime How often odometry should run. - */ - public Odometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime) { - loggerCategory = Category.ODOMETRY; - - odometryLimiter = new RateLimiter(rateLimiterTime); - motorList = motors; - CANCoderList = CANCoders; - motorCount = motorList.length; - log("Motor count " + motorCount); - prevPositions = new PointDir[motorCount]; - currPositions = new PointDir[motorCount]; - prevEncoderValues = new double[motorCount]; - currEncoderValues = new double[motorCount]; - - wheelPositions = wheelLocations; - WHEEL_CIRCUMFERENCE = wheelCircumference; - GEAR_RATIO = gearRatio; - ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant; - - currentPosition = new PointDir(0, 0, 0); - for (int i = 0; i < motorCount; i++) { - prevPositions[i] = new PointDir(0,0, 0); - currPositions[i] = new PointDir(0,0, 0); - prevEncoderValues[i] = 0; - currEncoderValues[i] = 0; - } - } - - public String getName() { - return "Odometry"; - } - - /** - * Sets the current position of the robot to Point P - * @param P The point to set the current robot position to - */ - public void setCurrentPosition(Point P) { - currentPosition.set(P); - log("Set Current Position to: " + P.toString()); - for (int i = 0; i < motorCount; i++) { - prevPositions[i].set(currentPosition.add(wheelPositions[i])); - currPositions[i].set(currentPosition.add(wheelPositions[i])); - } - log("Current Position: " + currentPosition.toString()); - } - - - /** - * Updates the odometry encoder values to the robot encoder values. - */ - private void setCurrentEncoderValues() { - for (int i = 0; i < motorCount; i++) { - prevEncoderValues[i] = currEncoderValues[i]; - currEncoderValues[i] = motorList[i].getSensorPosition(); - currEncoderValues[i] *= (DriverStation.getAlliance() == Alliance.Blue ? 1 : -1); - } - } - - private static Vector2D rotate(Vector2D v, double angle) { - return new Vector2D( - v.getX() * Math.cos(angle) - Math.sin(angle) * v.getY(), - v.getY() * Math.cos(angle) + v.getX() * Math.sin(angle)); - } - - /** - * Updates the position of each wheel of the robot by assuming each wheel moved in an arc. - */ - private void updateCurrentPositions() { - double angleChange; - double radius; - double deltaX; - double deltaY; - gyroPosition = -Robot.gyro.getGyroYaw(); - - /* - Point slopeFactor = new Point(Math.sqrt(Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))), - Math.sqrt(Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())))); - */ - - for (int i = 0; i < motorCount; i++) { - //prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currPositions[i].getHeading()); - //This following line only works if the average of wheel positions is (0,0) - prevPositions[i].set(currentPosition.add(wheelPositions[i]), currPositions[i].getHeading()); - currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition); - angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading(); - - double yaw = -Math.toRadians(Robot.gyro.getGyroYaw()); - double roll = Math.toRadians(Robot.gyro.getGyroRoll()); - double pitch = Math.toRadians(Robot.gyro.getGyroPitch()); - - double w = Math.toRadians(CANCoderList[i].getAbsolutePosition()); - Vector2D u = new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch)); - Vector2D v = new Vector2D(Math.cos(yaw) * Math.sin(pitch) * Math.sin(roll) - Math.sin(yaw) * Math.cos(roll), - Math.sin(yaw) * Math.sin(pitch) * Math.sin(roll) + Math.cos(yaw) * Math.cos(roll)); - Vector2D a = u.scalarMultiply(Math.cos(w)).add(v.scalarMultiply(Math.sin(w))); - Vector2D b = u.scalarMultiply(-Math.sin(w)).add(v.scalarMultiply(Math.cos(w))); - Vector2D wheelMotion; - - //log("u: " + u + " v: " + v + " a: " + a + " b: " + b); - - //double oldWheelX; - //double oldWheelY; - - if (angleChange != 0) { - radius = 180 * (currEncoderValues[i] - prevEncoderValues[i]) / (Math.PI * angleChange); - deltaX = radius * Math.sin(Math.toRadians(angleChange)); - deltaY = radius * (1 - Math.cos(Math.toRadians(angleChange))); - - wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY)); - - //oldWheelX = ((Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX - Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - //oldWheelY = ((Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX + Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - - } else { - wheelMotion = a.scalarMultiply((currEncoderValues[i] - prevEncoderValues[i])); - - //oldWheelX = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - //oldWheelY = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - } - wheelMotion = wheelMotion.scalarMultiply(WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - //wheelMotion = rotate(wheelMotion, Math.toRadians(gyroPosition)); - //log("Difference: " + (oldWheelX - wheelMotion.getX()) + ", " + (oldWheelY - wheelMotion.getY()) + "Old Method: " + oldWheelX + ", " + oldWheelY + "Current Method: " + wheelMotion.getX() + ", " + wheelMotion.getY()); - //log("Current: " + currPositions[i] + " Motion: " + wheelMotion + " New: " + currPositions[i].add(wheelMotion)); - currPositions[i].set(currPositions[i].subtract(wheelMotion)); - } - } - - /** - * Calculates the position of the robot by finding the average of the wheel positions. - */ - private void findRobotPosition() { - double sumX = 0; - double sumY = 0; - for (int i = 0; i < motorCount; i++) { - sumX += currPositions[i].getX(); - sumY += currPositions[i].getY(); - //log("sumX: " + sumX + " Motor Count: " + motorCount + " CurrentPosition: " + currPositions[i]); - } - currentPosition.set(sumX / motorCount, sumY / motorCount, gyroPosition); - } - - //Intended to be placed inside Robot.drive.run() - public PointDir run() { - if (odometryLimiter.next()) { - setCurrentEncoderValues(); - updateCurrentPositions(); - findRobotPosition(); - log(currentPosition.toString()); - } - return currentPosition; - } + private RateLimiter odometryLimiter; + + private MotorController[] motorList; + // The order of CANCoders should be the same as in motorList + private CANCoder[] CANCoderList; + private int motorCount; + + private PointDir[] prevPositions; + private PointDir[] currPositions; + private double[] prevEncoderValues; + private double[] currEncoderValues; + private double gyroPosition; + + private PointDir currentPosition; + + // In Meters + private static double WHEEL_CIRCUMFERENCE; + public static double GEAR_RATIO; + public static int ENCODER_TO_REVOLUTION_CONSTANT; + + // In the same order as motorList, relative to the center of the robot + private Point[] wheelPositions; + + /** + * Constructor for Odometry, taking in several defines for the robot. + * @param motors A list of every wheel-controlling motor on the robot. + * @param CANCoders A list of the CANCoders corresponding to each wheel, in the same order as motors. + * @param wheelLocations A list of the locations of each wheel, in the same order as motors. + * @param wheelCircumference The circumfrence of the wheels, including treads. + * @param gearRatio The gear ratio of the wheels. + * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels. + * @param rateLimiterTime How often odometry should run. + */ + public Odometry( + MotorController[] motors, + CANCoder[] CANCoders, + Point[] wheelLocations, + double wheelCircumference, + double gearRatio, + int encoderToRevolutionConstant, + double rateLimiterTime) { + loggerCategory = Category.ODOMETRY; + + odometryLimiter = new RateLimiter(rateLimiterTime); + motorList = motors; + CANCoderList = CANCoders; + motorCount = motorList.length; + log("Motor count " + motorCount); + prevPositions = new PointDir[motorCount]; + currPositions = new PointDir[motorCount]; + prevEncoderValues = new double[motorCount]; + currEncoderValues = new double[motorCount]; + + wheelPositions = wheelLocations; + WHEEL_CIRCUMFERENCE = wheelCircumference; + GEAR_RATIO = gearRatio; + ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant; + + currentPosition = new PointDir(0, 0, 0); + for (int i = 0; i < motorCount; i++) { + prevPositions[i] = new PointDir(0, 0, 0); + currPositions[i] = new PointDir(0, 0, 0); + prevEncoderValues[i] = 0; + currEncoderValues[i] = 0; + } + } + + public String getName() { + return "Odometry"; + } + + /** + * Sets the current position of the robot to Point P + * @param P The point to set the current robot position to + */ + public void setCurrentPosition(Point P) { + currentPosition.set(P); + log("Set Current Position to: " + P.toString()); + for (int i = 0; i < motorCount; i++) { + prevPositions[i].set(currentPosition.add(wheelPositions[i])); + currPositions[i].set(currentPosition.add(wheelPositions[i])); + } + log("Current Position: " + currentPosition.toString()); + } + + /** + * Updates the odometry encoder values to the robot encoder values. + */ + private void setCurrentEncoderValues() { + for (int i = 0; i < motorCount; i++) { + prevEncoderValues[i] = currEncoderValues[i]; + currEncoderValues[i] = motorList[i].getSensorPosition(); + Optional alliance = DriverStation.getAlliance(); + currEncoderValues[i] *= + ((alliance.isPresent() && (alliance.get() == Alliance.Blue)) ? 1 : -1); + } + } + + private static Vector2D rotate(Vector2D v, double angle) { + return new Vector2D( + v.getX() * Math.cos(angle) - Math.sin(angle) * v.getY(), + v.getY() * Math.cos(angle) + v.getX() * Math.sin(angle)); + } + + /** + * Updates the position of each wheel of the robot by assuming each wheel moved in an arc. + */ + private void updateCurrentPositions() { + double angleChange; + double radius; + double deltaX; + double deltaY; + gyroPosition = -Robot.gyro.getGyroYaw(); + + /* + Point slopeFactor = new Point(Math.sqrt(Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))), + Math.sqrt(Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())))); + */ + + for (int i = 0; i < motorCount; i++) { + // prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * + // DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * + // Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), + // currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / + // motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / + // motorCount)), currPositions[i].getHeading()); + // This following line only works if the average of wheel positions is (0,0) + prevPositions[i].set( + currentPosition.add(wheelPositions[i]), currPositions[i].getHeading()); + currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition); + angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading(); + + double yaw = -Math.toRadians(Robot.gyro.getGyroYaw()); + double roll = Math.toRadians(Robot.gyro.getGyroRoll()); + double pitch = Math.toRadians(Robot.gyro.getGyroPitch()); + + double w = Math.toRadians(CANCoderList[i].getAbsolutePosition()); + Vector2D u = + new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch)); + Vector2D v = + new Vector2D( + Math.cos(yaw) * Math.sin(pitch) * Math.sin(roll) + - Math.sin(yaw) * Math.cos(roll), + Math.sin(yaw) * Math.sin(pitch) * Math.sin(roll) + + Math.cos(yaw) * Math.cos(roll)); + Vector2D a = u.scalarMultiply(Math.cos(w)).add(v.scalarMultiply(Math.sin(w))); + Vector2D b = u.scalarMultiply(-Math.sin(w)).add(v.scalarMultiply(Math.cos(w))); + Vector2D wheelMotion; + + // log("u: " + u + " v: " + v + " a: " + a + " b: " + b); + + // double oldWheelX; + // double oldWheelY; + + if (angleChange != 0) { + radius = + 180 + * (currEncoderValues[i] - prevEncoderValues[i]) + / (Math.PI * angleChange); + deltaX = radius * Math.sin(Math.toRadians(angleChange)); + deltaY = radius * (1 - Math.cos(Math.toRadians(angleChange))); + + wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY)); + + // oldWheelX = ((Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX - + // Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * + // slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * + // ENCODER_TO_REVOLUTION_CONSTANT)); + // oldWheelY = ((Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX + + // Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * + // slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * + // ENCODER_TO_REVOLUTION_CONSTANT)); + + } else { + wheelMotion = a.scalarMultiply((currEncoderValues[i] - prevEncoderValues[i])); + + // oldWheelX = ((currEncoderValues[i] - prevEncoderValues[i]) * + // Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() * + // WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + // oldWheelY = ((currEncoderValues[i] - prevEncoderValues[i]) * + // Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() * + // WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + } + wheelMotion = + wheelMotion.scalarMultiply( + WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + // wheelMotion = rotate(wheelMotion, Math.toRadians(gyroPosition)); + // log("Difference: " + (oldWheelX - wheelMotion.getX()) + ", " + (oldWheelY - + // wheelMotion.getY()) + "Old Method: " + oldWheelX + ", " + oldWheelY + "Current + // Method: " + wheelMotion.getX() + ", " + wheelMotion.getY()); + // log("Current: " + currPositions[i] + " Motion: " + wheelMotion + " New: " + + // currPositions[i].add(wheelMotion)); + currPositions[i].set(currPositions[i].subtract(wheelMotion)); + } + } + + /** + * Calculates the position of the robot by finding the average of the wheel positions. + */ + private void findRobotPosition() { + double sumX = 0; + double sumY = 0; + for (int i = 0; i < motorCount; i++) { + sumX += currPositions[i].getX(); + sumY += currPositions[i].getY(); + // log("sumX: " + sumX + " Motor Count: " + motorCount + " CurrentPosition: " + + // currPositions[i]); + } + currentPosition.set(sumX / motorCount, sumY / motorCount, gyroPosition); + } + + // Intended to be placed inside Robot.drive.run() + public PointDir run() { + if (odometryLimiter.next()) { + setCurrentEncoderValues(); + updateCurrentPositions(); + findRobotPosition(); + log(currentPosition.toString()); + } + return currentPosition; + } } diff --git a/src/main/java/com/team766/odometry/Point.java b/src/main/java/com/team766/odometry/Point.java index 5876c6a55..ac0bfca17 100644 --- a/src/main/java/com/team766/odometry/Point.java +++ b/src/main/java/com/team766/odometry/Point.java @@ -1,93 +1,94 @@ package com.team766.odometry; import com.team766.framework.LoggingBase; -import java.lang.Math; import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; /** * Class of two-coordinate objects, an x-coordinate and a y-coordinate. */ public class Point extends LoggingBase { - private double x; - private double y; - - public String getName() { - return "Points"; - } - - public Point(double x, double y){ - this.x = x; - this.y = y; - } - - public double getX() { - return x; - } - - public double getY() { - return y; - } - - public void set(double x, double y) { - this.x = x; - this.y = y; - } - - public void set(Point P) { - this.x = P.getX(); - this.y = P.getY(); - } - - public void setX(double x) { - this.x = x; - } - - public void setY(double y) { - this.y = y; - } - - public double distance(Point a) { - return Math.sqrt(Math.pow(a.getX() - getX(), 2.0) + Math.pow(a.getY() - getY(), 2.0)); - } - - public double slope(Point a) { - double s; - final int MAX = 1000; - if (a.getX() == getX()) { - //If the points are on top of each other, returns positive or negative MAX. - if (a.getY() < getY()) { - s = -MAX; - } else { - s = MAX; - } - } else { - s = (getY() - a.getY()) / (getX() - a.getX()); - } - if (Math.abs(s) > MAX) { - s = Math.signum(s) * MAX; - } - return s; - } - - //Gets a vector with length scale between the point and another point. Used for swerve drive method input. - public Point scaleVector(Point inputPoint, double scale) { - double d = distance(inputPoint); - return new Point((inputPoint.getX() - getX()) * scale / d, (inputPoint.getY() - getY()) * scale / d); - } - - public Point add(Point p) { - return new Point(getX() + p.getX(), getY() + p.getY()); - } - - public Point add(Vector2D v) { - return new Point(getX() + v.getX(), getY() + v.getY()); - } - - public Point subtract(Vector2D v) { - return new Point(getX() - v.getX(), getY() - v.getY()); - } - - public String toString() { - return "X: " + getX() + " Y: " + getY(); - } -} \ No newline at end of file + private double x; + private double y; + + public String getName() { + return "Points"; + } + + public Point(double x, double y) { + this.x = x; + this.y = y; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public void set(double x, double y) { + this.x = x; + this.y = y; + } + + public void set(Point P) { + this.x = P.getX(); + this.y = P.getY(); + } + + public void setX(double x) { + this.x = x; + } + + public void setY(double y) { + this.y = y; + } + + public double distance(Point a) { + return Math.sqrt(Math.pow(a.getX() - getX(), 2.0) + Math.pow(a.getY() - getY(), 2.0)); + } + + public double slope(Point a) { + double s; + final int MAX = 1000; + if (a.getX() == getX()) { + // If the points are on top of each other, returns positive or negative MAX. + if (a.getY() < getY()) { + s = -MAX; + } else { + s = MAX; + } + } else { + s = (getY() - a.getY()) / (getX() - a.getX()); + } + if (Math.abs(s) > MAX) { + s = Math.signum(s) * MAX; + } + return s; + } + + // Gets a vector with length scale between the point and another point. Used for swerve drive + // method input. + public Point scaleVector(Point inputPoint, double scale) { + double d = distance(inputPoint); + return new Point( + (inputPoint.getX() - getX()) * scale / d, (inputPoint.getY() - getY()) * scale / d); + } + + public Point add(Point p) { + return new Point(getX() + p.getX(), getY() + p.getY()); + } + + public Point add(Vector2D v) { + return new Point(getX() + v.getX(), getY() + v.getY()); + } + + public Point subtract(Vector2D v) { + return new Point(getX() - v.getX(), getY() - v.getY()); + } + + public String toString() { + return "X: " + getX() + " Y: " + getY(); + } +} diff --git a/src/main/java/com/team766/odometry/PointDir.java b/src/main/java/com/team766/odometry/PointDir.java index 2f00c413c..536013439 100644 --- a/src/main/java/com/team766/odometry/PointDir.java +++ b/src/main/java/com/team766/odometry/PointDir.java @@ -1,82 +1,89 @@ package com.team766.odometry; import com.team766.logging.Category; -import java.lang.Math; public class PointDir extends Point { - private double heading; + private double heading; - public PointDir(double x, double y, double h){ - super(x, y); - heading = h; - loggerCategory = Category.DRIVE; - } + public PointDir(double x, double y, double h) { + super(x, y); + heading = h; + loggerCategory = Category.DRIVE; + } - public PointDir(double x, double y){ - super(x, y); - loggerCategory = Category.DRIVE; - } + public PointDir(double x, double y) { + super(x, y); + loggerCategory = Category.DRIVE; + } - public PointDir(Point P){ - super(P.getX(), P.getY()); - loggerCategory = Category.DRIVE; - } + public PointDir(Point P) { + super(P.getX(), P.getY()); + loggerCategory = Category.DRIVE; + } - public PointDir(Point P, double h) { - super(P.getX(), P.getY()); - heading = h; - loggerCategory = Category.DRIVE; - } + public PointDir(Point P, double h) { + super(P.getX(), P.getY()); + heading = h; + loggerCategory = Category.DRIVE; + } - public double getHeading() { - return heading; - } + public double getHeading() { + return heading; + } - public void set(double x, double y, double h) { - super.set(x, y); - heading = h; - } + public void set(double x, double y, double h) { + super.set(x, y); + heading = h; + } - public void set(Point P, double h) { - super.set(P); - heading = h; - } + public void set(Point P, double h) { + super.set(P); + heading = h; + } - public void setHeading(double h) { - heading = h; - } + public void setHeading(double h) { + heading = h; + } - public double getAngleDifference(Point a) { - //Returns a number between -1 and 1 to represent the number of rotations between the two angles. - PointDir unitVector; - if (distance(a) == 0) { - unitVector = new PointDir(1, 0, 0); - } else { - unitVector = new PointDir((a.getX() - getX()) / distance(a), (a.getY() - getY()) / distance(a), Math.toDegrees(Math.atan2((a.getY() - getY()) / distance(a), (a.getX() - getX()) / distance(a)))); - } - double headingAngle = getHeading() % 360; - if (headingAngle < 0) { - headingAngle += 360; - } - if (unitVector.getHeading() < 0) { - unitVector.setHeading(unitVector.getHeading() + 360); - } - double diff = headingAngle - unitVector.getHeading(); - if (diff < -180) { - diff += 360; - } else if (diff > 180) { - diff -= 360; - } - return diff / 180; - } + public double getAngleDifference(Point a) { + // Returns a number between -1 and 1 to represent the number of rotations between the two + // angles. + PointDir unitVector; + if (distance(a) == 0) { + unitVector = new PointDir(1, 0, 0); + } else { + unitVector = + new PointDir( + (a.getX() - getX()) / distance(a), + (a.getY() - getY()) / distance(a), + Math.toDegrees( + Math.atan2( + (a.getY() - getY()) / distance(a), + (a.getX() - getX()) / distance(a)))); + } + double headingAngle = getHeading() % 360; + if (headingAngle < 0) { + headingAngle += 360; + } + if (unitVector.getHeading() < 0) { + unitVector.setHeading(unitVector.getHeading() + 360); + } + double diff = headingAngle - unitVector.getHeading(); + if (diff < -180) { + diff += 360; + } else if (diff > 180) { + diff -= 360; + } + return diff / 180; + } - @Override - public String toString() { - return super.toString() + " Heading: " + getHeading(); - } + @Override + public String toString() { + return super.toString() + " Heading: " + getHeading(); + } - @Override - public PointDir clone() { - return new PointDir(getX(), getY(), getHeading()); - } + @Override + public PointDir clone() { + return new PointDir(getX(), getY(), getHeading()); + } } diff --git a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java index 0cd78ebf7..1e9da0972 100644 --- a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java +++ b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java @@ -3,29 +3,28 @@ import com.team766.framework.AutonomousMode; import com.team766.robot.gatorade.procedures.*; -import edu.wpi.first.wpilibj.DriverStation; - -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.framework.Procedure; - public class AutonomousModes { - public static final AutonomousMode[] AUTONOMOUS_MODES = new AutonomousMode[] { - // Add autonomous modes here like this: - // new AutonomousMode("NameOfAutonomousMode", () -> new MyAutonomousProcedure()), - // - // If your autonomous procedure has constructor arguments, you can - // define one or more different autonomous modes with it like this: - // new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)), - // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), - // new AutonomousMode("FollowPoints", () -> new FollowPoints()), - // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), - // new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), - new AutonomousMode("OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), - // AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), - // new AutonomousMode("FollowPointsFile", () -> new FollowPoints("FollowPoints.json")), - // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new PointDir(2, 1), new PointDir(0, 1)})), - // new AutonomousMode("DoNothing", () -> new DoNothing()), - }; -} \ No newline at end of file + public static final AutonomousMode[] AUTONOMOUS_MODES = + new AutonomousMode[] { + // Add autonomous modes here like this: + // new AutonomousMode("NameOfAutonomousMode", () -> new MyAutonomousProcedure()), + // + // If your autonomous procedure has constructor arguments, you can + // define one or more different autonomous modes with it like this: + // new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)), + // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), + // new AutonomousMode("FollowPoints", () -> new FollowPoints()), + // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), + // new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), + new AutonomousMode( + "OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), + // AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), + // new AutonomousMode("FollowPointsFile", () -> new + // FollowPoints("FollowPoints.json")), + // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new + // PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new + // PointDir(2, 1), new PointDir(0, 1)})), + // new AutonomousMode("DoNothing", () -> new DoNothing()), + }; +} diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index debd435f9..68e775a7a 100644 --- a/src/main/java/com/team766/robot/gatorade/OI.java +++ b/src/main/java/com/team766/robot/gatorade/OI.java @@ -1,19 +1,15 @@ package com.team766.robot.gatorade; -import com.team766.framework.Procedure; import com.team766.framework.Context; +import com.team766.framework.Procedure; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; import com.team766.library.RateLimiter; import com.team766.logging.Category; import com.team766.logging.Severity; import com.team766.robot.gatorade.constants.InputConstants; -import com.team766.robot.gatorade.procedures.*; -import com.team766.simulator.interfaces.ElectricalDevice.Input; -import com.team766.robot.gatorade.mechanisms.Drive; -import com.team766.robot.gatorade.mechanisms.Intake; import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; -import edu.wpi.first.cameraserver.CameraServer; +import com.team766.robot.gatorade.procedures.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -23,284 +19,304 @@ */ public class OI extends Procedure { - private JoystickReader leftJoystick; - private JoystickReader rightJoystick; - private JoystickReader boxopGamepad; - private double rightJoystickX = 0; - private double leftJoystickX = 0; - private double leftJoystickY = 0; - private boolean isCross = false; - - private static final double FINE_DRIVING_COEFFICIENT = 0.25; - double turningValue = 0; - boolean manualControl = true; - PlacementPosition placementPosition = PlacementPosition.NONE; - - private RateLimiter lightsRateLimit = new RateLimiter(1.3); - - public OI() { - loggerCategory = Category.OPERATOR_INTERFACE; - - leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); - rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); - boxopGamepad = RobotProvider.instance.getJoystick(InputConstants.BOXOP_GAMEPAD); - } - - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.lights); - - boolean elevatorManual = false; - boolean wristManual = false; - - while (true) { - context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); - RobotProvider.instance.refreshDriverStationData(); - - SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); - - // Add driver controls here - make sure to take/release ownership - // of mechanisms when appropriate. - - leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); - leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); - rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); - //Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - - if (leftJoystick.getButtonPressed(InputConstants.INTAKE_OUT)) { - new IntakeOut().run(context); - } else if (leftJoystick.getButtonReleased(InputConstants.INTAKE_OUT)) { - new IntakeStop().run(context); - } - - if (leftJoystick.getButtonPressed(InputConstants.RESET_GYRO)) { - Robot.gyro.resetGyro(); - } - - if (leftJoystick.getButtonPressed(InputConstants.RESET_POS)) { - Robot.drive.resetCurrentPosition(); - } - - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { - rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT) / 2; - } else { - rightJoystickX = 0; - } - - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { - leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); - } else { - leftJoystickY = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { - leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); - } else { - leftJoystickX = 0; - } - - // Sets the wheels to the cross position if the cross button is pressed - if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { - if (!isCross) { - context.startAsync(new SetCross()); - } - isCross = !isCross; - } - - // Moves the robot if there are joystick inputs - if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) { - context.takeOwnership(Robot.drive); - // If a button is pressed, drive is just fine adjustment - if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { - Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); - } else { - // On deafault, controls the robot field oriented - Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX), (leftJoystickY), (rightJoystickX)); - } - } else { - Robot.drive.stopDrive(); - } - - // Respond to boxop commands - - // first, check if the boxop is making a cone or cube selection - if (boxopGamepad.getPOV() == InputConstants.POV_UP) { - new GoForCones().run(context); - setLightsForGamePiece(); - } else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) { - new GoForCubes().run(context); - setLightsForGamePiece(); - } - - // look for button presses to queue placement of intake/wrist/elevator superstructure - if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { - placementPosition = PlacementPosition.LOW_NODE; - setLightsForPlacement(); - } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) { - placementPosition = PlacementPosition.MID_NODE; - setLightsForPlacement(); - } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) { - placementPosition = PlacementPosition.HIGH_NODE; - setLightsForPlacement(); - } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) { - placementPosition = PlacementPosition.HUMAN_PLAYER; - setLightsForPlacement(); - } - - // look for button hold to start intake, release to idle intake - if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_INTAKE_IN)) { - new IntakeIn().run(context); - } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) { - new IntakeIdle().run(context); - } - - // see if we should reset the button states - if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) { - // stop the intake - new IntakeStop().run(context); - // reset the placement position - placementPosition = PlacementPosition.NONE; - // reset the cone/cube selection to cones - new GoForCones().run(context); - } - - // look for button hold to extend intake/wrist/elevator superstructure, - // release to retract - if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - switch (placementPosition) { - case NONE: - break; - case LOW_NODE: - context.startAsync(new ExtendWristvatorToLow()); - break; - case MID_NODE: - context.startAsync(new ExtendWristvatorToMid()); - break; - case HIGH_NODE: - context.startAsync(new ExtendWristvatorToHigh()); - break; - case HUMAN_PLAYER: - context.startAsync(new ExtendWristvatorToHuman(Robot.intake.getGamePieceType())); - break; - default: - // warn, ignore - log(Severity.WARNING, "Unexpected placement position: " + placementPosition.toString()); - break; - } - } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - context.startAsync(new RetractWristvator()); - } - - // TODO: refactor this code. it's getting gnarly. - // look for manual nudges - // we only allow these if the extend elevator trigger is extended - if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - // the y axis is flipped from what we expect. invert so up is positive, down is negative. - double elevatorNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT); - double wristNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); - - if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { - // bypass PID - if (Math.abs(elevatorNudgeAxis) > 0.05) { - elevatorManual = true; - context.takeOwnership(Robot.elevator); - Robot.elevator.nudgeNoPID(elevatorNudgeAxis); - context.releaseOwnership(Robot.elevator); - } else if (elevatorManual) { - context.takeOwnership(Robot.elevator); - Robot.elevator.stopElevator(); - context.releaseOwnership(Robot.elevator); - elevatorManual = false; - } - - if ((Math.abs(wristNudgeAxis) > 0.05)) { - wristManual = true; - context.takeOwnership(Robot.wrist); - Robot.wrist.nudgeNoPID(wristNudgeAxis); - context.releaseOwnership(Robot.wrist); - } else if (wristManual) { - context.takeOwnership(Robot.wrist); - Robot.wrist.stopWrist(); - context.releaseOwnership(Robot.wrist); - elevatorManual = false; - } - } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { - context.takeOwnership(Robot.wrist); - context.takeOwnership(Robot.elevator); - Robot.wrist.resetEncoder(); - Robot.elevator.resetEncoder(); - context.releaseOwnership(Robot.wrist); - context.releaseOwnership(Robot.elevator); - } else { - // look for elevator nudges - if (Math.abs(elevatorNudgeAxis) > 0.05) { - context.takeOwnership(Robot.elevator); - if (elevatorNudgeAxis > 0) { - Robot.elevator.nudgeUp(); - } else { - Robot.elevator.nudgeDown(); - } - context.releaseOwnership(Robot.elevator); - } - - // look for wrist nudges - if (Math.abs(wristNudgeAxis) > 0.05) { - context.takeOwnership(Robot.wrist); - if (wristNudgeAxis > 0) { - Robot.wrist.nudgeUp(); - } else { - Robot.wrist.nudgeDown(); - } - context.releaseOwnership(Robot.wrist); - } - } - } - - if (lightsRateLimit.next()) { - if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) { - Robot.lights.rainbow(); - } else { - setLightsForPlacement(); - } - } - } - } - - private void setLightsForPlacement() { - switch (placementPosition) { - case NONE: - Robot.lights.white(); - break; - case LOW_NODE: - Robot.lights.green(); - break; - case MID_NODE: - Robot.lights.red(); - break; - case HIGH_NODE: - Robot.lights.orange(); - break; - case HUMAN_PLAYER: - setLightsForGamePiece(); - break; - default: - // warn, ignore - log(Severity.WARNING, "Unexpected placement position: " + placementPosition.toString()); - break; - } - - lightsRateLimit.reset(); - lightsRateLimit.next(); - } - - private void setLightsForGamePiece() { - if (Robot.intake.getGamePieceType() == GamePieceType.CUBE) { - Robot.lights.purple(); - } else { - Robot.lights.yellow(); - } - - lightsRateLimit.reset(); - lightsRateLimit.next(); - } -} \ No newline at end of file + private JoystickReader leftJoystick; + private JoystickReader rightJoystick; + private JoystickReader boxopGamepad; + private double rightJoystickX = 0; + private double leftJoystickX = 0; + private double leftJoystickY = 0; + private boolean isCross = false; + + private static final double FINE_DRIVING_COEFFICIENT = 0.25; + double turningValue = 0; + boolean manualControl = true; + PlacementPosition placementPosition = PlacementPosition.NONE; + + private RateLimiter lightsRateLimit = new RateLimiter(1.3); + + public OI() { + loggerCategory = Category.OPERATOR_INTERFACE; + + leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); + rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); + boxopGamepad = RobotProvider.instance.getJoystick(InputConstants.BOXOP_GAMEPAD); + } + + public void run(Context context) { + context.takeOwnership(Robot.drive); + context.takeOwnership(Robot.gyro); + context.takeOwnership(Robot.lights); + + boolean elevatorManual = false; + boolean wristManual = false; + + while (true) { + context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); + RobotProvider.instance.refreshDriverStationData(); + + SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); + + // Add driver controls here - make sure to take/release ownership + // of mechanisms when appropriate. + + leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + // Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); + + if (leftJoystick.getButtonPressed(InputConstants.INTAKE_OUT)) { + new IntakeOut().run(context); + } else if (leftJoystick.getButtonReleased(InputConstants.INTAKE_OUT)) { + new IntakeStop().run(context); + } + + if (leftJoystick.getButtonPressed(InputConstants.RESET_GYRO)) { + Robot.gyro.resetGyro(); + } + + if (leftJoystick.getButtonPressed(InputConstants.RESET_POS)) { + Robot.drive.resetCurrentPosition(); + } + + if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT) / 2; + } else { + rightJoystickX = 0; + } + + if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { + leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + } else { + leftJoystickY = 0; + } + if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + } else { + leftJoystickX = 0; + } + + // Sets the wheels to the cross position if the cross button is pressed + if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { + if (!isCross) { + context.startAsync(new SetCross()); + } + isCross = !isCross; + } + + // Moves the robot if there are joystick inputs + if (!isCross + && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) + > 0) { + context.takeOwnership(Robot.drive); + // If a button is pressed, drive is just fine adjustment + if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { + Robot.drive.controlFieldOriented( + Math.toRadians(Robot.gyro.getGyroYaw()), + (leftJoystickX * FINE_DRIVING_COEFFICIENT), + (leftJoystickY * FINE_DRIVING_COEFFICIENT), + (rightJoystickX * FINE_DRIVING_COEFFICIENT)); + } else { + // On deafault, controls the robot field oriented + Robot.drive.controlFieldOriented( + Math.toRadians(Robot.gyro.getGyroYaw()), + (leftJoystickX), + (leftJoystickY), + (rightJoystickX)); + } + } else { + Robot.drive.stopDrive(); + } + + // Respond to boxop commands + + // first, check if the boxop is making a cone or cube selection + if (boxopGamepad.getPOV() == InputConstants.POV_UP) { + new GoForCones().run(context); + setLightsForGamePiece(); + } else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) { + new GoForCubes().run(context); + setLightsForGamePiece(); + } + + // look for button presses to queue placement of intake/wrist/elevator superstructure + if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { + placementPosition = PlacementPosition.LOW_NODE; + setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) { + placementPosition = PlacementPosition.MID_NODE; + setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) { + placementPosition = PlacementPosition.HIGH_NODE; + setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) { + placementPosition = PlacementPosition.HUMAN_PLAYER; + setLightsForPlacement(); + } + + // look for button hold to start intake, release to idle intake + if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_INTAKE_IN)) { + new IntakeIn().run(context); + } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) { + new IntakeIdle().run(context); + } + + // see if we should reset the button states + if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) { + // stop the intake + new IntakeStop().run(context); + // reset the placement position + placementPosition = PlacementPosition.NONE; + // reset the cone/cube selection to cones + new GoForCones().run(context); + } + + // look for button hold to extend intake/wrist/elevator superstructure, + // release to retract + if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { + switch (placementPosition) { + case NONE: + break; + case LOW_NODE: + context.startAsync(new ExtendWristvatorToLow()); + break; + case MID_NODE: + context.startAsync(new ExtendWristvatorToMid()); + break; + case HIGH_NODE: + context.startAsync(new ExtendWristvatorToHigh()); + break; + case HUMAN_PLAYER: + context.startAsync( + new ExtendWristvatorToHuman(Robot.intake.getGamePieceType())); + break; + default: + // warn, ignore + log( + Severity.WARNING, + "Unexpected placement position: " + placementPosition.toString()); + break; + } + } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { + context.startAsync(new RetractWristvator()); + } + + // TODO: refactor this code. it's getting gnarly. + // look for manual nudges + // we only allow these if the extend elevator trigger is extended + if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { + // the y axis is flipped from what we expect. invert so up is positive, down is + // negative. + double elevatorNudgeAxis = + -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT); + double wristNudgeAxis = + -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); + + if (boxopGamepad.getButtonPressed( + InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { + // bypass PID + if (Math.abs(elevatorNudgeAxis) > 0.05) { + elevatorManual = true; + context.takeOwnership(Robot.elevator); + Robot.elevator.nudgeNoPID(elevatorNudgeAxis); + context.releaseOwnership(Robot.elevator); + } else if (elevatorManual) { + context.takeOwnership(Robot.elevator); + Robot.elevator.stopElevator(); + context.releaseOwnership(Robot.elevator); + elevatorManual = false; + } + + if ((Math.abs(wristNudgeAxis) > 0.05)) { + wristManual = true; + context.takeOwnership(Robot.wrist); + Robot.wrist.nudgeNoPID(wristNudgeAxis); + context.releaseOwnership(Robot.wrist); + } else if (wristManual) { + context.takeOwnership(Robot.wrist); + Robot.wrist.stopWrist(); + context.releaseOwnership(Robot.wrist); + elevatorManual = false; + } + } else if (boxopGamepad.getButtonReleased( + InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { + context.takeOwnership(Robot.wrist); + context.takeOwnership(Robot.elevator); + Robot.wrist.resetEncoder(); + Robot.elevator.resetEncoder(); + context.releaseOwnership(Robot.wrist); + context.releaseOwnership(Robot.elevator); + } else { + // look for elevator nudges + if (Math.abs(elevatorNudgeAxis) > 0.05) { + context.takeOwnership(Robot.elevator); + if (elevatorNudgeAxis > 0) { + Robot.elevator.nudgeUp(); + } else { + Robot.elevator.nudgeDown(); + } + context.releaseOwnership(Robot.elevator); + } + + // look for wrist nudges + if (Math.abs(wristNudgeAxis) > 0.05) { + context.takeOwnership(Robot.wrist); + if (wristNudgeAxis > 0) { + Robot.wrist.nudgeUp(); + } else { + Robot.wrist.nudgeDown(); + } + context.releaseOwnership(Robot.wrist); + } + } + } + + if (lightsRateLimit.next()) { + if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) { + Robot.lights.rainbow(); + } else { + setLightsForPlacement(); + } + } + } + } + + private void setLightsForPlacement() { + switch (placementPosition) { + case NONE: + Robot.lights.white(); + break; + case LOW_NODE: + Robot.lights.green(); + break; + case MID_NODE: + Robot.lights.red(); + break; + case HIGH_NODE: + Robot.lights.orange(); + break; + case HUMAN_PLAYER: + setLightsForGamePiece(); + break; + default: + // warn, ignore + log( + Severity.WARNING, + "Unexpected placement position: " + placementPosition.toString()); + break; + } + + lightsRateLimit.reset(); + lightsRateLimit.next(); + } + + private void setLightsForGamePiece() { + if (Robot.intake.getGamePieceType() == GamePieceType.CUBE) { + Robot.lights.purple(); + } else { + Robot.lights.yellow(); + } + + lightsRateLimit.reset(); + lightsRateLimit.next(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/PlacementPosition.java b/src/main/java/com/team766/robot/gatorade/PlacementPosition.java index 1a1118c60..6c6aa336f 100644 --- a/src/main/java/com/team766/robot/gatorade/PlacementPosition.java +++ b/src/main/java/com/team766/robot/gatorade/PlacementPosition.java @@ -1,9 +1,9 @@ package com.team766.robot.gatorade; public enum PlacementPosition { - NONE, - LOW_NODE, - MID_NODE, - HIGH_NODE, - HUMAN_PLAYER + NONE, + LOW_NODE, + MID_NODE, + HIGH_NODE, + HUMAN_PLAYER } diff --git a/src/main/java/com/team766/robot/gatorade/Robot.java b/src/main/java/com/team766/robot/gatorade/Robot.java index 891244ce9..e40383f33 100644 --- a/src/main/java/com/team766/robot/gatorade/Robot.java +++ b/src/main/java/com/team766/robot/gatorade/Robot.java @@ -3,21 +3,21 @@ import com.team766.robot.gatorade.mechanisms.*; public class Robot { - // Declare mechanisms here - public static Intake intake; - public static Wrist wrist; - public static Elevator elevator; - public static Drive drive; - public static Gyro gyro; - public static Lights lights; + // Declare mechanisms here + public static Intake intake; + public static Wrist wrist; + public static Elevator elevator; + public static Drive drive; + public static Gyro gyro; + public static Lights lights; - public static void robotInit() { - // Initialize mechanisms here - intake = new Intake(); - wrist = new Wrist(); - elevator = new Elevator(); - drive = new Drive(); - gyro = new Gyro(); - lights = new Lights(); - } + public static void robotInit() { + // Initialize mechanisms here + intake = new Intake(); + wrist = new Wrist(); + elevator = new Elevator(); + drive = new Drive(); + gyro = new Gyro(); + lights = new Lights(); + } } diff --git a/src/main/java/com/team766/robot/gatorade/RobotTargets.java b/src/main/java/com/team766/robot/gatorade/RobotTargets.java index c52270b3a..9e4673181 100644 --- a/src/main/java/com/team766/robot/gatorade/RobotTargets.java +++ b/src/main/java/com/team766/robot/gatorade/RobotTargets.java @@ -7,89 +7,85 @@ */ public final class RobotTargets { - private static final double BLUE_Y = 1.88595; - private static final double RED_Y = 14.6558; + private static final double BLUE_Y = 1.88595; + private static final double RED_Y = 14.6558; - private static final double ROW_1 = 0.508; - private static final double ROW_2 = 1.071626; - private static final double ROW_3 = 1.6256; - private static final double ROW_4 = 2.1844; - private static final double ROW_5 = 2.748026; - private static final double ROW_6 = 3.302; - private static final double ROW_7 = 3.8608; - private static final double ROW_8 = 4.424426; - private static final double ROW_9 = 4.9784; + private static final double ROW_1 = 0.508; + private static final double ROW_2 = 1.071626; + private static final double ROW_3 = 1.6256; + private static final double ROW_4 = 2.1844; + private static final double ROW_5 = 2.748026; + private static final double ROW_6 = 3.302; + private static final double ROW_7 = 3.8608; + private static final double ROW_8 = 4.424426; + private static final double ROW_9 = 4.9784; - //Points corresponding to nodes, in the format: NODES_COLOR_GRID_COLUMN - //This is from the perspective of the driver - public static final Point NODES_BLUE_RIGHT_RIGHT = new Point(BLUE_Y, ROW_1); - public static final Point NODES_BLUE_RIGHT_CENTER = new Point(BLUE_Y, ROW_2); - public static final Point NODES_BLUE_RIGHT_LEFT = new Point(BLUE_Y, ROW_3); - public static final Point NODES_BLUE_CENTER_RIGHT = new Point(BLUE_Y, ROW_4); - public static final Point NODES_BLUE_CENTER_CENTER = new Point(BLUE_Y, ROW_5); - public static final Point NODES_BLUE_CENTER_LEFT = new Point(BLUE_Y, ROW_6); - public static final Point NODES_BLUE_LEFT_RIGHT = new Point(BLUE_Y, ROW_7); - public static final Point NODES_BLUE_LEFT_CENTER = new Point(BLUE_Y, ROW_8); - public static final Point NODES_BLUE_LEFT_LEFT = new Point(BLUE_Y, ROW_9); + // Points corresponding to nodes, in the format: NODES_COLOR_GRID_COLUMN + // This is from the perspective of the driver + public static final Point NODES_BLUE_RIGHT_RIGHT = new Point(BLUE_Y, ROW_1); + public static final Point NODES_BLUE_RIGHT_CENTER = new Point(BLUE_Y, ROW_2); + public static final Point NODES_BLUE_RIGHT_LEFT = new Point(BLUE_Y, ROW_3); + public static final Point NODES_BLUE_CENTER_RIGHT = new Point(BLUE_Y, ROW_4); + public static final Point NODES_BLUE_CENTER_CENTER = new Point(BLUE_Y, ROW_5); + public static final Point NODES_BLUE_CENTER_LEFT = new Point(BLUE_Y, ROW_6); + public static final Point NODES_BLUE_LEFT_RIGHT = new Point(BLUE_Y, ROW_7); + public static final Point NODES_BLUE_LEFT_CENTER = new Point(BLUE_Y, ROW_8); + public static final Point NODES_BLUE_LEFT_LEFT = new Point(BLUE_Y, ROW_9); - public static final Point NODES_RED_LEFT_LEFT = new Point(RED_Y, ROW_1); - public static final Point NODES_RED_LEFT_CENTER = new Point(RED_Y, ROW_2); - public static final Point NODES_RED_LEFT_RIGHT = new Point(RED_Y, ROW_3); - public static final Point NODES_RED_CENTER_LEFT = new Point(RED_Y, ROW_4); - public static final Point NODES_RED_CENTER_CENTER = new Point(RED_Y, ROW_5); - public static final Point NODES_RED_CENTER_RIGHT = new Point(RED_Y, ROW_6); - public static final Point NODES_RED_RIGHT_LEFT = new Point(RED_Y, ROW_7); - public static final Point NODES_RED_RIGHT_CENTER = new Point(RED_Y, ROW_8); - public static final Point NODES_RED_RIGHT_RIGHT = new Point(RED_Y, ROW_9); + public static final Point NODES_RED_LEFT_LEFT = new Point(RED_Y, ROW_1); + public static final Point NODES_RED_LEFT_CENTER = new Point(RED_Y, ROW_2); + public static final Point NODES_RED_LEFT_RIGHT = new Point(RED_Y, ROW_3); + public static final Point NODES_RED_CENTER_LEFT = new Point(RED_Y, ROW_4); + public static final Point NODES_RED_CENTER_CENTER = new Point(RED_Y, ROW_5); + public static final Point NODES_RED_CENTER_RIGHT = new Point(RED_Y, ROW_6); + public static final Point NODES_RED_RIGHT_LEFT = new Point(RED_Y, ROW_7); + public static final Point NODES_RED_RIGHT_CENTER = new Point(RED_Y, ROW_8); + public static final Point NODES_RED_RIGHT_RIGHT = new Point(RED_Y, ROW_9); - public static final Point[] NODES = { - NODES_BLUE_RIGHT_RIGHT, - NODES_BLUE_RIGHT_CENTER, - NODES_BLUE_RIGHT_LEFT, - NODES_BLUE_CENTER_RIGHT, - NODES_BLUE_CENTER_CENTER, - NODES_BLUE_CENTER_LEFT, - NODES_BLUE_LEFT_RIGHT, - NODES_BLUE_LEFT_CENTER, - NODES_BLUE_LEFT_LEFT, - NODES_RED_RIGHT_RIGHT, - NODES_RED_RIGHT_CENTER, - NODES_RED_RIGHT_LEFT, - NODES_RED_CENTER_RIGHT, - NODES_RED_CENTER_CENTER, - NODES_RED_CENTER_LEFT, - NODES_RED_LEFT_RIGHT, - NODES_RED_LEFT_CENTER, - NODES_RED_LEFT_LEFT - }; + public static final Point[] NODES = { + NODES_BLUE_RIGHT_RIGHT, + NODES_BLUE_RIGHT_CENTER, + NODES_BLUE_RIGHT_LEFT, + NODES_BLUE_CENTER_RIGHT, + NODES_BLUE_CENTER_CENTER, + NODES_BLUE_CENTER_LEFT, + NODES_BLUE_LEFT_RIGHT, + NODES_BLUE_LEFT_CENTER, + NODES_BLUE_LEFT_LEFT, + NODES_RED_RIGHT_RIGHT, + NODES_RED_RIGHT_CENTER, + NODES_RED_RIGHT_LEFT, + NODES_RED_CENTER_RIGHT, + NODES_RED_CENTER_CENTER, + NODES_RED_CENTER_LEFT, + NODES_RED_LEFT_RIGHT, + NODES_RED_LEFT_CENTER, + NODES_RED_LEFT_LEFT + }; - public static final Point[] CONE_NODES = { - NODES_BLUE_RIGHT_RIGHT, - NODES_BLUE_RIGHT_LEFT, - NODES_BLUE_CENTER_RIGHT, - NODES_BLUE_CENTER_LEFT, - NODES_BLUE_LEFT_RIGHT, - NODES_BLUE_LEFT_LEFT, - NODES_RED_RIGHT_RIGHT, - NODES_RED_RIGHT_LEFT, - NODES_RED_CENTER_RIGHT, - NODES_RED_CENTER_LEFT, - NODES_RED_LEFT_RIGHT, - NODES_RED_LEFT_LEFT - }; + public static final Point[] CONE_NODES = { + NODES_BLUE_RIGHT_RIGHT, + NODES_BLUE_RIGHT_LEFT, + NODES_BLUE_CENTER_RIGHT, + NODES_BLUE_CENTER_LEFT, + NODES_BLUE_LEFT_RIGHT, + NODES_BLUE_LEFT_LEFT, + NODES_RED_RIGHT_RIGHT, + NODES_RED_RIGHT_LEFT, + NODES_RED_CENTER_RIGHT, + NODES_RED_CENTER_LEFT, + NODES_RED_LEFT_RIGHT, + NODES_RED_LEFT_LEFT + }; - public static final Point[] CUBE_NODES = { - NODES_BLUE_RIGHT_CENTER, - NODES_BLUE_CENTER_CENTER, - NODES_BLUE_LEFT_CENTER, - NODES_RED_RIGHT_CENTER, - NODES_RED_CENTER_CENTER, - NODES_RED_LEFT_CENTER, - }; + public static final Point[] CUBE_NODES = { + NODES_BLUE_RIGHT_CENTER, + NODES_BLUE_CENTER_CENTER, + NODES_BLUE_LEFT_CENTER, + NODES_RED_RIGHT_CENTER, + NODES_RED_CENTER_CENTER, + NODES_RED_LEFT_CENTER, + }; - public static final double[] CUBE_ROWS = { - ROW_2, - ROW_5, - ROW_8 - }; -} \ No newline at end of file + public static final double[] CUBE_ROWS = {ROW_2, ROW_5, ROW_8}; +} diff --git a/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java b/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java index 12023da4f..1ca99a389 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/ChargeConstants.java @@ -2,28 +2,28 @@ /** * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. - * + * * TODO: consider moving this into a config file. */ public final class ChargeConstants { - public static final double BLUE_BALANCE_TARGET_X = 3.9417625; - public static final double BLUE_BALANCE_LEFT_EDGE = 2.974975; - public static final double BLUE_BALANCE_RIGHT_EDGE = 4.90855; + public static final double BLUE_BALANCE_TARGET_X = 3.9417625; + public static final double BLUE_BALANCE_LEFT_EDGE = 2.974975; + public static final double BLUE_BALANCE_RIGHT_EDGE = 4.90855; - public static final double RED_BALANCE_TARGET_X = 12.5999875; - public static final double RED_BALANCE_LEFT_EDGE = 11.6332; - public static final double RED_BALANCE_RIGHT_EDGE = 13.566775; + public static final double RED_BALANCE_TARGET_X = 12.5999875; + public static final double RED_BALANCE_LEFT_EDGE = 11.6332; + public static final double RED_BALANCE_RIGHT_EDGE = 13.566775; - public static final double X_ALIGNMENT_THRESHOLD = 0.5; - public static final double Y_ALIGNMENT_THRESHOLD = 0.5; + public static final double X_ALIGNMENT_THRESHOLD = 0.5; + public static final double Y_ALIGNMENT_THRESHOLD = 0.5; - public static final double BLUE_LEFT_PT = BLUE_BALANCE_LEFT_EDGE - X_ALIGNMENT_THRESHOLD; - public static final double BLUE_RIGHT_PT = BLUE_BALANCE_RIGHT_EDGE + X_ALIGNMENT_THRESHOLD; - public static final double RED_LEFT_PT = RED_BALANCE_LEFT_EDGE - X_ALIGNMENT_THRESHOLD; - public static final double RED_RIGHT_PT = RED_BALANCE_RIGHT_EDGE + X_ALIGNMENT_THRESHOLD; + public static final double BLUE_LEFT_PT = BLUE_BALANCE_LEFT_EDGE - X_ALIGNMENT_THRESHOLD; + public static final double BLUE_RIGHT_PT = BLUE_BALANCE_RIGHT_EDGE + X_ALIGNMENT_THRESHOLD; + public static final double RED_LEFT_PT = RED_BALANCE_LEFT_EDGE - X_ALIGNMENT_THRESHOLD; + public static final double RED_RIGHT_PT = RED_BALANCE_RIGHT_EDGE + X_ALIGNMENT_THRESHOLD; - public static final double CHARGE_TOP_EDGE = 3.96875; - public static final double CHARGE_BOTTOM_EDGE = 1.4986; - public static final double MIDDLE = 2.733675; -} \ No newline at end of file + public static final double CHARGE_TOP_EDGE = 3.96875; + public static final double CHARGE_BOTTOM_EDGE = 1.4986; + public static final double MIDDLE = 2.733675; +} diff --git a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java index 85ad22bb2..8d20e4935 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java @@ -2,40 +2,38 @@ /** Constants used for reading values from the config file. */ public final class ConfigConstants { - // utility class - private ConfigConstants() {} - - // drive config values - public static final String DRIVE_DRIVE_FRONT_RIGHT = "drive.DriveFrontRight"; - public static final String DRIVE_DRIVE_FRONT_LEFT = "drive.DriveFrontLeft"; - public static final String DRIVE_DRIVE_BACK_RIGHT = "drive.DriveBackRight"; - public static final String DRIVE_DRIVE_BACK_LEFT = "drive.DriveBackLeft"; - - public static final String DRIVE_STEER_FRONT_RIGHT = "drive.SteerFrontRight"; - public static final String DRIVE_STEER_FRONT_LEFT = "drive.SteerFrontLeft"; - public static final String DRIVE_STEER_BACK_RIGHT = "drive.SteerBackRight"; - public static final String DRIVE_STEER_BACK_LEFT = "drive.SteerBackLeft"; - - // intake config values - public static final String INTAKE_MOTOR = "intake.motor"; - - // wrist config values - public static final String WRIST_MOTOR = "wrist.motor"; - public static final String WRIST_PGAIN = "wrist.sparkPID.pGain"; - public static final String WRIST_IGAIN = "wrist.sparkPID.iGain"; - public static final String WRIST_DGAIN = "wrist.sparkPID.dGain"; - public static final String WRIST_FFGAIN = "wrist.sparkPID.ffGain"; - - // elevator config values - public static final String ELEVATOR_LEFT_MOTOR = "elevator.leftMotor"; - public static final String ELEVATOR_RIGHT_MOTOR = "elevator.rightMotor"; - public static final String ELEVATOR_PGAIN = "elevator.sparkPID.pGain"; - public static final String ELEVATOR_IGAIN = "elevator.sparkPID.iGain"; - public static final String ELEVATOR_DGAIN = "elevator.sparkPID.dGain"; - public static final String ELEVATOR_FFGAIN = "elevator.sparkPID.ffGain"; - public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity"; - public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity"; - public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel"; + // utility class + private ConfigConstants() {} + + // drive config values + public static final String DRIVE_DRIVE_FRONT_RIGHT = "drive.DriveFrontRight"; + public static final String DRIVE_DRIVE_FRONT_LEFT = "drive.DriveFrontLeft"; + public static final String DRIVE_DRIVE_BACK_RIGHT = "drive.DriveBackRight"; + public static final String DRIVE_DRIVE_BACK_LEFT = "drive.DriveBackLeft"; + + public static final String DRIVE_STEER_FRONT_RIGHT = "drive.SteerFrontRight"; + public static final String DRIVE_STEER_FRONT_LEFT = "drive.SteerFrontLeft"; + public static final String DRIVE_STEER_BACK_RIGHT = "drive.SteerBackRight"; + public static final String DRIVE_STEER_BACK_LEFT = "drive.SteerBackLeft"; + + // intake config values + public static final String INTAKE_MOTOR = "intake.motor"; + + // wrist config values + public static final String WRIST_MOTOR = "wrist.motor"; + public static final String WRIST_PGAIN = "wrist.sparkPID.pGain"; + public static final String WRIST_IGAIN = "wrist.sparkPID.iGain"; + public static final String WRIST_DGAIN = "wrist.sparkPID.dGain"; + public static final String WRIST_FFGAIN = "wrist.sparkPID.ffGain"; + + // elevator config values + public static final String ELEVATOR_LEFT_MOTOR = "elevator.leftMotor"; + public static final String ELEVATOR_RIGHT_MOTOR = "elevator.rightMotor"; + public static final String ELEVATOR_PGAIN = "elevator.sparkPID.pGain"; + public static final String ELEVATOR_IGAIN = "elevator.sparkPID.iGain"; + public static final String ELEVATOR_DGAIN = "elevator.sparkPID.dGain"; + public static final String ELEVATOR_FFGAIN = "elevator.sparkPID.ffGain"; + public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity"; + public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity"; + public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel"; } - - diff --git a/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java index 35af105a9..85e7a1c72 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/FollowPointsInputConstants.java @@ -1,12 +1,10 @@ package com.team766.robot.gatorade.constants; public final class FollowPointsInputConstants { - - public static double RADIUS = 0.5; - public static double SPEED = 0.25; - public static double RATE_LIMITER_TIME = 0.01; - public static double MAX_ROTATION_SPEED = 0.4; - public static double ANGLE_DISTANCE_FOR_MAX_SPEED = 90; - + public static double RADIUS = 0.5; + public static double SPEED = 0.25; + public static double RATE_LIMITER_TIME = 0.01; + public static double MAX_ROTATION_SPEED = 0.4; + public static double ANGLE_DISTANCE_FOR_MAX_SPEED = 90; } diff --git a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java index f333af27b..c5572ecbe 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java @@ -5,46 +5,46 @@ */ public final class InputConstants { - // Joysticks - public static final int LEFT_JOYSTICK = 0; - public static final int RIGHT_JOYSTICK = 1; - public static final int BOXOP_GAMEPAD = 2; // should be in Logitech Mode - - // Navigation - public static final int AXIS_LEFT_RIGHT = 0; - public static final int AXIS_FORWARD_BACKWARD = 1; - public static final int AXIS_TWIST = 3; - - // Joystick buttons - public static final int INTAKE_OUT = 1; - public static final int FINE_DRIVING = 1; - public static final int CROSS_WHEELS = 3; - public static final int RESET_GYRO = 9; - public static final int RESET_POS = 15; - - // Boxop Gamepad Buttons - - // LT - public static final int BUTTON_INTAKE_IN = 7; - // RT - public static final int BUTTON_EXTEND_WRISTVATOR = 8; - // Start - public static final int BUTTON_RESET_STATE = 10; - - // left axis - public static final int AXIS_WRIST_MOVEMENT = 1; - // right axis - public static final int AXIS_ELEVATOR_MOVEMENT = 3; - - // pov - public static final int POV_UP = 0; - public static final int POV_DOWN = 180; - - // LB - public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5; - // X/A/B/Y - public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X - public static final int BUTTON_PLACEMENT_HIGH = 4; // Y - public static final int BUTTON_PLACEMENT_MID = 3; // B - public static final int BUTTON_PLACEMENT_LOW = 2; // A -} \ No newline at end of file + // Joysticks + public static final int LEFT_JOYSTICK = 0; + public static final int RIGHT_JOYSTICK = 1; + public static final int BOXOP_GAMEPAD = 2; // should be in Logitech Mode + + // Navigation + public static final int AXIS_LEFT_RIGHT = 0; + public static final int AXIS_FORWARD_BACKWARD = 1; + public static final int AXIS_TWIST = 3; + + // Joystick buttons + public static final int INTAKE_OUT = 1; + public static final int FINE_DRIVING = 1; + public static final int CROSS_WHEELS = 3; + public static final int RESET_GYRO = 9; + public static final int RESET_POS = 15; + + // Boxop Gamepad Buttons + + // LT + public static final int BUTTON_INTAKE_IN = 7; + // RT + public static final int BUTTON_EXTEND_WRISTVATOR = 8; + // Start + public static final int BUTTON_RESET_STATE = 10; + + // left axis + public static final int AXIS_WRIST_MOVEMENT = 1; + // right axis + public static final int AXIS_ELEVATOR_MOVEMENT = 3; + + // pov + public static final int POV_UP = 0; + public static final int POV_DOWN = 180; + + // LB + public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5; + // X/A/B/Y + public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X + public static final int BUTTON_PLACEMENT_HIGH = 4; // Y + public static final int BUTTON_PLACEMENT_MID = 3; // B + public static final int BUTTON_PLACEMENT_LOW = 2; // A +} diff --git a/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java index 75c0c12bf..86c9b78f0 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/OdometryInputConstants.java @@ -1,22 +1,23 @@ package com.team766.robot.gatorade.constants; -import com.team766.hal.MotorController; - /** * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. - * + * */ public final class OdometryInputConstants { - //Circumference of the wheels. It was measured to be 30.5cm, then experimentally this value had an error of 2.888%. This was then converted to meters. - public static final double WHEEL_CIRCUMFERENCE = 30.5 * 1.02888 / 100; - //Unique to the type of swerve module we have. This is the factor converting motor revolutions to wheel revolutions. - public static final double GEAR_RATIO = 6.75; - //Unique to the type of swerve module we have. For every revolution of the wheel, the encoders will increase by 2048. - public static final int ENCODER_TO_REVOLUTION_CONSTANT = 2048; - //The distance between the centers of the wheels on each side of the robot. This was measured as 20.5 inches, then converted to meters. - public static final double DISTANCE_BETWEEN_WHEELS = 20.5 * 2.54 / 100; - //How often odometry updates, in seconds. - public static final double RATE_LIMITER_TIME = 0.05; - + // Circumference of the wheels. It was measured to be 30.5cm, then experimentally this value had + // an error of 2.888%. This was then converted to meters. + public static final double WHEEL_CIRCUMFERENCE = 30.5 * 1.02888 / 100; + // Unique to the type of swerve module we have. This is the factor converting motor revolutions + // to wheel revolutions. + public static final double GEAR_RATIO = 6.75; + // Unique to the type of swerve module we have. For every revolution of the wheel, the encoders + // will increase by 2048. + public static final int ENCODER_TO_REVOLUTION_CONSTANT = 2048; + // The distance between the centers of the wheels on each side of the robot. This was measured + // as 20.5 inches, then converted to meters. + public static final double DISTANCE_BETWEEN_WHEELS = 20.5 * 2.54 / 100; + // How often odometry updates, in seconds. + public static final double RATE_LIMITER_TIME = 0.05; } diff --git a/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java b/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java index 31d09b766..de615d209 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/SwerveDriveConstants.java @@ -2,20 +2,19 @@ public final class SwerveDriveConstants { - // defines where the wheels are in relation to the center of the robot - // allows swerve drive code to calculate how to turn - public static final double FL_X = -1; - public static final double FL_Y = 1; - public static final double FR_X = 1; - public static final double FR_Y = 1; - public static final double BL_X = -1; - public static final double BL_Y = -1; - public static final double BR_X = 1; - public static final double BR_Y = -1; + // defines where the wheels are in relation to the center of the robot + // allows swerve drive code to calculate how to turn + public static final double FL_X = -1; + public static final double FL_Y = 1; + public static final double FR_X = 1; + public static final double FR_Y = 1; + public static final double BL_X = -1; + public static final double BL_Y = -1; + public static final double BR_X = 1; + public static final double BR_Y = -1; - public static final String SWERVE_CANBUS = "Swervavore"; - - public static final double DRIVE_MOTOR_CURRENT_LIMIT = 35; - public static final double STEER_MOTOR_CURRENT_LIMIT = 30; + public static final String SWERVE_CANBUS = "Swervavore"; + public static final double DRIVE_MOTOR_CURRENT_LIMIT = 35; + public static final double STEER_MOTOR_CURRENT_LIMIT = 30; } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java index 4735ab66d..d8c14b4ac 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java @@ -1,238 +1,324 @@ package com.team766.robot.gatorade.mechanisms; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + import com.ctre.phoenix.sensors.CANCoder; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; import com.team766.hal.MotorController.ControlMode; +import com.team766.hal.RobotProvider; import com.team766.logging.Category; -import static com.team766.robot.gatorade.constants.ConfigConstants.*; -import com.team766.robot.gatorade.constants.SwerveDriveConstants; -import com.team766.robot.gatorade.constants.OdometryInputConstants; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; import com.team766.odometry.Odometry; import com.team766.odometry.Point; import com.team766.odometry.PointDir; +import com.team766.robot.gatorade.constants.OdometryInputConstants; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; public class Drive extends Mechanism { - // Drive motors - private MotorController m_DriveFR; - private MotorController m_DriveFL; - private MotorController m_DriveBR; - private MotorController m_DriveBL; - - // Motors to turn each wheel in place - private MotorController m_SteerFR; - private MotorController m_SteerFL; - private MotorController m_SteerBR; - private MotorController m_SteerBL; - - // Absolute encoders (cancoders) - private CANCoder e_FrontRight; - private CANCoder e_FrontLeft; - private CANCoder e_BackRight; - private CANCoder e_BackLeft; - - /* - * Specific offsets to align each wheel - * These are assigned upon startup with setEncoderOffset() - */ - private double offsetFR; - private double offsetFL; - private double offsetBR; - private double offsetBL; - - /* - * Factor that converts between motor units and degrees - * Multiply to convert from degrees to motor units - * Divide to convert from motor units to degrees - */ - private final double ENCODER_CONVERSION_FACTOR = (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; - - // TODO: rework odometry so it doesn't have to go through drive - - // declaration of odometry object - private Odometry swerveOdometry; - // variable representing current position - private static PointDir currentPosition; - - // other variables to set up odometry - private MotorController[] motorList; - private CANCoder[] CANCoderList; - private Point[] wheelPositions; - - public Drive() { - - loggerCategory = Category.DRIVE; - // Initializations of motors - - // Initialize the drive motors - m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); - m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); - m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); - m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); - - // Initialize the steering motors - m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); - m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); - m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); - m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); - - // Initialize the encoders - e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); - e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); - e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); - e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); - - // Current limit for motors to avoid breaker problems - m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveFL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_SteerFR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerFL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - - // Sets up odometry - currentPosition = new PointDir(0, 0, 0); - motorList = new MotorController[] {m_DriveFR, m_DriveFL, m_DriveBL, - m_DriveBR}; - CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; - wheelPositions = - new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; - log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + CANCoderList.length); - swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - - // Sets the offset value for each steering motor so that each is aligned - setEncoderOffset(); - } - - /** - * Sets just the steer for a specific module - * Can be used to turn the wheels without moving - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteer(MotorController steerMotor, Vector2D vector, double offset) { - - // Calculates the angle of the vector from -180° to 180° - final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); - - // Add 360 * number of full rotations to vectorTheta, then add offset - final double angleDegrees = vectorTheta + 360*(Math.round((steerMotor.getSensorPosition()/ENCODER_CONVERSION_FACTOR - offset - vectorTheta)/360)) + offset; - - // Sets the degree of the steer wheel - // Needs to multiply by encoderconversionfactor to translate into a unit the motor understands - steerMotor.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR*angleDegrees); - - SmartDashboard.putNumber("Angle", angleDegrees); - } - - /** - * Sets the motion for a specific module - * @param driveMotor the drive motor of this module - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteerAndPower(MotorController driveMotor, MotorController steerMotor, Vector2D vector, double offset) { - checkContextOwnership(); - - controlModuleSteer(steerMotor, vector, offset); - - // Sets the power to the magnitude of the vector - driveMotor.set(vector.getNorm()); - - } - - /** - * Maps parameters to robot oriented swerve movement - * @param x the x value for the position joystick - * @param y the y value for the position joystick - * @param turn the turn value from the rotation joystick - */ - public void controlRobotOriented(double x, double y, double turn) { - // Finds the vectors for turning and for translation of each module, and adds them - // Applies this for each module - controlModuleSteerAndPower(m_DriveFL, m_SteerFL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X).normalize()), offsetFL); - controlModuleSteerAndPower(m_DriveFR, m_SteerFR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X).normalize()), offsetFR); - controlModuleSteerAndPower(m_DriveBR, m_SteerBR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X).normalize()), offsetBR); - controlModuleSteerAndPower(m_DriveBL, m_SteerBL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X).normalize()), offsetBL); - } - - /** - * Uses controlRobotOriented() to control the robot relative to the field - * @param yawRad the robot gyro's current yaw value in radians - * @param x the x value for the position joystick - * @param y the y value for the position joystick - * @param turn the turn value from the rotation joystick - */ - public void controlFieldOriented(double yawRad, double x, double y, double turn) { - // Applies a rotational translation to controlRobotOriented - // Counteracts the forward direction changing when the robot turns - // TODO: change to inverse rotation matrix (rather than negative angle) - controlRobotOriented(Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, turn); - } - - /* - * Compares the absolute encoder to the motor encoder to find each motor's offset - * This helps each wheel to always be aligned - */ - public void setEncoderOffset() { - offsetFR = (m_SteerFR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_FrontRight.getAbsolutePosition(); - offsetFL = (m_SteerFL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_FrontLeft.getAbsolutePosition(); - offsetBR = (m_SteerBR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_BackRight.getAbsolutePosition(); - offsetBL = (m_SteerBL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_BackLeft.getAbsolutePosition(); - } - - /* - * Stops each drive motor - */ - public void stopDrive() { - checkContextOwnership(); - m_DriveFR.stopMotor(); - m_DriveFL.stopMotor(); - m_DriveBR.stopMotor(); - m_DriveBL.stopMotor(); - } - - /* - * Turns wheels in a cross formation to prevent robot from moving - */ - public void setCross() { - controlModuleSteer(m_SteerFL, new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X), offsetFL); - controlModuleSteer(m_SteerFR, new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X), offsetFR); - controlModuleSteer(m_SteerBL, new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X), offsetBL); - controlModuleSteer(m_SteerBR, new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X), offsetBR); - } - - // TODO: rework odometry so it doesn't have to go through drive - // TODO: figure out why odometry x and y are swapped - public PointDir getCurrentPosition() { - return currentPosition; - } - - public void setCurrentPosition(Point P) { - swerveOdometry.setCurrentPosition(P); - } - - public void resetCurrentPosition() { - swerveOdometry.setCurrentPosition(new Point(0, 0)); - } - - // Odometry - @Override - public void run() { - currentPosition = swerveOdometry.run(); - log(currentPosition.toString()); - SmartDashboard.putString("position", currentPosition.toString()); - } -} \ No newline at end of file + // Drive motors + private MotorController m_DriveFR; + private MotorController m_DriveFL; + private MotorController m_DriveBR; + private MotorController m_DriveBL; + + // Motors to turn each wheel in place + private MotorController m_SteerFR; + private MotorController m_SteerFL; + private MotorController m_SteerBR; + private MotorController m_SteerBL; + + // Absolute encoders (cancoders) + private CANCoder e_FrontRight; + private CANCoder e_FrontLeft; + private CANCoder e_BackRight; + private CANCoder e_BackLeft; + + /* + * Specific offsets to align each wheel + * These are assigned upon startup with setEncoderOffset() + */ + private double offsetFR; + private double offsetFL; + private double offsetBR; + private double offsetBL; + + /* + * Factor that converts between motor units and degrees + * Multiply to convert from degrees to motor units + * Divide to convert from motor units to degrees + */ + private final double ENCODER_CONVERSION_FACTOR = + (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + + // TODO: rework odometry so it doesn't have to go through drive + + // declaration of odometry object + private Odometry swerveOdometry; + // variable representing current position + private static PointDir currentPosition; + + // other variables to set up odometry + private MotorController[] motorList; + private CANCoder[] CANCoderList; + private Point[] wheelPositions; + + public Drive() { + + loggerCategory = Category.DRIVE; + // Initializations of motors + + // Initialize the drive motors + m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); + + // Initialize the steering motors + m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); + + // Initialize the encoders + e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); + + // Current limit for motors to avoid breaker problems + m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_DriveFL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_DriveBR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_DriveBL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + m_SteerFR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + m_SteerFL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + m_SteerBR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + m_SteerBL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + + // Sets up odometry + currentPosition = new PointDir(0, 0, 0); + motorList = new MotorController[] {m_DriveFR, m_DriveFL, m_DriveBL, m_DriveBR}; + CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; + wheelPositions = + new Point[] { + new Point( + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point( + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point( + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point( + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2) + }; + log("MotorList Length: " + motorList.length); + log("CANCoderList Length: " + CANCoderList.length); + swerveOdometry = + new Odometry( + motorList, + CANCoderList, + wheelPositions, + OdometryInputConstants.WHEEL_CIRCUMFERENCE, + OdometryInputConstants.GEAR_RATIO, + OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, + OdometryInputConstants.RATE_LIMITER_TIME); + + // Sets the offset value for each steering motor so that each is aligned + setEncoderOffset(); + } + + /** + * Sets just the steer for a specific module + * Can be used to turn the wheels without moving + * @param steerMotor the steer motor of this module + * @param vector the vector specifying the module's motion + * @param offset the offset for this module + */ + public void controlModuleSteer(MotorController steerMotor, Vector2D vector, double offset) { + + // Calculates the angle of the vector from -180° to 180° + final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); + + // Add 360 * number of full rotations to vectorTheta, then add offset + final double angleDegrees = + vectorTheta + + 360 + * (Math.round( + (steerMotor.getSensorPosition() / ENCODER_CONVERSION_FACTOR + - offset + - vectorTheta) + / 360)) + + offset; + + // Sets the degree of the steer wheel + // Needs to multiply by encoderconversionfactor to translate into a unit the motor + // understands + steerMotor.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees); + + SmartDashboard.putNumber("Angle", angleDegrees); + } + + /** + * Sets the motion for a specific module + * @param driveMotor the drive motor of this module + * @param steerMotor the steer motor of this module + * @param vector the vector specifying the module's motion + * @param offset the offset for this module + */ + public void controlModuleSteerAndPower( + MotorController driveMotor, + MotorController steerMotor, + Vector2D vector, + double offset) { + checkContextOwnership(); + + controlModuleSteer(steerMotor, vector, offset); + + // Sets the power to the magnitude of the vector + driveMotor.set(vector.getNorm()); + } + + /** + * Maps parameters to robot oriented swerve movement + * @param x the x value for the position joystick + * @param y the y value for the position joystick + * @param turn the turn value from the rotation joystick + */ + public void controlRobotOriented(double x, double y, double turn) { + // Finds the vectors for turning and for translation of each module, and adds them + // Applies this for each module + controlModuleSteerAndPower( + m_DriveFL, + m_SteerFL, + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X) + .normalize()), + offsetFL); + controlModuleSteerAndPower( + m_DriveFR, + m_SteerFR, + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X) + .normalize()), + offsetFR); + controlModuleSteerAndPower( + m_DriveBR, + m_SteerBR, + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X) + .normalize()), + offsetBR); + controlModuleSteerAndPower( + m_DriveBL, + m_SteerBL, + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X) + .normalize()), + offsetBL); + } + + /** + * Uses controlRobotOriented() to control the robot relative to the field + * @param yawRad the robot gyro's current yaw value in radians + * @param x the x value for the position joystick + * @param y the y value for the position joystick + * @param turn the turn value from the rotation joystick + */ + public void controlFieldOriented(double yawRad, double x, double y, double turn) { + // Applies a rotational translation to controlRobotOriented + // Counteracts the forward direction changing when the robot turns + // TODO: change to inverse rotation matrix (rather than negative angle) + controlRobotOriented( + Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, + Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, + turn); + } + + /* + * Compares the absolute encoder to the motor encoder to find each motor's offset + * This helps each wheel to always be aligned + */ + public void setEncoderOffset() { + offsetFR = + (m_SteerFR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - e_FrontRight.getAbsolutePosition(); + offsetFL = + (m_SteerFL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - e_FrontLeft.getAbsolutePosition(); + offsetBR = + (m_SteerBR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - e_BackRight.getAbsolutePosition(); + offsetBL = + (m_SteerBL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - e_BackLeft.getAbsolutePosition(); + } + + /* + * Stops each drive motor + */ + public void stopDrive() { + checkContextOwnership(); + m_DriveFR.stopMotor(); + m_DriveFL.stopMotor(); + m_DriveBR.stopMotor(); + m_DriveBL.stopMotor(); + } + + /* + * Turns wheels in a cross formation to prevent robot from moving + */ + public void setCross() { + controlModuleSteer( + m_SteerFL, + new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X), + offsetFL); + controlModuleSteer( + m_SteerFR, + new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X), + offsetFR); + controlModuleSteer( + m_SteerBL, + new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X), + offsetBL); + controlModuleSteer( + m_SteerBR, + new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X), + offsetBR); + } + + // TODO: rework odometry so it doesn't have to go through drive + // TODO: figure out why odometry x and y are swapped + public PointDir getCurrentPosition() { + return currentPosition; + } + + public void setCurrentPosition(Point P) { + swerveOdometry.setCurrentPosition(P); + } + + public void resetCurrentPosition() { + swerveOdometry.setCurrentPosition(new Point(0, 0)); + } + + // Odometry + @Override + public void run() { + currentPosition = swerveOdometry.run(); + log(currentPosition.toString()); + SmartDashboard.putString("position", currentPosition.toString()); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java index a6929e401..bb5b2660c 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java @@ -1,8 +1,10 @@ package com.team766.robot.gatorade.mechanisms; + +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.SparkMaxPIDController.AccelStrategy; +import com.revrobotics.SparkMaxPIDController; import com.team766.config.ConfigFileReader; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; @@ -12,187 +14,186 @@ import com.team766.logging.Severity; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import static com.team766.robot.gatorade.constants.ConfigConstants.*; public class Elevator extends Mechanism { - public enum Position { - - // TODO: do we need separate heights for cones vs cubes? - - /** Elevator is fully retracted. */ - RETRACTED(0), - /** Elevator is the appropriate height to place game pieces at the low node. */ - LOW(5), - /** Elevator is the appropriate height to place game pieces at the mid node. */ - MID(18), - /** Elevator is at appropriate height to place game pieces at the high node. */ - HIGH(40), - /** Elevator is at appropriate height to grab cubes from the human player. */ - HUMAN_CUBES(40), - /** Elevator is at appropriate height to grab cones from the human player. */ - HUMAN_CONES(40), - /** Elevator is fully extended. */ - EXTENDED(40); - - private final double height; - - Position(double position) { - this.height = position; - } - - private double getHeight() { - return height; - } - } - - private static final double NUDGE_INCREMENT = 5.0; - private static final double NUDGE_DAMPENER = 0.25; - - private static final double NEAR_THRESHOLD = 2.0; - - private final CANSparkMax leftMotor; - private final CANSparkMax rightMotor; - private final SparkMaxPIDController pidController; - private final ValueProvider pGain; - private final ValueProvider iGain; - private final ValueProvider dGain; - private final ValueProvider ffGain; - private final ValueProvider maxVelocity; - private final ValueProvider minOutputVelocity; - private final ValueProvider maxAccel; - - - private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); - - /** - * Contructs a new Elevator. - */ - public Elevator() { - MotorController halLeftMotor = RobotProvider.instance.getMotor(ELEVATOR_LEFT_MOTOR); - MotorController halRightMotor = RobotProvider.instance.getMotor(ELEVATOR_RIGHT_MOTOR); - - if (!((halLeftMotor instanceof CANSparkMax)&&(halRightMotor instanceof CANSparkMax))) { - log(Severity.ERROR, "Motors are not CANSparkMaxes!"); - throw new IllegalStateException("Motor are not CANSparkMaxes!"); - } - - leftMotor = (CANSparkMax) halLeftMotor; - rightMotor = (CANSparkMax) halRightMotor; - - rightMotor.follow(leftMotor, true); - - resetEncoder(); - - pidController = leftMotor.getPIDController(); - pidController.setFeedbackDevice(leftMotor.getEncoder()); - - pGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_PGAIN); - iGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_IGAIN); - dGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_DGAIN); - ffGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_FFGAIN); - maxVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_VELOCITY); - minOutputVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MIN_OUTPUT_VELOCITY); - maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL); - } - - public void resetEncoder() { - leftMotor.getEncoder().setPosition( - EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); - } - - public double getRotations() { - return leftMotor.getEncoder().getPosition(); - } - - /** - * Returns the current height of the elevator, in inches ('Murica). - */ - public double getHeight() { - return EncoderUtils.elevatorRotationsToHeight(leftMotor.getEncoder().getPosition()); - } - - public boolean isNearTo(Position position) { - return isNearTo(position.getHeight()); - } - - public boolean isNearTo(double position) { - return Math.abs(position - getHeight()) < NEAR_THRESHOLD; - } - - public void nudgeNoPID(double value) { - checkContextOwnership(); - double clampedValue = MathUtil.clamp(value, -1, 1); - clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear - leftMotor.set(clampedValue); - } - - public void stopElevator() { - checkContextOwnership(); - leftMotor.set(0); - } - - public void nudgeUp() { - System.err.println("Nudging up."); - - double height = getHeight(); - // NOTE: this could artificially limit nudge range - double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight()); - System.err.println("Target: " + targetHeight); - - moveTo(targetHeight); - } - - public void nudgeDown() { - double height = getHeight(); - // NOTE: this could artificially limit nudge range - double targetHeight = Math.max(height - NUDGE_INCREMENT, Position.RETRACTED.getHeight()); - moveTo(targetHeight); - } - - /** - * Moves the elevator to a pre-set {@link Position}. - */ - public void moveTo(Position position) { - moveTo(position.getHeight()); - } - - /** - * Moves the elevator to a specific position (in inches). - */ - public void moveTo(double position) { - checkContextOwnership(); - - System.err.println("Setting target position to " + position); - // set the PID controller values with whatever the latest is in the config - pidController.setP(pGain.get()); - pidController.setI(iGain.get()); - pidController.setD(dGain.get()); - // pidController.setFF(ffGain.get()); - double ff = ffGain.get(); - - - pidController.setOutputRange(-0.4, 0.4); - - // pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0); - // pidController.setSmartMotionMaxVelocity(maxVelocity.get(), 0); - // pidController.setSmartMotionMinOutputVelocity(minOutputVelocity.get(), 0); - // pidController.setSmartMotionMaxAccel(maxAccel.get(), 0); - - // convert the desired target degrees to encoder units - double rotations = EncoderUtils.elevatorHeightToRotations(position); - - // SmartDashboard.putNumber("[ELEVATOR] ff", ff); - SmartDashboard.putNumber("[ELEVATOR] reference", rotations); - - // set the reference point for the wrist - pidController.setReference(rotations, ControlType.kPosition, 0, ff); - } - - @Override - public void run() { - if (rateLimiter.next()) { - SmartDashboard.putNumber("[ELEVATOR] Height", getHeight()); - SmartDashboard.putNumber("[ELEVATOR] Rotations", getRotations()); - } - } + public enum Position { + + // TODO: do we need separate heights for cones vs cubes? + + /** Elevator is fully retracted. */ + RETRACTED(0), + /** Elevator is the appropriate height to place game pieces at the low node. */ + LOW(5), + /** Elevator is the appropriate height to place game pieces at the mid node. */ + MID(18), + /** Elevator is at appropriate height to place game pieces at the high node. */ + HIGH(40), + /** Elevator is at appropriate height to grab cubes from the human player. */ + HUMAN_CUBES(40), + /** Elevator is at appropriate height to grab cones from the human player. */ + HUMAN_CONES(40), + /** Elevator is fully extended. */ + EXTENDED(40); + + private final double height; + + Position(double position) { + this.height = position; + } + + private double getHeight() { + return height; + } + } + + private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.25; + + private static final double NEAR_THRESHOLD = 2.0; + + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final ValueProvider maxVelocity; + private final ValueProvider minOutputVelocity; + private final ValueProvider maxAccel; + + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Contructs a new Elevator. + */ + public Elevator() { + MotorController halLeftMotor = RobotProvider.instance.getMotor(ELEVATOR_LEFT_MOTOR); + MotorController halRightMotor = RobotProvider.instance.getMotor(ELEVATOR_RIGHT_MOTOR); + + if (!((halLeftMotor instanceof CANSparkMax) && (halRightMotor instanceof CANSparkMax))) { + log(Severity.ERROR, "Motors are not CANSparkMaxes!"); + throw new IllegalStateException("Motor are not CANSparkMaxes!"); + } + + leftMotor = (CANSparkMax) halLeftMotor; + rightMotor = (CANSparkMax) halRightMotor; + + rightMotor.follow(leftMotor, true); + + resetEncoder(); + + pidController = leftMotor.getPIDController(); + pidController.setFeedbackDevice(leftMotor.getEncoder()); + + pGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_FFGAIN); + maxVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_VELOCITY); + minOutputVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MIN_OUTPUT_VELOCITY); + maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL); + } + + public void resetEncoder() { + leftMotor + .getEncoder() + .setPosition( + EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); + } + + public double getRotations() { + return leftMotor.getEncoder().getPosition(); + } + + /** + * Returns the current height of the elevator, in inches ('Murica). + */ + public double getHeight() { + return EncoderUtils.elevatorRotationsToHeight(leftMotor.getEncoder().getPosition()); + } + + public boolean isNearTo(Position position) { + return isNearTo(position.getHeight()); + } + + public boolean isNearTo(double position) { + return Math.abs(position - getHeight()) < NEAR_THRESHOLD; + } + + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + leftMotor.set(clampedValue); + } + + public void stopElevator() { + checkContextOwnership(); + leftMotor.set(0); + } + + public void nudgeUp() { + System.err.println("Nudging up."); + + double height = getHeight(); + // NOTE: this could artificially limit nudge range + double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight()); + System.err.println("Target: " + targetHeight); + + moveTo(targetHeight); + } + + public void nudgeDown() { + double height = getHeight(); + // NOTE: this could artificially limit nudge range + double targetHeight = Math.max(height - NUDGE_INCREMENT, Position.RETRACTED.getHeight()); + moveTo(targetHeight); + } + + /** + * Moves the elevator to a pre-set {@link Position}. + */ + public void moveTo(Position position) { + moveTo(position.getHeight()); + } + + /** + * Moves the elevator to a specific position (in inches). + */ + public void moveTo(double position) { + checkContextOwnership(); + + System.err.println("Setting target position to " + position); + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + // pidController.setFF(ffGain.get()); + double ff = ffGain.get(); + + pidController.setOutputRange(-0.4, 0.4); + + // pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0); + // pidController.setSmartMotionMaxVelocity(maxVelocity.get(), 0); + // pidController.setSmartMotionMinOutputVelocity(minOutputVelocity.get(), 0); + // pidController.setSmartMotionMaxAccel(maxAccel.get(), 0); + + // convert the desired target degrees to encoder units + double rotations = EncoderUtils.elevatorHeightToRotations(position); + + // SmartDashboard.putNumber("[ELEVATOR] ff", ff); + SmartDashboard.putNumber("[ELEVATOR] reference", rotations); + + // set the reference point for the wrist + pidController.setReference(rotations, ControlType.kPosition, 0, ff); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("[ELEVATOR] Height", getHeight()); + SmartDashboard.putNumber("[ELEVATOR] Rotations", getRotations()); + } + } } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java index f02571734..febf753f4 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java @@ -6,69 +6,71 @@ */ public final class EncoderUtils { - /** - * Utility class. - */ - private EncoderUtils() { - } + /** + * Utility class. + */ + private EncoderUtils() {} - /** - * Converts a target rotation (in degrees) to encoder units for the wrist motor. - */ - public static double wristDegreesToRotations(double angle) { - // angle * net gear ratio * (rotations / degrees) - // FIXME: replace 32 with actual # of teeth - return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.); - } - - /** - * Converts the wrist motor's rotations to degrees. - */ - public static double wristRotationsToDegrees(double rotations) { - // rotations * net gear ratio * (degrees / rotations) - // FIXME: replace 32 with actual # of teeth - return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.); - } + /** + * Converts a target rotation (in degrees) to encoder units for the wrist motor. + */ + public static double wristDegreesToRotations(double angle) { + // angle * net gear ratio * (rotations / degrees) + // FIXME: replace 32 with actual # of teeth + return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.); + } - /** - * Converts a desired height (in inches) to rotations for the elevator motors. - */ - public static double elevatorHeightToRotations(double height) { - // height * net gear ratio * (rotations / height) - return height * (36./12.) * (1./(1.641 * Math.PI)); - } + /** + * Converts the wrist motor's rotations to degrees. + */ + public static double wristRotationsToDegrees(double rotations) { + // rotations * net gear ratio * (degrees / rotations) + // FIXME: replace 32 with actual # of teeth + return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.); + } - /** - * Converts the elevator motor's rotations to a height (in inches). - */ - public static double elevatorRotationsToHeight(double rotations) { - // rotations * net gear ratio * (height / rotations) - // FIXME: everything - return rotations * (12./36.) * ((1.641 * Math.PI)/1.); - } + /** + * Converts a desired height (in inches) to rotations for the elevator motors. + */ + public static double elevatorHeightToRotations(double height) { + // height * net gear ratio * (rotations / height) + return height * (36. / 12.) * (1. / (1.641 * Math.PI)); + } - /** - * Cosine law - * @param side1 - * @param side2 - * @param angle in degrees - * @return - */ - public static double lawOfCosines(double side1, double side2, double angle) { - double side3Squared = (Math.pow(side1,2.0)+Math.pow(side2,2.0)-2*side1*side2*Math.cos(Math.toRadians(angle))); - return Math.sqrt(side3Squared); - } + /** + * Converts the elevator motor's rotations to a height (in inches). + */ + public static double elevatorRotationsToHeight(double rotations) { + // rotations * net gear ratio * (height / rotations) + // FIXME: everything + return rotations * (12. / 36.) * ((1.641 * Math.PI) / 1.); + } - public static double lawOfSines(double side1, double angle1, double side2) { - return Math.asin(side2*Math.sin(angle1)/side1); - } + /** + * Cosine law + * @param side1 + * @param side2 + * @param angle in degrees + * @return + */ + public static double lawOfCosines(double side1, double side2, double angle) { + double side3Squared = + (Math.pow(side1, 2.0) + + Math.pow(side2, 2.0) + - 2 * side1 * side2 * Math.cos(Math.toRadians(angle))); + return Math.sqrt(side3Squared); + } - public static double clampValueToRange(double value, double min, double max) { - if (value > max){ - value = max; - } else if (value < min){ - value = min; - } - return value; - } + public static double lawOfSines(double side1, double angle1, double side2) { + return Math.asin(side2 * Math.sin(angle1) / side1); + } + + public static double clampValueToRange(double value, double min, double max) { + if (value > max) { + value = max; + } else if (value < min) { + value = min; + } + return value; + } } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java index dafd427da..55a0177db 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Gyro.java @@ -1,4 +1,5 @@ package com.team766.robot.gatorade.mechanisms; + import com.ctre.phoenix.sensors.Pigeon2; import com.team766.framework.Mechanism; import com.team766.library.RateLimiter; @@ -7,57 +8,61 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Gyro extends Mechanism { - Pigeon2 g_gyro = new Pigeon2(0, SwerveDriveConstants.SWERVE_CANBUS); - double[] gyroArray = new double[3]; - private RateLimiter l_loggingRate = new RateLimiter(0.05); - public Gyro() { - loggerCategory = Category.GYRO; - } - public void resetGyro(){ - g_gyro.setYaw(0); - } - - public void resetGyro180() { - g_gyro.setYaw(180); - } - - public void setGyro(double angleDeg) { - g_gyro.setYaw(angleDeg); - } - - public double getGyroPitch() { - double angle = g_gyro.getPitch(); - return angle; - } - - public double getGyroYaw() { - double angle = -1* g_gyro.getYaw(); - return angle; - } - - public double getGyroRoll() { - double angle = g_gyro.getRoll(); - return angle; - } - - @Override - public void run() { - if (l_loggingRate.next()) { - gyroArray[0] = getGyroYaw(); - gyroArray[1] = getGyroPitch(); - gyroArray[2] = getGyroRoll(); - SmartDashboard.putNumber("Yaw", gyroArray[0]); - SmartDashboard.putNumber("Pitch", gyroArray[1]); - SmartDashboard.putNumber("Roll", gyroArray[2]); - g_gyro.getYawPitchRoll(gyroArray); - } - } - - /** - * @return combined pitch and roll values of gyro - */ - public double getAbsoluteTilt() { - return Math.toDegrees(Math.acos(Math.cos(Math.toRadians(getGyroRoll()))*Math.cos(Math.toRadians(getGyroPitch())))); - } - -} \ No newline at end of file + Pigeon2 g_gyro = new Pigeon2(0, SwerveDriveConstants.SWERVE_CANBUS); + double[] gyroArray = new double[3]; + private RateLimiter l_loggingRate = new RateLimiter(0.05); + + public Gyro() { + loggerCategory = Category.GYRO; + } + + public void resetGyro() { + g_gyro.setYaw(0); + } + + public void resetGyro180() { + g_gyro.setYaw(180); + } + + public void setGyro(double angleDeg) { + g_gyro.setYaw(angleDeg); + } + + public double getGyroPitch() { + double angle = g_gyro.getPitch(); + return angle; + } + + public double getGyroYaw() { + double angle = -1 * g_gyro.getYaw(); + return angle; + } + + public double getGyroRoll() { + double angle = g_gyro.getRoll(); + return angle; + } + + @Override + public void run() { + if (l_loggingRate.next()) { + gyroArray[0] = getGyroYaw(); + gyroArray[1] = getGyroPitch(); + gyroArray[2] = getGyroRoll(); + SmartDashboard.putNumber("Yaw", gyroArray[0]); + SmartDashboard.putNumber("Pitch", gyroArray[1]); + SmartDashboard.putNumber("Roll", gyroArray[2]); + g_gyro.getYawPitchRoll(gyroArray); + } + } + + /** + * @return combined pitch and roll values of gyro + */ + public double getAbsoluteTilt() { + return Math.toDegrees( + Math.acos( + Math.cos(Math.toRadians(getGyroRoll())) + * Math.cos(Math.toRadians(getGyroPitch())))); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java index b0aa3dc0c..ff32fd344 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java @@ -1,19 +1,19 @@ package com.team766.robot.gatorade.mechanisms; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.library.RateLimiter; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import static com.team766.robot.gatorade.constants.ConfigConstants.*; /** * Basic intake. Mounted on end of {@link Wrist}. The intake can be controlled to attempt to * pull a game piece in via {@link #in}, release a contained game piece via {@link #out}, or stop * moving via {@link #stop}. - * - * + * + * * While the Intake does not maintain any state as to whether or not it contains a game piece, * it does have different modes of operation based on what kind of game piece it is prepared to * intake or outtake. This is because the motor must spin in opposite directions to intake cubes @@ -21,113 +21,113 @@ */ public class Intake extends Mechanism { - private static final double POWER_IN = 0.3; - private static final double POWER_OUT = 0.25; - private static final double POWER_IDLE = 0.02; - - /** - * The current type of game piece the Intake is preparing to hold or is holding. - */ - public enum GamePieceType { - CONE, - CUBE - } - - /** - * The current movement state for the intake. - */ - public enum State { - STOPPED, - IDLE, - IN, - OUT - } - - private MotorController motor; - private GamePieceType gamePieceType = GamePieceType.CONE; - private State state = State.IDLE; - private RateLimiter rateLimiter = new RateLimiter(5 /* seconds */); - - /** - * Constructs a new Intake. - */ - public Intake() { - motor = RobotProvider.instance.getMotor(INTAKE_MOTOR); - } - - /** - * Returns the type of game piece the Intake is preparing to hold or is holding. - * @return The current game piece type. - */ - public GamePieceType getGamePieceType() { - return gamePieceType; - } - - /** - * Sets the type of game piece type the Intake is preparing to hold or is holding. - */ - public void setGamePieceType(GamePieceType type) { - checkContextOwnership(); - - this.gamePieceType = type; - } - - /** - * Returns the current movement state of the intake. - * - * @return The current movement state of the intake. - */ - public State getState() { - return state; - } - - /** - * Turns the intake motor on in order to pull a game piece into the mechanism. - */ - public void in() { - checkContextOwnership(); - - double power = (gamePieceType == GamePieceType.CONE) ? POWER_IN : (-1 * POWER_IN); - motor.set(power); - state = State.IN; - } - - /** - * Turns the intake motor on in reverse direction, to release any contained game piece. - */ - public void out() { - checkContextOwnership(); - - double power = (gamePieceType == GamePieceType.CONE) ? (-1 * POWER_OUT) : POWER_OUT; - motor.set(power); - state = State.OUT; - } - - /** - * Turns off the intake motor. - */ - public void stop() { - checkContextOwnership(); - motor.set(0.0); - state = State.STOPPED; - } - - /** - * Turns the intake to idle - run at low power to keep the game piece contained. - */ - public void idle() { - checkContextOwnership(); - - double power = (gamePieceType == GamePieceType.CONE) ? POWER_IDLE : (-1 * POWER_IDLE); - motor.set(power); - state = State.IDLE; - } - - @Override - public void run() { - if (rateLimiter.next()) { - SmartDashboard.putString("[INTAKE] Game Piece", gamePieceType.toString()); - SmartDashboard.putString("[INTAKE] State", state.toString()); - } - } -} \ No newline at end of file + private static final double POWER_IN = 0.3; + private static final double POWER_OUT = 0.25; + private static final double POWER_IDLE = 0.02; + + /** + * The current type of game piece the Intake is preparing to hold or is holding. + */ + public enum GamePieceType { + CONE, + CUBE + } + + /** + * The current movement state for the intake. + */ + public enum State { + STOPPED, + IDLE, + IN, + OUT + } + + private MotorController motor; + private GamePieceType gamePieceType = GamePieceType.CONE; + private State state = State.IDLE; + private RateLimiter rateLimiter = new RateLimiter(5 /* seconds */); + + /** + * Constructs a new Intake. + */ + public Intake() { + motor = RobotProvider.instance.getMotor(INTAKE_MOTOR); + } + + /** + * Returns the type of game piece the Intake is preparing to hold or is holding. + * @return The current game piece type. + */ + public GamePieceType getGamePieceType() { + return gamePieceType; + } + + /** + * Sets the type of game piece type the Intake is preparing to hold or is holding. + */ + public void setGamePieceType(GamePieceType type) { + checkContextOwnership(); + + this.gamePieceType = type; + } + + /** + * Returns the current movement state of the intake. + * + * @return The current movement state of the intake. + */ + public State getState() { + return state; + } + + /** + * Turns the intake motor on in order to pull a game piece into the mechanism. + */ + public void in() { + checkContextOwnership(); + + double power = (gamePieceType == GamePieceType.CONE) ? POWER_IN : (-1 * POWER_IN); + motor.set(power); + state = State.IN; + } + + /** + * Turns the intake motor on in reverse direction, to release any contained game piece. + */ + public void out() { + checkContextOwnership(); + + double power = (gamePieceType == GamePieceType.CONE) ? (-1 * POWER_OUT) : POWER_OUT; + motor.set(power); + state = State.OUT; + } + + /** + * Turns off the intake motor. + */ + public void stop() { + checkContextOwnership(); + motor.set(0.0); + state = State.STOPPED; + } + + /** + * Turns the intake to idle - run at low power to keep the game piece contained. + */ + public void idle() { + checkContextOwnership(); + + double power = (gamePieceType == GamePieceType.CONE) ? POWER_IDLE : (-1 * POWER_IDLE); + motor.set(power); + state = State.IDLE; + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putString("[INTAKE] Game Piece", gamePieceType.toString()); + SmartDashboard.putString("[INTAKE] State", state.toString()); + } + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java index e3c86a83a..7a7afba0a 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java @@ -1,55 +1,54 @@ package com.team766.robot.gatorade.mechanisms; + import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.RainbowAnimation; import com.team766.framework.Mechanism; - -public class Lights extends Mechanism{ - - private CANdle candle; - private static final int CANID = 5; - private static final int LED_COUNT = 90; - private Animation rainbowAnimation = new RainbowAnimation(1, 0.1, LED_COUNT); - - public Lights(){ - candle = new CANdle(CANID); - - } - - public void purple(){ - checkContextOwnership(); - candle.setLEDs(128, 0, 128); - } - - public void white(){ - checkContextOwnership(); - // NOTE: 255, 255, 255 trips the breaker. lol - candle.setLEDs(128, 128, 128); - } - - public void yellow(){ - checkContextOwnership(); - candle.setLEDs(255, 255, 0); - } - - public void red() { - checkContextOwnership(); - candle.setLEDs(255, 0, 0); - } - - public void green() { - checkContextOwnership(); - candle.setLEDs(0, 255, 0); - } - - public void orange() { - checkContextOwnership(); - candle.setLEDs(255, 64, 0); - } - - public void rainbow() { - checkContextOwnership(); - candle.animate(rainbowAnimation); - } +public class Lights extends Mechanism { + + private CANdle candle; + private static final int CANID = 5; + private static final int LED_COUNT = 90; + private Animation rainbowAnimation = new RainbowAnimation(1, 0.1, LED_COUNT); + + public Lights() { + candle = new CANdle(CANID); + } + + public void purple() { + checkContextOwnership(); + candle.setLEDs(128, 0, 128); + } + + public void white() { + checkContextOwnership(); + // NOTE: 255, 255, 255 trips the breaker. lol + candle.setLEDs(128, 128, 128); + } + + public void yellow() { + checkContextOwnership(); + candle.setLEDs(255, 255, 0); + } + + public void red() { + checkContextOwnership(); + candle.setLEDs(255, 0, 0); + } + + public void green() { + checkContextOwnership(); + candle.setLEDs(0, 255, 0); + } + + public void orange() { + checkContextOwnership(); + candle.setLEDs(255, 64, 0); + } + + public void rainbow() { + checkContextOwnership(); + candle.animate(rainbowAnimation); + } } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java index 3357153e9..a6942f53e 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java @@ -1,5 +1,10 @@ package com.team766.robot.gatorade.mechanisms; +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.SparkMaxPIDController; import com.team766.config.ConfigFileReader; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; @@ -9,178 +14,175 @@ import com.team766.logging.Severity; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import static com.team766.robot.gatorade.constants.ConfigConstants.*; /** * Basic wrist mechanism. Used in conjunction with the {@link Intake} and {@link Elevator}. * Can be moved up and down as part of teleop or autonomous control to move the {@link Intake} - * (attached to the end of the Wrist) closer to a game piece or game element (eg node in the + * (attached to the end of the Wrist) closer to a game piece or game element (eg node in the * field, human player station), at which point the {@link Intake} can grab or release the game * piece as appropriate. */ public class Wrist extends Mechanism { - /** - * Pre-set positions for the wrist. - */ - public enum Position { - - // TODO: adjust these values. - - /** Wrist is in top position. Starting position. */ - TOP(-135), - /** Wrist is in the position for moving around the field. */ - RETRACTED(-120.0), - /** Wrist is level with ground. */ - LEVEL(0.0), - HIGH_NODE(-30), - MID_NODE(-25.5), - /** Wrist is fully down. */ - BOTTOM(60); - - private final double angle; - - Position(double angle) { - this.angle = angle; - } - - private double getAngle() { - return angle; - } - } - - private static final double NUDGE_INCREMENT = 5.0; - private static final double NUDGE_DAMPENER = 0.15; - - private static final double NEAR_THRESHOLD = 5.0; - - private final CANSparkMax motor; - private final SparkMaxPIDController pidController; - private final ValueProvider pGain; - private final ValueProvider iGain; - private final ValueProvider dGain; - private final ValueProvider ffGain; - private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); - - /** - * Contructs a new Wrist. - */ - public Wrist() { - MotorController halMotor = RobotProvider.instance.getMotor(WRIST_MOTOR); - if (!(halMotor instanceof CANSparkMax)) { - log(Severity.ERROR, "Motor is not a CANSparkMax!"); - throw new IllegalStateException("Motor is not a CANSparkMax!"); - } - motor = (CANSparkMax) halMotor; - - resetEncoder(); - - // stash the PIDController for convenience. will update the PID values to the latest from the config - // file each time we use the motor. - pidController = motor.getPIDController(); - pidController.setFeedbackDevice(motor.getEncoder()); - - // grab config values for PID. - pGain = ConfigFileReader.getInstance().getDouble(WRIST_PGAIN); - iGain = ConfigFileReader.getInstance().getDouble(WRIST_IGAIN); - dGain = ConfigFileReader.getInstance().getDouble(WRIST_DGAIN); - ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN); - } - - public void resetEncoder() { - motor.getEncoder() - .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); - } - - public double getRotations() { - return motor.getEncoder().getPosition(); - } - - /** - * Returns the current angle of the wrist. - */ - public double getAngle() { - return EncoderUtils.wristRotationsToDegrees(motor.getEncoder().getPosition()); - } - - public boolean isNearTo(Position position) { - return isNearTo(position.getAngle()); - } - - public boolean isNearTo(double angle) { - return Math.abs(angle - getAngle()) < NEAR_THRESHOLD; - } - - public void nudgeNoPID(double value) { - checkContextOwnership(); - double clampedValue = MathUtil.clamp(value, -1, 1); - clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear - motor.set(clampedValue); - } - - public void stopWrist() { - checkContextOwnership(); - motor.set(0); - } - - public void nudgeUp() { - System.err.println("Nudging up."); - double angle = getAngle(); - double targetAngle = Math.max(angle - NUDGE_INCREMENT, Position.TOP.getAngle()); - System.err.println("Target: " + targetAngle); - - rotate(targetAngle); - } - - public void nudgeDown() { - System.err.println("Nudging down."); - double angle = getAngle(); - double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.BOTTOM.getAngle()); - rotate(targetAngle); - } - - /** - * Rotates the wrist to a pre-set {@link Position}. - */ - public void rotate(Position position) { - rotate(position.getAngle()); - } - - /** - * Starts rotating the wrist to the specified angle. - * NOTE: this method returns immediately. Check the current wrist position of the wrist - * with {@link #getAngle()}. - */ - public void rotate(double angle) { - checkContextOwnership(); - - System.err.println("Setting target angle to " + angle); - // set the PID controller values with whatever the latest is in the config - pidController.setP(pGain.get()); - pidController.setI(iGain.get()); - pidController.setD(dGain.get()); - // pidController.setFF(ffGain.get()); - double ff = ffGain.get() * Math.cos(Math.toRadians(angle)); - SmartDashboard.putNumber("[WRIST] ff", ff); - SmartDashboard.putNumber("[WRIST] reference", angle); - - pidController.setOutputRange(-1, 1); - - // convert the desired target degrees to rotations - double rotations = EncoderUtils.wristDegreesToRotations(angle); - - // set the reference point for the wrist - pidController.setReference(rotations, ControlType.kPosition, 0, ff); - } - - @Override - public void run() { - if (rateLimiter.next()) { - SmartDashboard.putNumber("[WRIST] Angle", getAngle()); - SmartDashboard.putNumber("[WRIST] Rotations", getRotations()); - } - } + /** + * Pre-set positions for the wrist. + */ + public enum Position { + + // TODO: adjust these values. + + /** Wrist is in top position. Starting position. */ + TOP(-135), + /** Wrist is in the position for moving around the field. */ + RETRACTED(-120.0), + /** Wrist is level with ground. */ + LEVEL(0.0), + HIGH_NODE(-30), + MID_NODE(-25.5), + /** Wrist is fully down. */ + BOTTOM(60); + + private final double angle; + + Position(double angle) { + this.angle = angle; + } + + private double getAngle() { + return angle; + } + } + + private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.15; + + private static final double NEAR_THRESHOLD = 5.0; + + private final CANSparkMax motor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Contructs a new Wrist. + */ + public Wrist() { + MotorController halMotor = RobotProvider.instance.getMotor(WRIST_MOTOR); + if (!(halMotor instanceof CANSparkMax)) { + log(Severity.ERROR, "Motor is not a CANSparkMax!"); + throw new IllegalStateException("Motor is not a CANSparkMax!"); + } + motor = (CANSparkMax) halMotor; + + resetEncoder(); + + // stash the PIDController for convenience. will update the PID values to the latest from + // the config + // file each time we use the motor. + pidController = motor.getPIDController(); + pidController.setFeedbackDevice(motor.getEncoder()); + + // grab config values for PID. + pGain = ConfigFileReader.getInstance().getDouble(WRIST_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(WRIST_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(WRIST_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN); + } + + public void resetEncoder() { + motor.getEncoder() + .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); + } + + public double getRotations() { + return motor.getEncoder().getPosition(); + } + + /** + * Returns the current angle of the wrist. + */ + public double getAngle() { + return EncoderUtils.wristRotationsToDegrees(motor.getEncoder().getPosition()); + } + + public boolean isNearTo(Position position) { + return isNearTo(position.getAngle()); + } + + public boolean isNearTo(double angle) { + return Math.abs(angle - getAngle()) < NEAR_THRESHOLD; + } + + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + motor.set(clampedValue); + } + + public void stopWrist() { + checkContextOwnership(); + motor.set(0); + } + + public void nudgeUp() { + System.err.println("Nudging up."); + double angle = getAngle(); + double targetAngle = Math.max(angle - NUDGE_INCREMENT, Position.TOP.getAngle()); + System.err.println("Target: " + targetAngle); + + rotate(targetAngle); + } + + public void nudgeDown() { + System.err.println("Nudging down."); + double angle = getAngle(); + double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.BOTTOM.getAngle()); + rotate(targetAngle); + } + + /** + * Rotates the wrist to a pre-set {@link Position}. + */ + public void rotate(Position position) { + rotate(position.getAngle()); + } + + /** + * Starts rotating the wrist to the specified angle. + * NOTE: this method returns immediately. Check the current wrist position of the wrist + * with {@link #getAngle()}. + */ + public void rotate(double angle) { + checkContextOwnership(); + + System.err.println("Setting target angle to " + angle); + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + // pidController.setFF(ffGain.get()); + double ff = ffGain.get() * Math.cos(Math.toRadians(angle)); + SmartDashboard.putNumber("[WRIST] ff", ff); + SmartDashboard.putNumber("[WRIST] reference", angle); + + pidController.setOutputRange(-1, 1); + + // convert the desired target degrees to rotations + double rotations = EncoderUtils.wristDegreesToRotations(angle); + + // set the reference point for the wrist + pidController.setReference(rotations, ControlType.kPosition, 0, ff); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("[WRIST] Angle", getAngle()); + SmartDashboard.putNumber("[WRIST] Rotations", getRotations()); + } + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java b/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java index 4b48015da..2e0f90625 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java @@ -1,84 +1,104 @@ package com.team766.robot.gatorade.procedures; -import java.util.ArrayList; -import java.util.List; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.constants.ChargeConstants; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.ArrayList; +import java.util.List; // Helper class for AlignCharger, see AlignCharger.md for more details public class ChargeStationPathFinder { - // Since this class does not extend Procedure, we need to instantiate logger - private static final Logger logger = Logger.get(Category.PROCEDURES); - private final Alliance alliance; - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public ChargeStationPathFinder(Alliance alliance) { - this.alliance = alliance; - } + // Since this class does not extend Procedure, we need to instantiate logger + private static final Logger logger = Logger.get(Category.PROCEDURES); + private final Alliance alliance; + + /** + * Constructor which takes alliance + * @param alliance Alliance for choosing which charge station to align to + */ + public ChargeStationPathFinder(Alliance alliance) { + this.alliance = alliance; + } + + /** + * Calculates points that the robot needs to follow based on alliance and current position + * @param curPos the current robot position, represented by a pointDir + * @return PointDir[]: list of the calculated points that can be entered into FollowPoints + */ + public PointDir[] calculatePoints(PointDir curPos) { + + // Sets curX and curY variables based on curPos param + double curX = curPos.getX(); + double curY = curPos.getY(); - /** - * Calculates points that the robot needs to follow based on alliance and current position - * @param curPos the current robot position, represented by a pointDir - * @return PointDir[]: list of the calculated points that can be entered into FollowPoints - */ - public PointDir[] calculatePoints(PointDir curPos) { + // Creates ArrayList for points + List points = new ArrayList(); + switch (alliance) { + case Red: + // Calls addPoints method for red alliance coordinates + addPoints( + points, + curX, + curY, + ChargeConstants.RED_BALANCE_TARGET_X, + ChargeConstants.RED_LEFT_PT, + ChargeConstants.RED_RIGHT_PT, + ChargeConstants.MIDDLE); + break; - // Sets curX and curY variables based on curPos param - double curX = curPos.getX(); - double curY = curPos.getY(); - - // Creates ArrayList for points - List points = new ArrayList(); - switch (alliance) { - case Red: - // Calls addPoints method for red alliance coordinates - addPoints(points, curX, curY, ChargeConstants.RED_BALANCE_TARGET_X, ChargeConstants.RED_LEFT_PT, ChargeConstants.RED_RIGHT_PT, ChargeConstants.MIDDLE); - break; - - case Blue: - // Calls addPoints method for red alliance coordinates - addPoints(points, curX, curY, ChargeConstants.BLUE_BALANCE_TARGET_X, ChargeConstants.BLUE_LEFT_PT, ChargeConstants.BLUE_RIGHT_PT, ChargeConstants.MIDDLE); - break; - default: - logger.logRaw(null, "Invalid Alliance"); - } + case Blue: + // Calls addPoints method for red alliance coordinates + addPoints( + points, + curX, + curY, + ChargeConstants.BLUE_BALANCE_TARGET_X, + ChargeConstants.BLUE_LEFT_PT, + ChargeConstants.BLUE_RIGHT_PT, + ChargeConstants.MIDDLE); + break; + default: + logger.logRaw(null, "Invalid Alliance"); + } - // Converts pointDir arrayList to array and returns it - return points.toArray(new PointDir[points.size()]); - } + // Converts pointDir arrayList to array and returns it + return points.toArray(new PointDir[points.size()]); + } - /** - * Adds a set of points to an arrayList based on passed charge station coordinates - * @param points ArrayList to add points to - * @param curX Current robot X value - * @param curY Current robot Y value - * @param target X value of center of target charge station - * @param left X value of left side of target charge station - * @param right X value of right side of target charge station - * @param height Y value of center of target charge station - */ - private void addPoints(List points, double curX, double curY, double target, double left, double right, double height) { - // If on the right side of the target, check if robot is blocked by the right side of the charge station, and add points accordingly - if (curX > target) { - if (curX < right) - points.add(new PointDir(right, curY)); + /** + * Adds a set of points to an arrayList based on passed charge station coordinates + * @param points ArrayList to add points to + * @param curX Current robot X value + * @param curY Current robot Y value + * @param target X value of center of target charge station + * @param left X value of left side of target charge station + * @param right X value of right side of target charge station + * @param height Y value of center of target charge station + */ + private void addPoints( + List points, + double curX, + double curY, + double target, + double left, + double right, + double height) { + // If on the right side of the target, check if robot is blocked by the right side of the + // charge station, and add points accordingly + if (curX > target) { + if (curX < right) points.add(new PointDir(right, curY)); - points.add(new PointDir(right, height)); + points.add(new PointDir(right, height)); - // Otherwise, it will be on the left side of the target, so check if robot is blocked by the left side of the charge station, and add points accordingly - } else { - if (curX > left) - points.add(new PointDir(left, curY)); + // Otherwise, it will be on the left side of the target, so check if robot is blocked by + // the left side of the charge station, and add points accordingly + } else { + if (curX > left) points.add(new PointDir(left, curY)); - points.add(new PointDir(left, height)); - } - } - -} \ No newline at end of file + points.add(new PointDir(left, height)); + } + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java index 5ed5eb5a9..23c2f172a 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java @@ -5,7 +5,7 @@ public class ExtendWristvatorToHigh extends MoveWristvator { - public ExtendWristvatorToHigh() { - super(Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); - } + public ExtendWristvatorToHigh() { + super(Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java index 66b8f21f4..d01d999d6 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java @@ -1,15 +1,16 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; -import com.team766.robot.gatorade.mechanisms.Wrist; import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; +import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToHuman extends MoveWristvator { - public ExtendWristvatorToHuman(GamePieceType gamePieceType) { - super(gamePieceType == GamePieceType.CONE - ? Elevator.Position.HUMAN_CONES - : Elevator.Position.HUMAN_CUBES, - Wrist.Position.LEVEL); - } + public ExtendWristvatorToHuman(GamePieceType gamePieceType) { + super( + gamePieceType == GamePieceType.CONE + ? Elevator.Position.HUMAN_CONES + : Elevator.Position.HUMAN_CUBES, + Wrist.Position.LEVEL); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java index c009207b7..602f8497d 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java @@ -5,7 +5,7 @@ public class ExtendWristvatorToLow extends MoveWristvator { - public ExtendWristvatorToLow() { - super(Elevator.Position.LOW, Wrist.Position.LEVEL); - } + public ExtendWristvatorToLow() { + super(Elevator.Position.LOW, Wrist.Position.LEVEL); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java index c8fd16188..e34b96930 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java @@ -5,7 +5,7 @@ public class ExtendWristvatorToMid extends MoveWristvator { - public ExtendWristvatorToMid() { - super(Elevator.Position.MID, Wrist.Position.MID_NODE); - } + public ExtendWristvatorToMid() { + super(Elevator.Position.MID, Wrist.Position.MID_NODE); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java b/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java index fd8cbdab5..d8763dda6 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/GoForCones.java @@ -7,10 +7,10 @@ public class GoForCones extends Procedure { - @Override - public void run(Context context) { - context.takeOwnership(Robot.intake); + @Override + public void run(Context context) { + context.takeOwnership(Robot.intake); - Robot.intake.setGamePieceType(GamePieceType.CONE); - } + Robot.intake.setGamePieceType(GamePieceType.CONE); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java b/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java index 4b6673a26..844747e04 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/GoForCubes.java @@ -6,11 +6,11 @@ import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; public class GoForCubes extends Procedure { - - @Override - public void run(Context context) { - context.takeOwnership(Robot.intake); - - Robot.intake.setGamePieceType(GamePieceType.CUBE); - } + + @Override + public void run(Context context) { + context.takeOwnership(Robot.intake); + + Robot.intake.setGamePieceType(GamePieceType.CUBE); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java index a7ffb2b23..f61647aab 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java @@ -5,7 +5,6 @@ import com.team766.robot.gatorade.Robot; import com.team766.robot.gatorade.constants.ChargeConstants; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * {@link Procedure} to use the gyro to automatically balance on the charge station. @@ -13,166 +12,168 @@ */ public class GyroBalance extends Procedure { - // State machine with 4 states for position on ramp - private enum State { - GROUND, - RAMP_TRANSITION, - RAMP_TILT, - RAMP_LEVEL - } - - // Direction determines which direction the robot moves - private enum Direction { - LEFT, - RIGHT, - STOP, - } - - // tilt is the overall combination of pitch and roll - private double tilt = Robot.gyro.getAbsoluteTilt(); - - // absSpeed is unsigned speed value - private double absSpeed; - private State prevState; - private State curState; - private Direction direction; - private final Alliance alliance; - - private final double TOP_TILT = 15.0; - private final double FLAP_TILT = 11; - - // Tweak these values to adjust how the robot balances - private final double LEVEL = 7; - private final double CORRECTION_DELAY = 0.7; - private final double SPEED_GROUND = .3; - private final double SPEED_TRANSITION = .25; - private final double SPEED_TILT = .18; - - /** - * Constructor to create a new GyroBalance instance - * @param alliance Alliance for setting direction towards charge station - */ - public GyroBalance(Alliance alliance) { - this.alliance = alliance; - } - - public void run(Context context) { - context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.drive); - - // curY is current robot y position - double curY = Robot.drive.getCurrentPosition().getY(); - - // driveSpeed is actual value of speed passed into the swerveDrive method - double driveSpeed = 1; - - // Sets movement direction ground state if on ground - setDir(curY); - - // sets starting state if not on ground - if (tilt < LEVEL && curState != State.GROUND) { - curState = State.RAMP_LEVEL; - } else if (tilt < TOP_TILT && tilt > FLAP_TILT) { - curState = State.RAMP_TILT; - } else if (tilt > LEVEL) { - curState = State.RAMP_TRANSITION; - } - - do { - // Sets prevState to the current state and calculates curState - prevState = curState; - curY = Robot.drive.getCurrentPosition().getY(); - tilt = Robot.gyro.getAbsoluteTilt(); - //log("curX:" + curX); - //log("direction: " + direction); - setState(); - - // Both being on Red alliance and needing to move right would make the movement direction negative, so this expression corrects for that - if ((alliance == Alliance.Red) ^ (direction == Direction.RIGHT)) { - driveSpeed = -absSpeed; - } else { - driveSpeed = absSpeed; - } - - // Drives the robot with the calculated speed and direction - Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), 0, driveSpeed, 0); - context.yield(); - } - // Loops until robot is level or until a call to the abort() method - while (!(curState == State.RAMP_LEVEL)); - - // After the robot is level, drives for correctionDelay seconds. - // Direction is opposite due to inversion of speed in setState() so it corrects for overshooting - context.waitForSeconds(CORRECTION_DELAY); - - // Locks wheels once balanced - context.startAsync(new SetCross()); - - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - } - - // Sets state in state machine, see more details in GyroBalance.md - private void setState() { - if (prevState == State.GROUND && tilt > LEVEL) { - curState = State.RAMP_TRANSITION; - absSpeed = SPEED_TRANSITION; - log("Transition, prevState: " + prevState + ", curState: " + curState); - } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { - curState = State.RAMP_TILT; - absSpeed = SPEED_TILT; - log("Tilt, prevState: " + prevState + ", curState: " + curState); - } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { - curState = State.RAMP_LEVEL; - // If level, sets speed to negative to correct for overshooting - absSpeed = -absSpeed; - log("Level, prevState: " + prevState + ", curState: " + curState); - } - if (curState == State.GROUND) { - absSpeed = SPEED_GROUND; - } - } - - /** - * Sets direction towards desired charge station - * If robot is level and outside of charge station boundaries, sets state to ground - * @param curY current robot x position - */ - private void setDir(double curY) { - switch (alliance) { - case Red: - // If to the right of the charge station, go left - if (curY > ChargeConstants.RED_BALANCE_TARGET_X) { - // If level and outside of charge station boundaries, set state to ground - if (tilt < LEVEL && curY > ChargeConstants.RED_RIGHT_PT) { - curState = State.GROUND; - } - direction = Direction.LEFT; - // If to the left of the charge station, go right - } else { - if (tilt < LEVEL && curY < ChargeConstants.RED_LEFT_PT) { - curState = State.GROUND; - } - direction = Direction.RIGHT; - } - break; - case Blue: - // Same logic for blue alliance coordinates - if (curY > ChargeConstants.BLUE_BALANCE_TARGET_X) { - if (tilt < LEVEL && curY > ChargeConstants.BLUE_RIGHT_PT) { - curState = State.GROUND; - } - direction = Direction.LEFT; - } else { - if (tilt < LEVEL && curY < ChargeConstants.BLUE_LEFT_PT) { - curState = State.GROUND; - } - direction = Direction.RIGHT; - } - break; - default: - log("Invalid alliance"); - } - } - + // State machine with 4 states for position on ramp + private enum State { + GROUND, + RAMP_TRANSITION, + RAMP_TILT, + RAMP_LEVEL + } + + // Direction determines which direction the robot moves + private enum Direction { + LEFT, + RIGHT, + STOP, + } + + // tilt is the overall combination of pitch and roll + private double tilt = Robot.gyro.getAbsoluteTilt(); + + // absSpeed is unsigned speed value + private double absSpeed; + private State prevState; + private State curState; + private Direction direction; + private final Alliance alliance; + + private final double TOP_TILT = 15.0; + private final double FLAP_TILT = 11; + + // Tweak these values to adjust how the robot balances + private final double LEVEL = 7; + private final double CORRECTION_DELAY = 0.7; + private final double SPEED_GROUND = .3; + private final double SPEED_TRANSITION = .25; + private final double SPEED_TILT = .18; + + /** + * Constructor to create a new GyroBalance instance + * @param alliance Alliance for setting direction towards charge station + */ + public GyroBalance(Alliance alliance) { + this.alliance = alliance; + } + + public void run(Context context) { + context.takeOwnership(Robot.gyro); + context.takeOwnership(Robot.drive); + + // curY is current robot y position + double curY = Robot.drive.getCurrentPosition().getY(); + + // driveSpeed is actual value of speed passed into the swerveDrive method + double driveSpeed = 1; + + // Sets movement direction ground state if on ground + setDir(curY); + + // sets starting state if not on ground + if (tilt < LEVEL && curState != State.GROUND) { + curState = State.RAMP_LEVEL; + } else if (tilt < TOP_TILT && tilt > FLAP_TILT) { + curState = State.RAMP_TILT; + } else if (tilt > LEVEL) { + curState = State.RAMP_TRANSITION; + } + + do { + // Sets prevState to the current state and calculates curState + prevState = curState; + curY = Robot.drive.getCurrentPosition().getY(); + tilt = Robot.gyro.getAbsoluteTilt(); + // log("curX:" + curX); + // log("direction: " + direction); + setState(); + + // Both being on Red alliance and needing to move right would make the movement + // direction negative, so this expression corrects for that + if ((alliance == Alliance.Red) ^ (direction == Direction.RIGHT)) { + driveSpeed = -absSpeed; + } else { + driveSpeed = absSpeed; + } + + // Drives the robot with the calculated speed and direction + Robot.drive.controlFieldOriented( + Math.toRadians(Robot.gyro.getGyroYaw()), 0, driveSpeed, 0); + context.yield(); + } + // Loops until robot is level or until a call to the abort() method + while (!(curState == State.RAMP_LEVEL)); + + // After the robot is level, drives for correctionDelay seconds. + // Direction is opposite due to inversion of speed in setState() so it corrects for + // overshooting + context.waitForSeconds(CORRECTION_DELAY); + + // Locks wheels once balanced + context.startAsync(new SetCross()); + + context.releaseOwnership(Robot.drive); + context.releaseOwnership(Robot.gyro); + } + + // Sets state in state machine, see more details in GyroBalance.md + private void setState() { + if (prevState == State.GROUND && tilt > LEVEL) { + curState = State.RAMP_TRANSITION; + absSpeed = SPEED_TRANSITION; + log("Transition, prevState: " + prevState + ", curState: " + curState); + } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { + curState = State.RAMP_TILT; + absSpeed = SPEED_TILT; + log("Tilt, prevState: " + prevState + ", curState: " + curState); + } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { + curState = State.RAMP_LEVEL; + // If level, sets speed to negative to correct for overshooting + absSpeed = -absSpeed; + log("Level, prevState: " + prevState + ", curState: " + curState); + } + if (curState == State.GROUND) { + absSpeed = SPEED_GROUND; + } + } + + /** + * Sets direction towards desired charge station + * If robot is level and outside of charge station boundaries, sets state to ground + * @param curY current robot x position + */ + private void setDir(double curY) { + switch (alliance) { + case Red: + // If to the right of the charge station, go left + if (curY > ChargeConstants.RED_BALANCE_TARGET_X) { + // If level and outside of charge station boundaries, set state to ground + if (tilt < LEVEL && curY > ChargeConstants.RED_RIGHT_PT) { + curState = State.GROUND; + } + direction = Direction.LEFT; + // If to the left of the charge station, go right + } else { + if (tilt < LEVEL && curY < ChargeConstants.RED_LEFT_PT) { + curState = State.GROUND; + } + direction = Direction.RIGHT; + } + break; + case Blue: + // Same logic for blue alliance coordinates + if (curY > ChargeConstants.BLUE_BALANCE_TARGET_X) { + if (tilt < LEVEL && curY > ChargeConstants.BLUE_RIGHT_PT) { + curState = State.GROUND; + } + direction = Direction.LEFT; + } else { + if (tilt < LEVEL && curY < ChargeConstants.BLUE_LEFT_PT) { + curState = State.GROUND; + } + direction = Direction.RIGHT; + } + break; + default: + log("Invalid alliance"); + } + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java index ce045141b..e33ef40e4 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIdle.java @@ -5,9 +5,9 @@ import com.team766.robot.gatorade.Robot; public class IntakeIdle extends Procedure { - public void run(Context context) { - context.takeOwnership(Robot.intake); + public void run(Context context) { + context.takeOwnership(Robot.intake); - Robot.intake.idle(); - } + Robot.intake.idle(); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java index e7da25948..ba54148cd 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeIn.java @@ -4,9 +4,9 @@ import com.team766.framework.Procedure; import com.team766.robot.gatorade.Robot; -public class IntakeIn extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - Robot.intake.in(); - } +public class IntakeIn extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.intake); + Robot.intake.in(); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java index aab4a8592..3acad4aeb 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeOut.java @@ -4,9 +4,9 @@ import com.team766.framework.Procedure; import com.team766.robot.gatorade.Robot; -public class IntakeOut extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - Robot.intake.out(); - } -} \ No newline at end of file +public class IntakeOut extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.intake); + Robot.intake.out(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java index e4b850c46..f884e1294 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeStop.java @@ -4,9 +4,9 @@ import com.team766.framework.Procedure; import com.team766.robot.gatorade.Robot; -public class IntakeStop extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - Robot.intake.stop(); - } +public class IntakeStop extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.intake); + Robot.intake.stop(); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java index 38f440795..25d45df6d 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java @@ -7,30 +7,30 @@ import com.team766.robot.gatorade.mechanisms.Wrist; public class MoveWristvator extends Procedure { - private final Elevator.Position elevatorSetpoint; - private final Wrist.Position wristSetpoint; + private final Elevator.Position elevatorSetpoint; + private final Wrist.Position wristSetpoint; - public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) { - this.elevatorSetpoint = elevatorSetpoint_; - this.wristSetpoint = wristSetpoint_; - } + public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) { + this.elevatorSetpoint = elevatorSetpoint_; + this.wristSetpoint = wristSetpoint_; + } - @Override - public final void run(Context context) { - context.takeOwnership(Robot.wrist); - context.takeOwnership(Robot.elevator); + @Override + public final void run(Context context) { + context.takeOwnership(Robot.wrist); + context.takeOwnership(Robot.elevator); - // Always retract the wrist before moving the elevator. - // It might already be retracted, so it's possible that this step finishes instantaneously. - Robot.wrist.rotate(Wrist.Position.RETRACTED); - context.waitFor(() -> Robot.wrist.isNearTo(Wrist.Position.RETRACTED)); + // Always retract the wrist before moving the elevator. + // It might already be retracted, so it's possible that this step finishes instantaneously. + Robot.wrist.rotate(Wrist.Position.RETRACTED); + context.waitFor(() -> Robot.wrist.isNearTo(Wrist.Position.RETRACTED)); - // Move the elevator. Wait until it gets near the target position. - Robot.elevator.moveTo(elevatorSetpoint); - context.waitFor(() -> Robot.elevator.isNearTo(elevatorSetpoint)); + // Move the elevator. Wait until it gets near the target position. + Robot.elevator.moveTo(elevatorSetpoint); + context.waitFor(() -> Robot.elevator.isNearTo(elevatorSetpoint)); - // Lastly, move the wrist. - Robot.wrist.rotate(wristSetpoint); - context.waitFor(() -> Robot.wrist.isNearTo(wristSetpoint)); - } + // Lastly, move the wrist. + Robot.wrist.rotate(wristSetpoint); + context.waitFor(() -> Robot.wrist.isNearTo(wristSetpoint)); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java b/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java index 8d3e4631c..45698f51e 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java @@ -7,17 +7,18 @@ public class OPECHelper extends Procedure { - private static final double DIST = 4; + private static final double DIST = 4; - public void run(Context context) { - context.takeOwnership(Robot.drive); - // context.takeOwnership(Robot.intake); - double startY = Robot.drive.getCurrentPosition().getY(); - // robot gyro is offset 90º from how we want, so we reset it to 90º to account for this - Robot.gyro.resetGyro(); - // new IntakeRelease().run(context); - Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0); - context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST); - Robot.drive.stopDrive(); - } + public void run(Context context) { + context.takeOwnership(Robot.drive); + // context.takeOwnership(Robot.intake); + double startY = Robot.drive.getCurrentPosition().getY(); + // robot gyro is offset 90º from how we want, so we reset it to 90º to account for this + Robot.gyro.resetGyro(); + // new IntakeRelease().run(context); + Robot.drive.controlFieldOriented( + Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0); + context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST); + Robot.drive.stopDrive(); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java index 2f5628cf4..28e7d0481 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java @@ -4,28 +4,35 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; - import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; public class OnePieceBalance extends Procedure { - public void run(Context context) { - context.takeOwnership(Robot.drive); - //context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(2.7, 14.5)); - break; - default: - log("invalid alliance"); - return; + public void run(Context context) { + context.takeOwnership(Robot.drive); + // context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.gyro); + Robot.gyro.resetGyro(); - } - // new IntakeRelease().run(context); - new GyroBalance(DriverStation.getAlliance()).run(context); - } + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + switch (alliance.get()) { + case Blue: + Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); + break; + case Red: + Robot.drive.setCurrentPosition(new PointDir(2.7, 14.5)); + break; + default: + log("invalid alliance"); + return; + } + } else { + log("invalid alliance"); + return; + } + // new IntakeRelease().run(context); + new GyroBalance(alliance.get()).run(context); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java index 9a9242621..19d609ece 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java @@ -4,27 +4,35 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; - import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; public class OnePieceExitCommunity extends Procedure { - public void run (Context context) { - context.takeOwnership(Robot.drive); - //context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); - break; - default: - log("invalid alliance"); - return; - } - log("exiting"); - new OPECHelper().run(context); - } -} \ No newline at end of file + public void run(Context context) { + context.takeOwnership(Robot.drive); + // context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.gyro); + Robot.gyro.resetGyro(); + + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + switch (alliance.get()) { + case Blue: + Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); + break; + case Red: + Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); + break; + default: + log("invalid alliance"); + return; + } + } else { + log("invalid alliance"); + return; + } + log("exiting"); + new OPECHelper().run(context); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java index 8e81519a0..6baf0ae02 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java @@ -4,30 +4,37 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; - import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; public class OnePieceExitCommunityBalance extends Procedure { - public void run (Context context) { - context.takeOwnership(Robot.drive); - //context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(2.7, 14.5)); - break; - default: - log("invalid alliance"); - return; - } - log("exiting"); - new OPECHelper().run(context); - log("Transitioning"); - new GyroBalance(DriverStation.getAlliance()).run(context); - } - + public void run(Context context) { + context.takeOwnership(Robot.drive); + // context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.gyro); + Robot.gyro.resetGyro(); + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isPresent()) { + switch (alliance.get()) { + case Blue: + Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); + break; + case Red: + Robot.drive.setCurrentPosition(new PointDir(2.7, 14.5)); + break; + default: + log("invalid alliance"); + return; + } + } else { + log("invalid alliance"); + return; + } + log("exiting"); + new OPECHelper().run(context); + log("Transitioning"); + new GyroBalance(alliance.get()).run(context); // already checked, alliance should be set + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/README b/src/main/java/com/team766/robot/gatorade/procedures/README new file mode 100644 index 000000000..fd6888a51 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/README @@ -0,0 +1 @@ +Add procedure classes here \ No newline at end of file diff --git a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java index 1feb332a8..5323e13f2 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java @@ -5,7 +5,7 @@ public class RetractWristvator extends MoveWristvator { - public RetractWristvator() { - super(Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); - } + public RetractWristvator() { + super(Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); + } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java b/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java index 34e4e9ad4..aac107641 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/SetCross.java @@ -4,12 +4,11 @@ import com.team766.framework.Procedure; import com.team766.robot.gatorade.Robot; -public class SetCross extends Procedure { +public class SetCross extends Procedure { - public void run(Context context) { - context.takeOwnership(Robot.drive); - Robot.drive.stopDrive(); - Robot.drive.setCross(); - } - + public void run(Context context) { + context.takeOwnership(Robot.drive); + Robot.drive.stopDrive(); + Robot.drive.setCross(); + } } From 8f12b14842cd00ad97a739b83966f08533273ad4 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Tue, 9 Jan 2024 02:05:30 -0800 Subject: [PATCH 3/3] update latest from 2023-Gatorade --- .../robot/gatorade/AutonomousModes.java | 10 +- .../java/com/team766/robot/gatorade/OI.java | 150 +++++------ .../com/team766/robot/gatorade/Robot.java | 2 + .../gatorade/constants/ConfigConstants.java | 11 + .../gatorade/constants/InputConstants.java | 18 +- .../robot/gatorade/mechanisms/Drive.java | 242 ++++-------------- .../robot/gatorade/mechanisms/Elevator.java | 35 +-- .../gatorade/mechanisms/EncoderUtils.java | 21 +- .../robot/gatorade/mechanisms/Intake.java | 2 +- .../robot/gatorade/mechanisms/Lights.java | 7 +- .../robot/gatorade/mechanisms/Shoulder.java | 200 +++++++++++++++ .../gatorade/mechanisms/SwerveModule.java | 105 ++++++++ .../robot/gatorade/mechanisms/Wrist.java | 22 +- .../procedures/ChargeStationPathFinder.java | 104 -------- .../gatorade/procedures/ExitCommunity.java | 20 ++ .../procedures/ExtendToHumanWithIntake.java | 23 ++ .../procedures/ExtendWristvatorToHigh.java | 3 +- .../procedures/ExtendWristvatorToHuman.java | 6 +- .../procedures/ExtendWristvatorToLow.java | 4 +- .../procedures/ExtendWristvatorToMid.java | 3 +- .../gatorade/procedures/GyroBalance.java | 39 +-- .../gatorade/procedures/MoveWristvator.java | 19 +- .../gatorade/procedures/OnePieceBalance.java | 12 +- .../procedures/OnePieceExitCommunity.java | 18 +- .../OnePieceExitCommunityBalance.java | 15 +- .../procedures/RetractWristvator.java | 3 +- .../RetractWristvatorIdleIntake.java | 11 + .../robot/gatorade/procedures/ScoreHigh.java | 24 ++ 28 files changed, 668 insertions(+), 461 deletions(-) create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java create mode 100644 src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java delete mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java create mode 100644 src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java diff --git a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java index 1e9da0972..2f84aa4d8 100644 --- a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java +++ b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java @@ -1,6 +1,7 @@ package com.team766.robot.gatorade; import com.team766.framework.AutonomousMode; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import com.team766.robot.gatorade.procedures.*; public class AutonomousModes { @@ -16,10 +17,13 @@ public class AutonomousModes { // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), // new AutonomousMode("FollowPoints", () -> new FollowPoints()), // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), - // new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), + // new AutonomousMode("ScoreHighCube", () -> new ScoreHighCube(GamePieceType.CUBE)), + // new AutonomousMode("OnePieceExitCommunity", () -> new + // OnePieceExitCommunity(GamePieceType.CUBE)), + // new AutonomousMode("OnePieceExitCommunityBalance", () -> new + // OnePieceExitCommunityBalance(GamePieceType.CUBE)), new AutonomousMode( - "OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), - // AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), + "OnePieceBalanceCube", () -> new OnePieceBalance(GamePieceType.CUBE)), // new AutonomousMode("FollowPointsFile", () -> new // FollowPoints("FollowPoints.json")), // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index 68e775a7a..5cc924c9e 100644 --- a/src/main/java/com/team766/robot/gatorade/OI.java +++ b/src/main/java/com/team766/robot/gatorade/OI.java @@ -109,7 +109,7 @@ public void run(Context context) { > 0) { context.takeOwnership(Robot.drive); // If a button is pressed, drive is just fine adjustment - if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { + if (rightJoystick.getButton(InputConstants.FINE_DRIVING)) { Robot.drive.controlFieldOriented( Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), @@ -133,24 +133,29 @@ public void run(Context context) { if (boxopGamepad.getPOV() == InputConstants.POV_UP) { new GoForCones().run(context); setLightsForGamePiece(); + SmartDashboard.putBoolean("Game Piece", true); } else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) { new GoForCubes().run(context); setLightsForGamePiece(); + SmartDashboard.putBoolean("Game Piece", false); } // look for button presses to queue placement of intake/wrist/elevator superstructure - if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { + if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) { + placementPosition = PlacementPosition.NONE; + // setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { placementPosition = PlacementPosition.LOW_NODE; - setLightsForPlacement(); + // setLightsForPlacement(); } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) { placementPosition = PlacementPosition.MID_NODE; - setLightsForPlacement(); + // setLightsForPlacement(); } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) { placementPosition = PlacementPosition.HIGH_NODE; - setLightsForPlacement(); + // setLightsForPlacement(); } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) { placementPosition = PlacementPosition.HUMAN_PLAYER; - setLightsForPlacement(); + // setLightsForPlacement(); } // look for button hold to start intake, release to idle intake @@ -158,16 +163,8 @@ public void run(Context context) { new IntakeIn().run(context); } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) { new IntakeIdle().run(context); - } - - // see if we should reset the button states - if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) { - // stop the intake + } else if (boxopGamepad.getButton(InputConstants.BUTTON_INTAKE_STOP)) { new IntakeStop().run(context); - // reset the placement position - placementPosition = PlacementPosition.NONE; - // reset the cone/cube selection to cones - new GoForCones().run(context); } // look for button hold to extend intake/wrist/elevator superstructure, @@ -187,7 +184,7 @@ public void run(Context context) { break; case HUMAN_PLAYER: context.startAsync( - new ExtendWristvatorToHuman(Robot.intake.getGamePieceType())); + new ExtendToHumanWithIntake(Robot.intake.getGamePieceType())); break; default: // warn, ignore @@ -197,84 +194,59 @@ public void run(Context context) { break; } } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - context.startAsync(new RetractWristvator()); + if (placementPosition == PlacementPosition.HUMAN_PLAYER) { + context.startAsync(new RetractWristvatorIdleIntake()); + } else { + context.startAsync(new RetractWristvator()); + } } - // TODO: refactor this code. it's getting gnarly. // look for manual nudges // we only allow these if the extend elevator trigger is extended if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - // the y axis is flipped from what we expect. invert so up is positive, down is - // negative. + + // look for elevator nudges double elevatorNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT); - double wristNudgeAxis = - -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); - - if (boxopGamepad.getButtonPressed( - InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { - // bypass PID - if (Math.abs(elevatorNudgeAxis) > 0.05) { - elevatorManual = true; - context.takeOwnership(Robot.elevator); - Robot.elevator.nudgeNoPID(elevatorNudgeAxis); - context.releaseOwnership(Robot.elevator); - } else if (elevatorManual) { - context.takeOwnership(Robot.elevator); - Robot.elevator.stopElevator(); - context.releaseOwnership(Robot.elevator); - elevatorManual = false; - } - - if ((Math.abs(wristNudgeAxis) > 0.05)) { - wristManual = true; - context.takeOwnership(Robot.wrist); - Robot.wrist.nudgeNoPID(wristNudgeAxis); - context.releaseOwnership(Robot.wrist); - } else if (wristManual) { - context.takeOwnership(Robot.wrist); - Robot.wrist.stopWrist(); - context.releaseOwnership(Robot.wrist); - elevatorManual = false; - } - } else if (boxopGamepad.getButtonReleased( - InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { - context.takeOwnership(Robot.wrist); + if (Math.abs(elevatorNudgeAxis) > 0.05) { + // elevatorManual = true; context.takeOwnership(Robot.elevator); - Robot.wrist.resetEncoder(); - Robot.elevator.resetEncoder(); - context.releaseOwnership(Robot.wrist); - context.releaseOwnership(Robot.elevator); - } else { - // look for elevator nudges - if (Math.abs(elevatorNudgeAxis) > 0.05) { - context.takeOwnership(Robot.elevator); - if (elevatorNudgeAxis > 0) { - Robot.elevator.nudgeUp(); - } else { - Robot.elevator.nudgeDown(); - } - context.releaseOwnership(Robot.elevator); + // Robot.elevator.nudgeNoPID(elevatorNudgeAxis); + if (elevatorNudgeAxis > 0) { + Robot.elevator.nudgeUp(); + } else { + Robot.elevator.nudgeDown(); } + context.releaseOwnership(Robot.elevator); + } else if (false && elevatorManual) { + Robot.elevator.stopElevator(); + elevatorManual = false; + } - // look for wrist nudges - if (Math.abs(wristNudgeAxis) > 0.05) { - context.takeOwnership(Robot.wrist); - if (wristNudgeAxis > 0) { - Robot.wrist.nudgeUp(); - } else { - Robot.wrist.nudgeDown(); - } - context.releaseOwnership(Robot.wrist); + // look for wrist nudges + double wristNudgeAxis = + -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); + if (Math.abs(wristNudgeAxis) > 0.05) { + // wristManual = true; + context.takeOwnership(Robot.wrist); + // Robot.wrist.nudgeNoPID(wristNudgeAxis); + if (wristNudgeAxis > 0) { + Robot.wrist.nudgeUp(); + } else { + Robot.wrist.nudgeDown(); } + context.releaseOwnership(Robot.wrist); + } else if (false && wristManual) { + Robot.wrist.stopWrist(); + wristManual = true; } } if (lightsRateLimit.next()) { - if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) { + if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 17) { Robot.lights.rainbow(); } else { - setLightsForPlacement(); + setLightsForGamePiece(); } } } @@ -282,18 +254,18 @@ public void run(Context context) { private void setLightsForPlacement() { switch (placementPosition) { - case NONE: - Robot.lights.white(); - break; - case LOW_NODE: - Robot.lights.green(); - break; - case MID_NODE: - Robot.lights.red(); - break; - case HIGH_NODE: - Robot.lights.orange(); - break; + // case NONE: + // Robot.lights.white(); + // break; + // case LOW_NODE: + // Robot.lights.green(); + // break; + // case MID_NODE: + // Robot.lights.red(); + // break; + // case HIGH_NODE: + // Robot.lights.orange(); + // break; case HUMAN_PLAYER: setLightsForGamePiece(); break; diff --git a/src/main/java/com/team766/robot/gatorade/Robot.java b/src/main/java/com/team766/robot/gatorade/Robot.java index e40383f33..5881f5b7f 100644 --- a/src/main/java/com/team766/robot/gatorade/Robot.java +++ b/src/main/java/com/team766/robot/gatorade/Robot.java @@ -7,6 +7,7 @@ public class Robot { public static Intake intake; public static Wrist wrist; public static Elevator elevator; + public static Shoulder shoulder; public static Drive drive; public static Gyro gyro; public static Lights lights; @@ -16,6 +17,7 @@ public static void robotInit() { intake = new Intake(); wrist = new Wrist(); elevator = new Elevator(); + shoulder = new Shoulder(); drive = new Drive(); gyro = new Gyro(); lights = new Lights(); diff --git a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java index 8d20e4935..e86573c84 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java @@ -36,4 +36,15 @@ private ConfigConstants() {} public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity"; public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity"; public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel"; + + // shoulder config values + public static final String SHOULDER_LEFT_MOTOR = "shoulder.leftMotor"; + public static final String SHOULDER_RIGHT_MOTOR = "shoulder.rightMotor"; + public static final String SHOULDER_PGAIN = "shoulder.sparkPID.pGain"; + public static final String SHOULDER_IGAIN = "shoulder.sparkPID.iGain"; + public static final String SHOULDER_DGAIN = "shoulder.sparkPID.dGain"; + public static final String SHOULDER_FFGAIN = "shoulder.sparkPID.ffGain"; + public static final String SHOULDER_MAX_VELOCITY = "shoulder.sparkPID.maxVelocity"; + public static final String SHOULDER_MIN_OUTPUT_VELOCITY = "shoulder.sparkPID.minOutputVelocity"; + public static final String SHOULDER_MAX_ACCEL = "shoulder.sparkPID.maxAccel"; } diff --git a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java index c5572ecbe..ede98047e 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java @@ -24,24 +24,24 @@ public final class InputConstants { // Boxop Gamepad Buttons - // LT - public static final int BUTTON_INTAKE_IN = 7; // RT - public static final int BUTTON_EXTEND_WRISTVATOR = 8; + public static final int BUTTON_INTAKE_IN = 8; + // LT + public static final int BUTTON_EXTEND_WRISTVATOR = 7; // Start - public static final int BUTTON_RESET_STATE = 10; + public static final int BUTTON_INTAKE_STOP = 10; // used for development - // left axis - public static final int AXIS_WRIST_MOVEMENT = 1; // right axis - public static final int AXIS_ELEVATOR_MOVEMENT = 3; + public static final int AXIS_WRIST_MOVEMENT = 3; + // left axis + public static final int AXIS_ELEVATOR_MOVEMENT = 1; // pov public static final int POV_UP = 0; public static final int POV_DOWN = 180; - // LB - public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5; + // RB + public static final int BUTTON_PLACEMENT_NONE = 6; // X/A/B/Y public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X public static final int BUTTON_PLACEMENT_HIGH = 4; // Y diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java index d8c14b4ac..41e314675 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java @@ -5,7 +5,6 @@ import com.ctre.phoenix.sensors.CANCoder; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; -import com.team766.hal.MotorController.ControlMode; import com.team766.hal.RobotProvider; import com.team766.logging.Category; import com.team766.odometry.Odometry; @@ -18,40 +17,11 @@ public class Drive extends Mechanism { - // Drive motors - private MotorController m_DriveFR; - private MotorController m_DriveFL; - private MotorController m_DriveBR; - private MotorController m_DriveBL; - - // Motors to turn each wheel in place - private MotorController m_SteerFR; - private MotorController m_SteerFL; - private MotorController m_SteerBR; - private MotorController m_SteerBL; - - // Absolute encoders (cancoders) - private CANCoder e_FrontRight; - private CANCoder e_FrontLeft; - private CANCoder e_BackRight; - private CANCoder e_BackLeft; - - /* - * Specific offsets to align each wheel - * These are assigned upon startup with setEncoderOffset() - */ - private double offsetFR; - private double offsetFL; - private double offsetBR; - private double offsetBL; - - /* - * Factor that converts between motor units and degrees - * Multiply to convert from degrees to motor units - * Divide to convert from motor units to degrees - */ - private final double ENCODER_CONVERSION_FACTOR = - (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + // SwerveModules + private final SwerveModule swerveFR; + private final SwerveModule swerveFL; + private final SwerveModule swerveBR; + private final SwerveModule swerveBL; // TODO: rework odometry so it doesn't have to go through drive @@ -60,49 +30,38 @@ public class Drive extends Mechanism { // variable representing current position private static PointDir currentPosition; - // other variables to set up odometry - private MotorController[] motorList; - private CANCoder[] CANCoderList; - private Point[] wheelPositions; - public Drive() { - loggerCategory = Category.DRIVE; - // Initializations of motors - // Initialize the drive motors - m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); - m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); - m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); - m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); - - // Initialize the steering motors - m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); - m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); - m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); - m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); - - // Initialize the encoders - e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); - e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); - e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); - e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); - - // Current limit for motors to avoid breaker problems - m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveFL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_SteerFR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerFL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + // create the drive motors + MotorController driveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + MotorController driveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + MotorController driveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + MotorController driveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); + + // create the steering motors + MotorController steerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + MotorController steerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + MotorController steerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + MotorController steerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); + + // create the encoders + CANCoder encoderFR = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderFL = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBR = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBL = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); + + // initialize the swerve modules + swerveFR = new SwerveModule("FR", driveFR, steerFR, encoderFR); + swerveFL = new SwerveModule("FL", driveFL, steerFL, encoderFL); + swerveBR = new SwerveModule("BR", driveBR, steerBR, encoderBR); + swerveBL = new SwerveModule("BL", driveBL, steerBL, encoderBL); // Sets up odometry currentPosition = new PointDir(0, 0, 0); - motorList = new MotorController[] {m_DriveFR, m_DriveFL, m_DriveBL, m_DriveBR}; - CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; - wheelPositions = + MotorController[] motorList = new MotorController[] {driveFR, driveFL, driveBL, driveBR}; + CANCoder[] encoderList = new CANCoder[] {encoderFR, encoderFL, encoderBL, encoderBR}; + Point[] wheelPositions = new Point[] { new Point( OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, @@ -118,70 +77,16 @@ public Drive() { OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2) }; log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + CANCoderList.length); + log("CANCoderList Length: " + encoderList.length); swerveOdometry = new Odometry( motorList, - CANCoderList, + encoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - - // Sets the offset value for each steering motor so that each is aligned - setEncoderOffset(); - } - - /** - * Sets just the steer for a specific module - * Can be used to turn the wheels without moving - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteer(MotorController steerMotor, Vector2D vector, double offset) { - - // Calculates the angle of the vector from -180° to 180° - final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); - - // Add 360 * number of full rotations to vectorTheta, then add offset - final double angleDegrees = - vectorTheta - + 360 - * (Math.round( - (steerMotor.getSensorPosition() / ENCODER_CONVERSION_FACTOR - - offset - - vectorTheta) - / 360)) - + offset; - - // Sets the degree of the steer wheel - // Needs to multiply by encoderconversionfactor to translate into a unit the motor - // understands - steerMotor.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees); - - SmartDashboard.putNumber("Angle", angleDegrees); - } - - /** - * Sets the motion for a specific module - * @param driveMotor the drive motor of this module - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteerAndPower( - MotorController driveMotor, - MotorController steerMotor, - Vector2D vector, - double offset) { - checkContextOwnership(); - - controlModuleSteer(steerMotor, vector, offset); - - // Sets the power to the magnitude of the vector - driveMotor.set(vector.getNorm()); } /** @@ -191,44 +96,34 @@ public void controlModuleSteerAndPower( * @param turn the turn value from the rotation joystick */ public void controlRobotOriented(double x, double y, double turn) { + checkContextOwnership(); + // Finds the vectors for turning and for translation of each module, and adds them // Applies this for each module - controlModuleSteerAndPower( - m_DriveFL, - m_SteerFL, + swerveFL.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X) - .normalize()), - offsetFL); - controlModuleSteerAndPower( - m_DriveFR, - m_SteerFR, + .normalize())); + swerveFR.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X) - .normalize()), - offsetFR); - controlModuleSteerAndPower( - m_DriveBR, - m_SteerBR, + .normalize())); + swerveBR.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X) - .normalize()), - offsetBR); - controlModuleSteerAndPower( - m_DriveBL, - m_SteerBL, + .normalize())); + swerveBL.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X) - .normalize()), - offsetBL); + .normalize())); } /** @@ -239,6 +134,8 @@ public void controlRobotOriented(double x, double y, double turn) { * @param turn the turn value from the rotation joystick */ public void controlFieldOriented(double yawRad, double x, double y, double turn) { + checkContextOwnership(); + // Applies a rotational translation to controlRobotOriented // Counteracts the forward direction changing when the robot turns // TODO: change to inverse rotation matrix (rather than negative angle) @@ -248,56 +145,27 @@ public void controlFieldOriented(double yawRad, double x, double y, double turn) turn); } - /* - * Compares the absolute encoder to the motor encoder to find each motor's offset - * This helps each wheel to always be aligned - */ - public void setEncoderOffset() { - offsetFR = - (m_SteerFR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_FrontRight.getAbsolutePosition(); - offsetFL = - (m_SteerFL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_FrontLeft.getAbsolutePosition(); - offsetBR = - (m_SteerBR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_BackRight.getAbsolutePosition(); - offsetBL = - (m_SteerBL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_BackLeft.getAbsolutePosition(); - } - /* * Stops each drive motor */ public void stopDrive() { checkContextOwnership(); - m_DriveFR.stopMotor(); - m_DriveFL.stopMotor(); - m_DriveBR.stopMotor(); - m_DriveBL.stopMotor(); + swerveFR.stopDrive(); + swerveFL.stopDrive(); + swerveBR.stopDrive(); + swerveBL.stopDrive(); } /* * Turns wheels in a cross formation to prevent robot from moving */ public void setCross() { - controlModuleSteer( - m_SteerFL, - new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X), - offsetFL); - controlModuleSteer( - m_SteerFR, - new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X), - offsetFR); - controlModuleSteer( - m_SteerBL, - new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X), - offsetBL); - controlModuleSteer( - m_SteerBR, - new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X), - offsetBR); + checkContextOwnership(); + + swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); + swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); + swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); + swerveBR.steer(new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X)); } // TODO: rework odometry so it doesn't have to go through drive diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java index bb5b2660c..47efa290b 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java @@ -2,6 +2,7 @@ import static com.team766.robot.gatorade.constants.ConfigConstants.*; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.SparkMaxPIDController; @@ -15,21 +16,25 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** + * Basic elevator mechanism. Used in conjunction with the {@link Intake} and {@link Wrist}. + * Can be moved up and down as part of teleop or autonomous control to move the {@link Wrist} + * and {@link Intake} closer to a game piece or game element (eg node in the + * field, human player station). + */ public class Elevator extends Mechanism { public enum Position { - // TODO: do we need separate heights for cones vs cubes? - - /** Elevator is fully retracted. */ + /** Elevator is fully retracted. Starting position. */ RETRACTED(0), /** Elevator is the appropriate height to place game pieces at the low node. */ - LOW(5), + LOW(0), /** Elevator is the appropriate height to place game pieces at the mid node. */ MID(18), /** Elevator is at appropriate height to place game pieces at the high node. */ HIGH(40), /** Elevator is at appropriate height to grab cubes from the human player. */ - HUMAN_CUBES(40), + HUMAN_CUBES(39), /** Elevator is at appropriate height to grab cones from the human player. */ HUMAN_CONES(40), /** Elevator is fully extended. */ @@ -46,7 +51,7 @@ private double getHeight() { } } - private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_INCREMENT = 2.0; private static final double NUDGE_DAMPENER = 0.25; private static final double NEAR_THRESHOLD = 2.0; @@ -76,12 +81,18 @@ public Elevator() { throw new IllegalStateException("Motor are not CANSparkMaxes!"); } + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + leftMotor = (CANSparkMax) halLeftMotor; rightMotor = (CANSparkMax) halRightMotor; - rightMotor.follow(leftMotor, true); + rightMotor.follow(leftMotor, true /* invert */); - resetEncoder(); + leftMotor + .getEncoder() + .setPosition( + EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); pidController = leftMotor.getPIDController(); pidController.setFeedbackDevice(leftMotor.getEncoder()); @@ -95,13 +106,6 @@ public Elevator() { maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL); } - public void resetEncoder() { - leftMotor - .getEncoder() - .setPosition( - EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); - } - public double getRotations() { return leftMotor.getEncoder().getPosition(); } @@ -139,7 +143,6 @@ public void nudgeUp() { double height = getHeight(); // NOTE: this could artificially limit nudge range double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight()); - System.err.println("Target: " + targetHeight); moveTo(targetHeight); } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java index febf753f4..731ce300b 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java @@ -16,7 +16,6 @@ private EncoderUtils() {} */ public static double wristDegreesToRotations(double angle) { // angle * net gear ratio * (rotations / degrees) - // FIXME: replace 32 with actual # of teeth return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.); } @@ -25,7 +24,6 @@ public static double wristDegreesToRotations(double angle) { */ public static double wristRotationsToDegrees(double rotations) { // rotations * net gear ratio * (degrees / rotations) - // FIXME: replace 32 with actual # of teeth return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.); } @@ -42,10 +40,25 @@ public static double elevatorHeightToRotations(double height) { */ public static double elevatorRotationsToHeight(double rotations) { // rotations * net gear ratio * (height / rotations) - // FIXME: everything return rotations * (12. / 36.) * ((1.641 * Math.PI) / 1.); } + /** + * Converts a target rotation (in degrees) to encoder units for the shoulder motor. + */ + public static double shoulderDegreesToRotations(double angle) { + // angle * sprocket ratio * net gear ratio * (rotations / degrees) + return angle * (52.0 / 12.0) * (64.0 / 30.0) * (4. / 1.) * (3. / 1.) * (1. / 360.); + } + + /** + * Converts the shoulder motor's rotations to degrees. + */ + public static double shoulderRotationsToDegrees(double rotations) { + // rotations * sprocket ratio * net gear ratio * (degrees / rotations) + return rotations * (12.0 / 52.0) * (30.0 / 64.0) * (1. / 4.) * (1. / 3.) * (360. / 1.); + } + /** * Cosine law * @param side1 @@ -57,7 +70,7 @@ public static double lawOfCosines(double side1, double side2, double angle) { double side3Squared = (Math.pow(side1, 2.0) + Math.pow(side2, 2.0) - - 2 * side1 * side2 * Math.cos(Math.toRadians(angle))); + - (2 * side1 * side2 * Math.cos(Math.toRadians(angle)))); return Math.sqrt(side3Squared); } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java index ff32fd344..7e42a32eb 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java @@ -23,7 +23,7 @@ public class Intake extends Mechanism { private static final double POWER_IN = 0.3; private static final double POWER_OUT = 0.25; - private static final double POWER_IDLE = 0.02; + private static final double POWER_IDLE = 0.05; /** * The current type of game piece the Intake is preparing to hold or is holding. diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java index 7a7afba0a..d4dc89e04 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java @@ -4,16 +4,17 @@ import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.RainbowAnimation; import com.team766.framework.Mechanism; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; public class Lights extends Mechanism { private CANdle candle; private static final int CANID = 5; private static final int LED_COUNT = 90; - private Animation rainbowAnimation = new RainbowAnimation(1, 0.1, LED_COUNT); + private Animation rainbowAnimation = new RainbowAnimation(1, 1.5, LED_COUNT); public Lights() { - candle = new CANdle(CANID); + candle = new CANdle(CANID, SwerveDriveConstants.SWERVE_CANBUS); } public void purple() { @@ -29,7 +30,7 @@ public void white() { public void yellow() { checkContextOwnership(); - candle.setLEDs(255, 255, 0); + candle.setLEDs(255, 150, 0); } public void red() { diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java new file mode 100644 index 000000000..e84a7d49d --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java @@ -0,0 +1,200 @@ +package com.team766.robot.gatorade.mechanisms; + +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.SparkMaxPIDController; +import com.team766.config.ConfigFileReader; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Basic shoulder mechanism. Rotates the {@link Elevator} to different angles, to allow it (and the + * attached {@link Wrist} and {@link Intake}) to reach different positions, from the floor to different + * heights of nodes. + */ +public class Shoulder extends Mechanism { + + /** + * Pre-set positions for the shoulder. + */ + public enum Position { + + // TODO: adjust these! + + /** Shoulder is at the highest achievable position. */ + TOP(45), + + /** Shoulder is in position to intake from the substation or score in the upper nodes. */ + RAISED(40), + + /** Shoulder is in position to intake and outtake pieces from/to the floor. */ + FLOOR(10), + + /** Shoulder is fully down. Starting position. **/ + BOTTOM(0); + + private final double angle; + + Position(double angle) { + this.angle = angle; + } + + public double getAngle() { + return angle; + } + } + + private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.15; + + private static final double NEAR_THRESHOLD = 5.0; + + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final ValueProvider maxVelocity; + private final ValueProvider minOutputVelocity; + private final ValueProvider maxAccel; + + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Constructs a new Shoulder. + */ + public Shoulder() { + MotorController halLeftMotor = RobotProvider.instance.getMotor(SHOULDER_LEFT_MOTOR); + MotorController halRightMotor = RobotProvider.instance.getMotor(SHOULDER_RIGHT_MOTOR); + + if (!((halLeftMotor instanceof CANSparkMax) && (halRightMotor instanceof CANSparkMax))) { + log(Severity.ERROR, "Motors are not CANSparkMaxes!"); + throw new IllegalStateException("Motor are not CANSparkMaxes!"); + } + + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + + leftMotor = (CANSparkMax) halLeftMotor; + rightMotor = (CANSparkMax) halRightMotor; + + rightMotor.follow(leftMotor, true /* invert */); + + leftMotor + .getEncoder() + .setPosition(EncoderUtils.shoulderDegreesToRotations(Position.BOTTOM.getAngle())); + + pidController = leftMotor.getPIDController(); + pidController.setFeedbackDevice(leftMotor.getEncoder()); + + pGain = ConfigFileReader.getInstance().getDouble(SHOULDER_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(SHOULDER_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(SHOULDER_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(SHOULDER_FFGAIN); + maxVelocity = ConfigFileReader.getInstance().getDouble(SHOULDER_MAX_VELOCITY); + minOutputVelocity = ConfigFileReader.getInstance().getDouble(SHOULDER_MIN_OUTPUT_VELOCITY); + maxAccel = ConfigFileReader.getInstance().getDouble(SHOULDER_MAX_ACCEL); + } + + public double getRotations() { + return leftMotor.getEncoder().getPosition(); + } + + /** + * Returns the current angle of the wrist. + */ + public double getAngle() { + return EncoderUtils.shoulderRotationsToDegrees(leftMotor.getEncoder().getPosition()); + } + + public boolean isNearTo(Position position) { + return isNearTo(position.getAngle()); + } + + public boolean isNearTo(double angle) { + return Math.abs(angle - getAngle()) < NEAR_THRESHOLD; + } + + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + leftMotor.set(clampedValue); + } + + public void stopShoulder() { + checkContextOwnership(); + leftMotor.set(0); + } + + public void nudgeUp() { + System.err.println("Nudging up."); + double angle = getAngle(); + double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.TOP.getAngle()); + + rotate(targetAngle); + } + + public void nudgeDown() { + System.err.println("Nudging down."); + double angle = getAngle(); + double targetAngle = Math.max(angle - NUDGE_INCREMENT, Position.BOTTOM.getAngle()); + rotate(targetAngle); + } + + /** + * Rotates the wrist to a pre-set {@link Position}. + */ + public void rotate(Position position) { + rotate(position.getAngle()); + } + + /** + * Starts rotating the wrist to the specified angle. + * NOTE: this method returns immediately. Check the current wrist position of the wrist + * with {@link #getAngle()}. + */ + public void rotate(double angle) { + checkContextOwnership(); + + System.err.println("Setting target angle to " + angle); + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + // pidController.setFF(ffGain.get()); + double ff = ffGain.get() * Math.cos(Math.toRadians(angle)); + SmartDashboard.putNumber("[SHOULDER] ff", ff); + SmartDashboard.putNumber("[SHOULDER] reference", angle); + + pidController.setOutputRange(-0.4, 0.4); + + // convert the desired target degrees to rotations + double rotations = EncoderUtils.shoulderDegreesToRotations(angle); + SmartDashboard.putNumber("[SHOULDER] Setpoint", rotations); + + // set the reference point for the wrist + pidController.setReference(rotations, ControlType.kPosition, 0, ff); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); + SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations()); + SmartDashboard.putNumber("[SHOULDER] Left Effort", leftMotor.getOutputCurrent()); + SmartDashboard.putNumber("[SHOULDER] Right Effort", rightMotor.getOutputCurrent()); + } + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java new file mode 100644 index 000000000..9b37dc763 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java @@ -0,0 +1,105 @@ +package com.team766.robot.gatorade.mechanisms; + +import com.ctre.phoenix.sensors.CANCoder; +import com.team766.hal.MotorController; +import com.team766.hal.MotorController.ControlMode; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +/** + * Encapsulates the motors and encoders used for each physical swerve module and + * provides driving and steering controls for each module. + */ +public class SwerveModule { + private final String modulePlacement; + private final MotorController drive; + private final MotorController steer; + private final CANCoder encoder; + private final double offset; + + /* + * Factor that converts between motor units and degrees + * Multiply to convert from degrees to motor units + * Divide to convert from motor units to degrees + */ + private static final double ENCODER_CONVERSION_FACTOR = + (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + + /** + * Creates a new SwerveModule. + * + * @param modulePlacement String description of the placement for this module, eg "FL". + * @param drive Drive MotorController for this module. + * @param steer Steer MotorController for this module. + * @param encoder CANCoder for this module. + */ + public SwerveModule( + String modulePlacement, + MotorController drive, + MotorController steer, + CANCoder encoder) { + this.modulePlacement = modulePlacement; + this.drive = drive; + this.steer = steer; + this.encoder = encoder; + this.offset = computeEncoderOffset(); + + // Current limit for motors to avoid breaker problems + drive.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + steer.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + } + + private double computeEncoderOffset() { + return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - encoder.getAbsolutePosition(); + } + + /** + * Controls just the steer for this module. + * Can be used to turn the wheels without moving + * @param vector the vector specifying the module's motion + */ + public void steer(Vector2D vector) { + // Calculates the angle of the vector from -180° to 180° + final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); + + // Add 360 * number of full rotations to vectorTheta, then add offset + final double angleDegrees = + vectorTheta + + 360 + * (Math.round( + (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR + - offset + - vectorTheta) + / 360)) + + offset; + + // Sets the degree of the steer wheel + // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor + // understands + steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees); + + SmartDashboard.putNumber("[" + modulePlacement + "]" + "Angle", angleDegrees); + } + + /** + * Controls both steer and power (based on the target vector) for this module. + * @param vector the vector specifying the module's motion + */ + public void driveAndSteer(Vector2D vector) { + // apply the steer + steer(vector); + + // sets the power to the magnitude of the vector + // TODO: does this need to be clamped to a specific range, eg btn -1 and 1? + drive.set(vector.getNorm()); + } + + /** + * Stops the drive motor for this module. + */ + public void stopDrive() { + drive.stopMotor(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java index a6942f53e..ab740bad6 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java @@ -29,17 +29,17 @@ public class Wrist extends Mechanism { */ public enum Position { - // TODO: adjust these values. - /** Wrist is in top position. Starting position. */ - TOP(-135), + TOP(-180), /** Wrist is in the position for moving around the field. */ - RETRACTED(-120.0), + RETRACTED(-175.0), /** Wrist is level with ground. */ - LEVEL(0.0), - HIGH_NODE(-30), + LEVEL(-65), + HIGH_NODE(-20), MID_NODE(-25.5), - /** Wrist is fully down. */ + HUMAN_CONES(-4), + HUMAN_CUBES(-8), + /** Wrist is fully down. **/ BOTTOM(60); private final double angle; @@ -77,7 +77,8 @@ public Wrist() { } motor = (CANSparkMax) halMotor; - resetEncoder(); + motor.getEncoder() + .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); // stash the PIDController for convenience. will update the PID values to the latest from // the config @@ -92,11 +93,6 @@ public Wrist() { ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN); } - public void resetEncoder() { - motor.getEncoder() - .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); - } - public double getRotations() { return motor.getEncoder().getPosition(); } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java b/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java deleted file mode 100644 index 2e0f90625..000000000 --- a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java +++ /dev/null @@ -1,104 +0,0 @@ -package com.team766.robot.gatorade.procedures; - -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.odometry.PointDir; -import com.team766.robot.gatorade.constants.ChargeConstants; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import java.util.ArrayList; -import java.util.List; - -// Helper class for AlignCharger, see AlignCharger.md for more details -public class ChargeStationPathFinder { - - // Since this class does not extend Procedure, we need to instantiate logger - private static final Logger logger = Logger.get(Category.PROCEDURES); - private final Alliance alliance; - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public ChargeStationPathFinder(Alliance alliance) { - this.alliance = alliance; - } - - /** - * Calculates points that the robot needs to follow based on alliance and current position - * @param curPos the current robot position, represented by a pointDir - * @return PointDir[]: list of the calculated points that can be entered into FollowPoints - */ - public PointDir[] calculatePoints(PointDir curPos) { - - // Sets curX and curY variables based on curPos param - double curX = curPos.getX(); - double curY = curPos.getY(); - - // Creates ArrayList for points - List points = new ArrayList(); - switch (alliance) { - case Red: - // Calls addPoints method for red alliance coordinates - addPoints( - points, - curX, - curY, - ChargeConstants.RED_BALANCE_TARGET_X, - ChargeConstants.RED_LEFT_PT, - ChargeConstants.RED_RIGHT_PT, - ChargeConstants.MIDDLE); - break; - - case Blue: - // Calls addPoints method for red alliance coordinates - addPoints( - points, - curX, - curY, - ChargeConstants.BLUE_BALANCE_TARGET_X, - ChargeConstants.BLUE_LEFT_PT, - ChargeConstants.BLUE_RIGHT_PT, - ChargeConstants.MIDDLE); - break; - default: - logger.logRaw(null, "Invalid Alliance"); - } - - // Converts pointDir arrayList to array and returns it - return points.toArray(new PointDir[points.size()]); - } - - /** - * Adds a set of points to an arrayList based on passed charge station coordinates - * @param points ArrayList to add points to - * @param curX Current robot X value - * @param curY Current robot Y value - * @param target X value of center of target charge station - * @param left X value of left side of target charge station - * @param right X value of right side of target charge station - * @param height Y value of center of target charge station - */ - private void addPoints( - List points, - double curX, - double curY, - double target, - double left, - double right, - double height) { - // If on the right side of the target, check if robot is blocked by the right side of the - // charge station, and add points accordingly - if (curX > target) { - if (curX < right) points.add(new PointDir(right, curY)); - - points.add(new PointDir(right, height)); - - // Otherwise, it will be on the left side of the target, so check if robot is blocked by - // the left side of the charge station, and add points accordingly - } else { - if (curX > left) points.add(new PointDir(left, curY)); - - points.add(new PointDir(left, height)); - } - } -} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java new file mode 100644 index 000000000..16d86a292 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java @@ -0,0 +1,20 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.constants.FollowPointsInputConstants; + +public class ExitCommunity extends Procedure { + + private static final double DIST = 4; + + public void run(Context context) { + context.takeOwnership(Robot.drive); + double startY = Robot.drive.getCurrentPosition().getY(); + Robot.drive.controlFieldOriented( + Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0); + context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST); + Robot.drive.stopDrive(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java new file mode 100644 index 000000000..ff7644f00 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java @@ -0,0 +1,23 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class ExtendToHumanWithIntake extends Procedure { + private final GamePieceType gamePieceType; + + public ExtendToHumanWithIntake(GamePieceType gamePieceType) { + this.gamePieceType = gamePieceType; + } + + public void run(Context context) { + context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.wrist); + context.takeOwnership(Robot.elevator); + + new IntakeIn().run(context); + new ExtendWristvatorToHuman(gamePieceType).run(context); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java index 23c2f172a..01db3df37 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java @@ -1,11 +1,12 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToHigh extends MoveWristvator { public ExtendWristvatorToHigh() { - super(Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); + super(Shoulder.Position.RAISED, Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java index d01d999d6..cc970a5de 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java @@ -2,15 +2,19 @@ import com.team766.robot.gatorade.mechanisms.Elevator; import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToHuman extends MoveWristvator { public ExtendWristvatorToHuman(GamePieceType gamePieceType) { super( + Shoulder.Position.RAISED, gamePieceType == GamePieceType.CONE ? Elevator.Position.HUMAN_CONES : Elevator.Position.HUMAN_CUBES, - Wrist.Position.LEVEL); + gamePieceType == GamePieceType.CONE + ? Wrist.Position.HUMAN_CONES + : Wrist.Position.HUMAN_CUBES); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java index 602f8497d..486455ab5 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java @@ -1,11 +1,13 @@ package com.team766.robot.gatorade.procedures; +import com.team766.robot.gatorade.constants.*; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToLow extends MoveWristvator { public ExtendWristvatorToLow() { - super(Elevator.Position.LOW, Wrist.Position.LEVEL); + super(Shoulder.Position.FLOOR, Elevator.Position.LOW, Wrist.Position.LEVEL); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java index e34b96930..33b47090e 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java @@ -1,11 +1,12 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToMid extends MoveWristvator { public ExtendWristvatorToMid() { - super(Elevator.Position.MID, Wrist.Position.MID_NODE); + super(Shoulder.Position.RAISED, Elevator.Position.MID, Wrist.Position.MID_NODE); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java index f61647aab..20e3ac1e0 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java @@ -17,7 +17,8 @@ private enum State { GROUND, RAMP_TRANSITION, RAMP_TILT, - RAMP_LEVEL + RAMP_LEVEL, + OVERSHOOT } // Direction determines which direction the robot moves @@ -45,7 +46,9 @@ private enum Direction { private final double CORRECTION_DELAY = 0.7; private final double SPEED_GROUND = .3; private final double SPEED_TRANSITION = .25; - private final double SPEED_TILT = .18; + private final double SPEED_TILT = .12; + private final double SPEED_OVERSHOOT = .08; + private final double OVERSHOOT_INCORRECT_MULT = 0.5; /** * Constructor to create a new GyroBalance instance @@ -65,6 +68,9 @@ public void run(Context context) { // driveSpeed is actual value of speed passed into the swerveDrive method double driveSpeed = 1; + // extend wristvator to put CG in a place where robot can climb ramp + new ExtendWristvatorToMid().run(context); + // Sets movement direction ground state if on ground setDir(curY); @@ -84,7 +90,7 @@ public void run(Context context) { tilt = Robot.gyro.getAbsoluteTilt(); // log("curX:" + curX); // log("direction: " + direction); - setState(); + setState(context); // Both being on Red alliance and needing to move right would make the movement // direction negative, so this expression corrects for that @@ -101,21 +107,10 @@ public void run(Context context) { } // Loops until robot is level or until a call to the abort() method while (!(curState == State.RAMP_LEVEL)); - - // After the robot is level, drives for correctionDelay seconds. - // Direction is opposite due to inversion of speed in setState() so it corrects for - // overshooting - context.waitForSeconds(CORRECTION_DELAY); - - // Locks wheels once balanced - context.startAsync(new SetCross()); - - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); } // Sets state in state machine, see more details in GyroBalance.md - private void setState() { + private void setState(Context context) { if (prevState == State.GROUND && tilt > LEVEL) { curState = State.RAMP_TRANSITION; absSpeed = SPEED_TRANSITION; @@ -123,12 +118,24 @@ private void setState() { } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { curState = State.RAMP_TILT; absSpeed = SPEED_TILT; + context.startAsync(new RetractWristvator()); log("Tilt, prevState: " + prevState + ", curState: " + curState); } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { - curState = State.RAMP_LEVEL; + curState = State.OVERSHOOT; // If level, sets speed to negative to correct for overshooting + absSpeed = SPEED_OVERSHOOT; absSpeed = -absSpeed; + log("Overshoot, prevState: " + prevState + ", curState: " + curState); + } else if (prevState == State.OVERSHOOT && tilt < LEVEL) { + context.startAsync(new SetCross()); log("Level, prevState: " + prevState + ", curState: " + curState); + context.waitForSeconds(1); + tilt = Robot.gyro.getAbsoluteTilt(); + if (tilt < LEVEL) { + curState = State.RAMP_LEVEL; + } else { + absSpeed *= -OVERSHOOT_INCORRECT_MULT; + } } if (curState == State.GROUND) { absSpeed = SPEED_GROUND; diff --git a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java index 25d45df6d..7053ed7c1 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java @@ -4,13 +4,19 @@ import com.team766.framework.Procedure; import com.team766.robot.gatorade.Robot; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class MoveWristvator extends Procedure { + private final Shoulder.Position shoulderSetpoint; private final Elevator.Position elevatorSetpoint; private final Wrist.Position wristSetpoint; - public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) { + public MoveWristvator( + Shoulder.Position shoulderSetpoint_, + Elevator.Position elevatorSetpoint_, + Wrist.Position wristSetpoint_) { + this.shoulderSetpoint = shoulderSetpoint_; this.elevatorSetpoint = elevatorSetpoint_; this.wristSetpoint = wristSetpoint_; } @@ -19,18 +25,29 @@ public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristS public final void run(Context context) { context.takeOwnership(Robot.wrist); context.takeOwnership(Robot.elevator); + context.takeOwnership(Robot.shoulder); // Always retract the wrist before moving the elevator. // It might already be retracted, so it's possible that this step finishes instantaneously. Robot.wrist.rotate(Wrist.Position.RETRACTED); + // If raising the shoulder, do that before the elevator (else, lower it after the elevator). + if (shoulderSetpoint.getAngle() > Robot.shoulder.getAngle()) { + Robot.shoulder.rotate(shoulderSetpoint); + context.waitFor(() -> Robot.shoulder.isNearTo(shoulderSetpoint)); + } context.waitFor(() -> Robot.wrist.isNearTo(Wrist.Position.RETRACTED)); // Move the elevator. Wait until it gets near the target position. Robot.elevator.moveTo(elevatorSetpoint); context.waitFor(() -> Robot.elevator.isNearTo(elevatorSetpoint)); + // If lowering the shoulder, do that after the elevator. + if (shoulderSetpoint.getAngle() < Robot.shoulder.getAngle()) { + Robot.shoulder.rotate(shoulderSetpoint); + } // Lastly, move the wrist. Robot.wrist.rotate(wristSetpoint); context.waitFor(() -> Robot.wrist.isNearTo(wristSetpoint)); + context.waitFor(() -> Robot.shoulder.isNearTo(shoulderSetpoint)); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java index 28e7d0481..aa16e623a 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java @@ -4,16 +4,22 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; public class OnePieceBalance extends Procedure { + private final GamePieceType type; + + public OnePieceBalance(GamePieceType type) { + this.type = type; + } + public void run(Context context) { context.takeOwnership(Robot.drive); - // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); + Robot.gyro.resetGyro180(); Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { @@ -32,7 +38,7 @@ public void run(Context context) { log("invalid alliance"); return; } - // new IntakeRelease().run(context); + new ScoreHigh(type).run(context); new GyroBalance(alliance.get()).run(context); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java index 19d609ece..07234b7e3 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java @@ -4,25 +4,33 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; public class OnePieceExitCommunity extends Procedure { + private final GamePieceType type; + + public OnePieceExitCommunity(GamePieceType type) { + this.type = type; + } + public void run(Context context) { context.takeOwnership(Robot.drive); // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); + Robot.gyro.resetGyro180(); Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { switch (alliance.get()) { case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); + Robot.drive.setCurrentPosition(new PointDir(0.75, 2)); break; case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); + Robot.drive.setCurrentPosition(new PointDir(0.75, 14.5)); break; default: log("invalid alliance"); @@ -33,6 +41,8 @@ public void run(Context context) { return; } log("exiting"); - new OPECHelper().run(context); + new ScoreHigh(type).run(context); + new RetractWristvator().run(context); + new ExitCommunity().run(context); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java index 6baf0ae02..3264b8595 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java @@ -4,16 +4,24 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; public class OnePieceExitCommunityBalance extends Procedure { + private final GamePieceType type; + + public OnePieceExitCommunityBalance(GamePieceType type) { + this.type = type; + } + public void run(Context context) { context.takeOwnership(Robot.drive); // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); + Robot.gyro.resetGyro180(); + Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { @@ -33,8 +41,9 @@ public void run(Context context) { return; } log("exiting"); - new OPECHelper().run(context); + new ScoreHigh(type).run(context); + new ExitCommunity().run(context); log("Transitioning"); - new GyroBalance(alliance.get()).run(context); // already checked, alliance should be set + new GyroBalance(alliance.get()).run(context); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java index 5323e13f2..eb0238312 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java @@ -1,11 +1,12 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class RetractWristvator extends MoveWristvator { public RetractWristvator() { - super(Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); + super(Shoulder.Position.BOTTOM, Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java new file mode 100644 index 000000000..a71250577 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java @@ -0,0 +1,11 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; + +public class RetractWristvatorIdleIntake extends Procedure { + public void run(Context context) { + new RetractWristvator().run(context); + new IntakeIdle().run(context); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java b/src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java new file mode 100644 index 000000000..1133c16ac --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java @@ -0,0 +1,24 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class ScoreHigh extends Procedure { + private final GamePieceType type; + + public ScoreHigh(GamePieceType type) { + this.type = type; + } + + public void run(Context context) { + context.takeOwnership(Robot.intake); + Robot.intake.setGamePieceType(type); + context.releaseOwnership(Robot.intake); + new ExtendWristvatorToHigh().run(context); + new IntakeOut().run(context); + context.waitForSeconds(1); + new IntakeStop().run(context); + } +}