Limelight #110
Open
Limelight #110
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
* Change example items to drivetrain skeleton * Format using the formatting tool in VScode * Update FRC library version * Update WPILIB extension, add motorcontrollers, and resolve merge conflicts * Change example items to drivetrain (#2) * Change example items to drivetrain skeleton * Format using the formatting tool in VScode * Update WPILIB extension, add motorcontrollers, and resolve merge conflicts * Fix TeleopDrive * Fix TeleopDrive for arcade and tank * Fix Arcade Drive * Split TeleopDrive.java into Arcade and Tank Drive * Add SmartDashboard ports and Encoders * add continuous integration (#6) * add travis config and update README.md * Fix copyright typo introduced in earlier commit. * Add motor configuration methods for talons and victors * Add gyros (note: there are some errors - that's because AHRS has not been added) * Fix ArcadeDrive logic * initial changes * Add basic cargo syubsystem functionality * Add squared joystick functionality * clean up code and add comments * add correct port numbers for motor controllers * add correct port numbers for motor controllers (#8) * add correct port numbers for motor controllers * invert right motors drivetrain motors run the opposite direction on each side, the right motors run the incorrect direction * Update cargo code to match this year's robot, and OI complements. Can now intake/eject * Format and organize all changed files according to VSCode auto-formatter * Set distance per pulse of drivetrain encoders * add camera code * improvements to cargo code (#12) * add hatch code (#14) * Add Climber code * Remove test code (accidental commit) * Make joysticks negative; correctly square joysticks; initialize OI; put Encoder values to S.D (#20) * Add variable rollerPort for hasCargo() method. * Move cargoPDPPort to RobotMap * Move TODO statement to more relevant spot, formatted two documents * Work around copySign problem * Remove test prints and SmartDashboard * Comment and format * Add code for rails * make motor controller initialization compatible with both robots (#24) * Put piston state value to Smart Dashboard (#21) * Put piston state value to Smart Dashboard * Add default state for pistons * Fix unreachable statement error in HatchPanel.toggle() * Change names of pistonState * Enter correct hatch solenoid numbers (#26) * Cargo null error (#27) * Enter correct hatch solenoid numbers * Fix null error in cargo intake and eject * Update names and organization to match rest of code and use AHRS * Change climber code to reflect the use of the accelerometer to check climbing bounds * Implement accelerometer code for the climber * Fix logic, add rail button * Fix logic in climber code * Change climbing logic to single command * Fix syntax error * Fix additional syntax errors * fix compilation errors and clean up code * use navx instead of built in accelerometer * format code * requested changes * add climb button and fix drivetrain/oi stuff (#29) * add climb button and fix drivetrain/oi stuff * format code * Restructure climb into climb and retract, add manual override * Change climb logic to enum State * Make requested changes in climber * Organize up into state and if/elses into switch * add manipulator "enum" (#31) Java enums don't really map to ints so I'm just making a fake enum * Add encoder logic and clean up climbing logic * Make buttons more descriptive in OI.java (#32) * add manipulator "enum" Java enums don't really map to ints so I'm just making a fake enum * Make names in OI for buttons more descriptive * Update to 3.2 (#33) * Change climber port number to correct one * changes from on-robot testing (#35) * improvements to cargo code * fix merge errors * changes from on-robot testing * Fix syntax error in OI.java * Issue 38 (#40) * Add cargo status in SmartDashboard * Include accidentally unstaged change * Fix finished/interrupted logic in cargo * Address issue 37 by having the hatch stay out while intaking cargo (#41) * Address issue 37 by having the hatch stay out while intaking cargo * Make WaitMove only detect one-way movement * Fix incorrect subtraction logic * Add error checking (and printing) for the motor controller config statements (#47) * fix right arcade drive trigger (#44) * fix #43 * change default slow rotation value * Change climber ports (#48) * Add code to keep climber up (#53) * Add code to keep climber up * Remove extraneous variable, add to keepclimber logic * Fix climber logic (#54) * Fix climber logic * Change logic to reflect new priority of changing directions too much (from climbing too often) * Add resetting code to drivetrain encoders when they are created (#55) * add manual climb * update max climb height to be the height of the robot * Put correct port number in hasCargo() (#58) * Add wobbledrive capability * Drivetrain characterization (#25) * Copy drivetrain characterization code to Robot 2019 * Fix Dean's complaints and format * Replace SpeedController with WPI_TalonSRX * Merge IncreaseVoltageLinear and IncreaseVoltageStepwise into DrivetrainCharacterization Command. * Remove old IncreaseVoltage commands * Fix maxVoltage error * Add Drivetrain Characterization Analysis * Surround Scanner with Try-Catch in Drivetrain.java * Fix try-catch in Drivetrain.java * Reduce and simplify DrivetrainCharAnalysis.java * Resolve building issues associated with the last two branches * Resolve build changes in the last commit * Try-Catch FileReader and CSVReader in DrivetrainCharacterization.java * Add CharacterizedDrive capability * Make DrivetrainAnalysis a SmartDashboard button * Split kV, kA, vIntercept into left and right values * Split kv,ka,vIntercept into forward-left, forward-right, backward-left, and backward-right values * Fix parseCSV (now the code actually works!) * Fix Characterized Drive * Fix testing issues * Utilize acceleration term in drivetrain characterization formula * Fix CharDrive in TeleopDrive * Partial Changes the night of 2/18/19 * Remove absolute value from leftDV and rightDV * Fix turning issue with acceleration clipping * Invert right encoder * Change wobble to a whileHeld * Add button to switch between arcade drive and tank drive (#67) * Add button to switch between arcade drive and tank drive * Auto-format files * Add wobbledrive capability (#65) * Add wobbledrive capability * Change wobble to a whileHeld * Add early stop when drivetrain is stuck during wobbling * Add wobble reset when wobble button is released * Fix logic in wobble drive * Fix bug with wobbledrive looking at distance instead of rate * Initial commit * Fix cargo and climber ports * Fix logic in finishing wobble drive * Make wobble stop doing a finished side, fix logic * Add SmartDashboard output when wobble done * Catch singular matrix exception and dump X and Y matrices * Set climb speeds to proven values * Add timer to differentiate between climb levels * Fix getAngle function in climber * Fix getAngle function in climber * Add lights code * Fix bug calling wrong command from oi * Add lights color to smartdashboard * Replace timer with encoder to finish climb * Fix syntax error, edit constant to more reasonable value. * Update some TODO values with actual number * Reverse climber encoder logic (and rename variables) * Make requested change * Capitalize SmartDashboard values * different buttons for auto and manual climb * change climb buttons to be consistent (also because I'm adding hatch buttons but ignore that) * Solves issue-88 (#89) * add new hatch mech code (#86) * add new hatch mech code * minor changes and add comments * Change climber piston ports * change to manipulator controls for manual climbing (#96) * change to manipulator controls for climbing * im bad -kev * update hatch code (#97) * add new hatch mech code * minor changes and add comments * hardcode no delay for hatch eject * change naming for clarity * re-delete files from merge * delete the imports of removed files * Fix angles for climbing (#94) * Fix angles for climbing * Change climber piston ports * Use atan for climber angle * use pitch instead of accel calculations * change rails to toggle * Fix issue #98 (#102)
brettle
requested changes
Oct 31, 2019
Comment on lines
269
to
275
| if (!SmartDashboard.getBoolean("Use Limelight", false) || SmartDashboard.getBoolean("Finished Aligning", false)) { | ||
| SmartDashboard.putBoolean("Use Limelight", false); | ||
| SmartDashboard.putBoolean("Finished Aligning", false); | ||
| return true; | ||
| } else { | ||
| return false; | ||
| } |
There was a problem hiding this comment.
I don't understand this logic. Why does this command finish?
| } | ||
| } | ||
| else if (limelightMode == Limelight.Mode.STEER) { | ||
| adjustment = lime.distanceAssist(); |
There was a problem hiding this comment.
Did you want lime.steerAssist() here?
Comment on lines
30
to
31
| tv = table.getEntry("tv").getDouble(0.0); | ||
| ty = table.getEntry("ty").getDouble(0.0); |
There was a problem hiding this comment.
Please document what tv, ty, and ta are and, if appropriate, include a link to relevant limelight doc.
|
Honestly I have no idea how this works but it looks cool. You should test it out. |
brettle
requested changes
Nov 17, 2019
Comment on lines
46
to
48
| if (ta < area_threshold) { | ||
| adjustment += Kp * ty; | ||
| } |
There was a problem hiding this comment.
This seems odd. It will move further faster if ty is larger, but ty will generally be larger if you are closer to the target. I also don't understand the point of area_threshold. As it stands, it will prevent moving backward if you are too close to the target. I recommend reviewing:
http://docs.limelightvision.io/en/latest/cs_estimating_distance.html (and doing the associated math)
and
http://docs.limelightvision.io/en/latest/cs_autorange.html
brettle
approved these changes
Dec 21, 2019
|
|
||
| if (tv == 1.0) { | ||
| adjustment += Kp * tx; | ||
| if (ta > 0.02) { | ||
| adjustment += Kp * tx; |
There was a problem hiding this comment.
Using += instead of = here seems unnecessarily confusing. Am I missing something?
| } else { | ||
| adjustment += steering_factor; | ||
| adjustment += Math.signum(prev_tx) * steering_factor; |
There was a problem hiding this comment.
Same comment regarding += vs =.
|
|
||
| /* 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) { |
There was a problem hiding this comment.
You haven't added the code that uses this yet, right?
Also, please document the world coordinate system. Is the origin on the robot (where on the robot?), a fixed spot on the field, someplace else? Which direction do each of the axes point?
Comment on lines
+110
to
+112
| /* 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. | ||
| */ |
There was a problem hiding this comment.
I think I sorta understand, but I'm really unsure and I suspect other would be ever more unsure. Can you try to clarify? Maybe explain why the 2 distances aren't the same and what 2 points the second distance is between?
There was a problem hiding this comment.
Also looks like this actually returns 2 values. A distance and an angle?
| xSum += cornerXs[i]; | ||
| ySum += cornerYs[i]; | ||
| } | ||
| double[] cameraPos = {xSum / 4, ySum / 4, 1}; |
There was a problem hiding this comment.
Why is the camera's z equal to 1?
Comment on lines
+142
to
+224
| 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; |
There was a problem hiding this comment.
There are plenty of existing java linear algebra libs that should do all of this stuff. Use one of those if at all possible.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Basic Limelight functionality to allow for auto aligning and targeting. As of writing this pull request, this branch has not be tested. I want my code to be reviewed by several veteran programmers before beginning testing. After testing we can determine whether or not we should merge.