diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index f4fb838..363da62 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -26,6 +26,7 @@ import frc.robot.commands.ToggleClimberRails; import frc.robot.commands.ToggleHatchEject; import frc.robot.commands.ToggleLight; +import frc.robot.commands.ToggleLimelight; import frc.robot.commands.WobbleDrive; import frc.robot.subsystems.Cargo; import frc.robot.subsystems.Climber; @@ -44,6 +45,7 @@ public class OI { JoystickButton arcadeOrTankBtn; JoystickButton normDriveBtn; JoystickButton gradDriveBtn; + JoystickButton limelightBtn; JoystickButton hatchIntakeBtn, hatchEjectBtn; JoystickButton cargoIntakeBtn, cargoEjectBtn; JoystickButton climberRailBtn; @@ -74,6 +76,8 @@ public class OI { normDriveBtn.whenPressed(new NormalDrive()); gradDriveBtn = new JoystickButton(leftJoy, 5); gradDriveBtn.whenPressed(new GradualDrive()); + limelightBtn = new JoystickButton(rightJoy, 2); // Figure out the correct button + limelightBtn.whenPressed(new ToggleLimelight()); hatchIntakeBtn = new JoystickButton(manipulator, Manip.X); hatchIntakeBtn.whenPressed(new IntakeHatch(hp, dt)); @@ -94,7 +98,6 @@ public class OI { manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger); manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights)); - slowClimbBtn = new JoystickButton(manipulator, Manip.LB_lShoulder); slowClimbBtn.whileHeld(new SlowClimb()); diff --git a/Robot2019/src/main/java/frc/robot/Robot.java b/Robot2019/src/main/java/frc/robot/Robot.java index 20d1dc0..efc38ac 100644 --- a/Robot2019/src/main/java/frc/robot/Robot.java +++ b/Robot2019/src/main/java/frc/robot/Robot.java @@ -13,7 +13,8 @@ import frc.robot.commands.DrivetrainAnalysis; import frc.robot.commands.IncreaseVoltageLinear; import frc.robot.commands.IncreaseVoltageStepwise; -import frc.robot.commands.TeleopDrive; +import frc.robot.commands.Drive; +import frc.robot.lib.Limelight; import frc.robot.subsystems.Cargo; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; @@ -25,6 +26,7 @@ public class Robot extends TimedRobot { private static Drivetrain dt; private static HatchPanel hp; private static OI oi; + private static Limelight lime; private static Cargo cargo; private static Climber climber; private static Lights lights; @@ -40,6 +42,7 @@ public void robotInit() { climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons); lights = new Lights(RobotMap.lights); oi = new OI(dt, hp, cargo, climber, lights, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer); + lime = new Limelight(); fname1 = "/home/lvuser/drive_char_linear_for.csv"; fname2 = "/home/lvuser/drive_char_stepwise_for.csv"; @@ -56,10 +59,11 @@ public void robotInit() { SmartDashboard.putData("Increase Voltage Stepwise Backward", ivsb); SmartDashboard.putData("Drivetrain Characterization Analysis", dca); - dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy)); + dt.setDefaultCommand(new Drive(dt, lime, oi.leftJoy, oi.rightJoy)); SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0); SmartDashboard.putBoolean("Outreach Mode", false); + SmartDashboard.putBoolean("Using Limelight", false); timey = new Timer(); } diff --git a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java b/Robot2019/src/main/java/frc/robot/commands/Drive.java similarity index 77% rename from Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java rename to Robot2019/src/main/java/frc/robot/commands/Drive.java index 0c5daa5..99dfd20 100644 --- a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/Robot2019/src/main/java/frc/robot/commands/Drive.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,28 +7,28 @@ package frc.robot.commands; +import frc.robot.lib.Limelight; +import frc.robot.subsystems.Drivetrain; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.Drivetrain; - -public class TeleopDrive extends Command { - Drivetrain dt; - Joystick leftJoy, rightJoy; - - double prevSpeed = 0, prevLeft = 0, prevRight = 0; - double outreachSpeed = 0.3; - - /** - * Handles all the teleoperated driving functionality - * - * @param dt the Drivetrain object to use, passing it in is useful for testing - * purposes - */ - public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) { +public class Drive extends Command { + private Drivetrain dt; + private Joystick leftJoy, rightJoy; + private Limelight lime; + private Limelight.Mode limelightMode = Limelight.Mode.STEER; + private double adjustment = 0; + private double minError = 0.05; // TODO: Test minimum values + private double prevSpeed = 0, prevLeft = 0, prevRight = 0; + private double outreachSpeed = 0.3; + + public Drive(Drivetrain dt, Limelight lime, Joystick leftJoy, Joystick rightJoy) { requires(dt); this.dt = dt; + this.lime = lime; + this.dt = dt; this.leftJoy = leftJoy; this.rightJoy = rightJoy; @@ -45,12 +45,22 @@ public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) { } } + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (SmartDashboard.getBoolean("Arcade Drive", true)) { - arcadeDrive(); + if (SmartDashboard.getBoolean("Using Limelight", false)) { + autoAlign(); } else { - tankDrive(); + if (SmartDashboard.getBoolean("Arcade Drive", true)) { + arcadeDrive(); + } else { + tankDrive(); + } } } @@ -230,16 +240,43 @@ private void charDrive(double left, double right) { dt.disableVoltageCompensation(); } + private void autoAlign() { + if (limelightMode == Limelight.Mode.DIST) { + adjustment = lime.distanceAssist(); + dt.drive(adjustment, adjustment); + if (Math.abs(adjustment) < minError) { + SmartDashboard.putBoolean("Finished Aligning", true); + } + } + else if (limelightMode == Limelight.Mode.STEER) { + adjustment = lime.steeringAssist(); + dt.drive(adjustment, -adjustment); + if (Math.abs(adjustment) < minError) { + SmartDashboard.putBoolean("Finished Aligning", true); + } + } else { + double[] params = lime.autoTarget(); + dt.drive(params[0], params[1]); + double maxInput = Math.max(Math.abs(params[0]), Math.abs(params[1])); + if (maxInput < minError) { + SmartDashboard.putBoolean("Finished Aligning", true); + } + } + } + + // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { return false; } + // Called once after isFinished returns true @Override protected void end() { - dt.stop(); } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run @Override protected void interrupted() { end(); diff --git a/Robot2019/src/main/java/frc/robot/commands/SetLight.java b/Robot2019/src/main/java/frc/robot/commands/SetLight.java index 5f7c7c7..9efd22e 100644 --- a/Robot2019/src/main/java/frc/robot/commands/SetLight.java +++ b/Robot2019/src/main/java/frc/robot/commands/SetLight.java @@ -28,4 +28,4 @@ protected void initialize() { lights.setLights(state); } -} +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleLimelight.java b/Robot2019/src/main/java/frc/robot/commands/ToggleLimelight.java new file mode 100644 index 0000000..52c0f68 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleLimelight.java @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ToggleLimelight extends InstantCommand { + public ToggleLimelight() { + } + + @Override + protected void initialize() { + if (SmartDashboard.getBoolean("Using Limelight", false)) { + SmartDashboard.putBoolean("Using Limelight", false); + } else { + SmartDashboard.putBoolean("Using Limelight", true); + } + } +} diff --git a/Robot2019/src/main/java/frc/robot/lib/Limelight.java b/Robot2019/src/main/java/frc/robot/lib/Limelight.java new file mode 100644 index 0000000..2b05448 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/lib/Limelight.java @@ -0,0 +1,260 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.lib; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Limelight { + public enum Mode { + DIST, STEER, TARGET + } + + /* http://docs.limelightvision.io/en/latest/networktables_api.html + tv = Whether the limelight has any valid targets (0 or 1) + tx = Horizontal Offset From Crosshair To Target (-27 degrees to 27 degrees) + ty = Vertical Offset From Crosshair To Target (-20.5 degrees to 20.5 degrees) + ta = Target Area (0% of image to 100% of image) + There are more values we could be using. Check the documentation. + */ + private double tv, tx, ty, ta; + private boolean stopSteer = false; + private double mounting_angle; + private double prev_tx = 1.0; + + // Parameters for vision using linear algebra. + private double[][] rotMat = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + private double[] translateVec = {0, 0, 0}; + private double[] defaultValue = {0, 0, 0, 0}; + + public Limelight() { + SmartDashboard.putNumber("Area Threshold", 0.02); + } + + // For the shooter. Given what the limelight sees and the shooter angle, compute the desired initial speed for the shooter. + public double computeSpeed(double angle, double cameraHeight, double objectHeight) { + double distance = determineObjectDist(cameraHeight, objectHeight); + return Math.sqrt((16.1 * Math.pow(distance, 2)) / (distance * Math.tan(angle) - cameraHeight - objectHeight)) / Math.cos(angle); + } + + /* Determine the mounting angle of the camera given a vision target and its known distance, height off of the ground, + and the height of the camera off of the ground. */ + public void determineMountingAngle(double distance, double cameraHeight, double objectHeight) { + // NOTE: ty may be negative. + ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0); + mounting_angle = Math.atan((cameraHeight - objectHeight) / distance) - ty; + } + + // Determine the distance an object is from the limelight given the camera's height off of the ground and the object's height off of the ground. + public double determineObjectDist(double cameraHeight, double objectHeight) { + ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0); + return (objectHeight - cameraHeight) / (Math.tan(mounting_angle + ty)); + } + + /* Given what is currently seen, determine the entries rotMat and translateVec parameters + by solving a system of equations using Gaussian-elimination */ + public void computeParams(double[] worldXs, double[] worldYs, double[] worldZs) { + double[] cornerXs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornx").getDoubleArray(defaultValue); + double[] cornerYs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcorny").getDoubleArray(defaultValue); + double[][] corners = {cornerXs, cornerYs, {1, 1, 1, 1}}; + + for (int i = 0; i < 3; i++) { + // Set up the matrix + double[][] matrix = new double[4][5]; + for (int row = 0; row < 4; row++) { + matrix[row][0] = worldXs[row]; + matrix[row][1] = worldYs[row]; + matrix[row][2] = worldZs[row]; + matrix[row][3] = 1; + matrix[row][4] = corners[i][row]; + } + + /* Row reduce and find solutions; assumed that echelon is of the form [I | x] + where I is the identity matrix and x are the solutions. This has not been tested yet. */ + double[][] echelon = Gaussian(matrix); + rotMat[i][0] = echelon[0][4]; + rotMat[i][1] = echelon[1][4]; + rotMat[i][2] = echelon[2][4]; + translateVec[i] = echelon[3][4]; + } + } + + // Adjusts the distance between a vision target and the robot. Uses basic PID with the ty value from the network table. + public double distanceAssist() { + tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0); + ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0.0); + SmartDashboard.putNumber("Crosshair Vertical Offset", ty); + double adjustment = 0.0; + double area_threshold = 1.75; + double Kp = 0.225; + + if (tv == 1.0) { + adjustment = (area_threshold - ta) * Kp; + } + adjustment = Math.signum(adjustment) * Math.min(Math.abs(adjustment), 0.5); + return adjustment; + } + + // Adjusts the angle facing a vision target. Uses basic PID with the tx value from the network table. + public double steeringAssist() { + tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0); + tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0); + ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0.0); + SmartDashboard.putNumber("Crosshair Horizontal Offset", tx); + SmartDashboard.putNumber("Found Vision Target", tv); + SmartDashboard.putNumber("Prev_tx", prev_tx); + double adjustment = 0.0; + double steering_factor = 0.25; + double Kp = 0.015; + + if (tv == 1.0 && !stopSteer) { + if (ta > SmartDashboard.getNumber("Area Threshold", 0.02)) { + adjustment += Kp * tx; + prev_tx = tx; + } + } else { + adjustment += Math.signum(prev_tx) * steering_factor; + } + + if (Math.abs(tx) < 1.0) { + stopSteer = true; + } else { + stopSteer = false; + } + SmartDashboard.putBoolean("Stop Auto Steering", stopSteer); + + adjustment = Math.signum(adjustment) * Math.min(Math.abs(adjustment), 0.5); + SmartDashboard.putNumber("Adjustment", adjustment); + return adjustment; + } + + // Combination of distance assist and steering assist + public double[] autoTarget() { + double dist_assist = distanceAssist(); + double steer_assist = steeringAssist(); + double[] params = {dist_assist + steer_assist, dist_assist - steer_assist}; + return params; + } + + /* Given a desired straight-line distance targetDist away from the vision target, determine the distance + in order to face the target from head-on. Returns the required distance at the current heading. + */ + public double[] determineDist(double targetDist) { + // Get the x and y coordinates of the corners of the bounding box. + double[] cornerXs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornx").getDoubleArray(defaultValue); + double[] cornerYs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcorny").getDoubleArray(defaultValue); + + // Average the corners to get the center of the vision target as viewed by the camera. + double xSum = 0.0; + double ySum = 0.0; + for (int i = 0; i < 4; i += 1) { + xSum += cornerXs[i]; + ySum += cornerYs[i]; + } + double[] cameraPos = {xSum / 4, ySum / 4, 1}; + + // Calculate the position of the center in 3D space. + double[] worldPos = dot(transpose(rotMat), difference(cameraPos, translateVec)); + double x = worldPos[0]; + double y = worldPos[1]; + + // Use trigonometry to find the required distance. + double r = Math.sqrt(x * x + y * y); + double a1 = Math.atan2(y, x); + double a3 = Math.asin(r * Math.sin(a1) / targetDist); + double a2 = Math.PI - a1 - a3; + double[] params = {r * Math.sin(a2) / Math.sin(a3), a3}; + return params; + } + + // Returns the transpose of a matrix. + private double[][] transpose(double[][] matrix) { + int n = matrix.length; + int m = matrix[0].length; + double[][] matrixTranspose = new double[m][n]; + for (int i = 0; i < n; i++) { + for (int j = 0; j < m; j++) { + matrixTranspose[m][n] = matrix[n][m]; + } + } + return matrixTranspose; + } + + // Returns matrix-vector product. + private double[] dot(double[][] matrix, double[] vector) { + int n = matrix.length; + int m = matrix[0].length; + assert m == vector.length; + double[] dotVector = new double[n]; + for (int i = 0; i < n; i++) { + dotVector[i] = dot(matrix[i], vector); + } + return dotVector; + } + + // Returns x * yT. + private double dot(double[] x, double[] y) { + int n = x.length; + assert x.length == y.length; + double dot = 0.0; + for (int i = 0; i < n; i++) { dot += x[i] * y[i]; } + return dot; + } + + // Returns the difference of two vectors. + private double[] difference(double[] x, double[] y) { + double[] diff = new double[x.length]; + for (int i = 0; i < x.length; i++) { diff[i] = x[i] - y[i]; } + return diff; + } + + // Returns the reduced row-echelon form of a matrix. + private double[][] Gaussian(double[][] matrix) { + int n = matrix.length; + int m = matrix[0].length; + double[][] echelon = new double[n][]; + for(int i = 0; i < matrix.length; i++) { + echelon[i] = matrix[i].clone(); + } + + while (!isEchelon(echelon)) { + for (int i = 0; i < Math.min(n, m); i++) { + double pivot = matrix[i][i]; + if (pivot != 0) { + if (pivot != 1) { + for (int j = 0; j < m; j++) { echelon[i][j] /= pivot; } + } + for (int rowNum = 0; rowNum < n; rowNum++) { + for (int j = 0; j < m; j++) { echelon[rowNum][j] -= echelon[rowNum][i] * echelon[i][j]; } + } + } + } + } + return echelon; + } + + // Tests whether a matrix is in reduced-row-echelon form + private boolean isEchelon(double[][] matrix) { + int n = matrix.length; + int m = matrix[0].length; + double[][] matrixTranspose = transpose(matrix); + for (int i = 0; i < n; i++) { + for (int j = 0; j < m; j++) { + double val = matrix[i][j]; + if (Math.abs(val) > 0) { + int non_zero_count = 0; + for (int k = 0; k < n; k++) { + if (matrixTranspose[j][k] != 0) { non_zero_count += 1; } + } + if (non_zero_count != 1){ return false; } + } + } + } + return true; + } +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Lights.java b/Robot2019/src/main/java/frc/robot/subsystems/Lights.java index 4c1d61d..3612726 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Lights.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Lights.java @@ -8,7 +8,6 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.Relay; -import edu.wpi.first.wpilibj.Relay.Value; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.commands.SetLight;