added pose prediction + added misc changes from our testing branch#114
added pose prediction + added misc changes from our testing branch#114JAMthepersonj wants to merge 8 commits intomainfrom
Conversation
JAMthepersonj
changed the title
JAMthepersonj
requested review from
MikeKantrowitz,
armadilloz and
batchen1
as code owners
batchen1
left a comment
batchen1
left a comment
There was a problem hiding this comment.
Great job Jackson, this seems like it will improve shooting accuracy by a lot.
I looked briefly at the code and my main comments are:
1 - instead of using Joystick for prediction, it is probably more accurate to use the robot field velocity from the swerve subsystem. It will simplify the code quite a bit and will be more accurate
2 - currently the future pose prediction is only used for angler and shooter adjustment, I think the turret angle is still not accounting for that.
3 - once you fix that, I will do a more thorough review of the changes.
| public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later | ||
| public static final double MAX_VISION_DISTANCE_SIMULATION = 6; | ||
|
|
||
| public static final double PREDICTION_TIME = 2.25; |
There was a problem hiding this comment.
How did you get this constant? was it measured? is it the same for all distances? this may be good enough but can be changed to use some formula based on distance.
There was a problem hiding this comment.
This was tuned for while testing and it was the best value we got
| return 3.281 * distance > 16 ? 6 : 3.281 * distance < 4.66 ? 1.42 : distance; | ||
| } | ||
|
|
||
| private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { |
There was a problem hiding this comment.
I'm not sure why you chose to use joystick for the estimated future pose. The joystick show driver intent but not actual speed. I think it will be a lot more accurate to use the swerve getFieldVelocity() instead. If you use that you don't need to keep multiple poses to estimate speed.
There was a problem hiding this comment.
i agree but i will have to test this after finishing up the turret PID
There was a problem hiding this comment.
fixed just not tested
| private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { | ||
| return robotPose.getTranslation().getDistance(targetPose.getTranslation()); | ||
| double distance = robotPosePredicitionCalculation(robotPose).getTranslation().getDistance(targetPose.getTranslation()); | ||
| return 3.281 * distance > 16 ? 6 : 3.281 * distance < 4.66 ? 1.42 : distance; |
There was a problem hiding this comment.
what is this? are you trying to clamp the distance? there's ways to code this that are more readable. Use constants and if statements instead of ternary expression.
3 participants
Tested on the robot