Octoquad support #512
Octoquad support #512
Conversation
…es so it can actually be used for tuning
# Conflicts: # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
…ose versions, have `OTOSLocalizer.update()` return robot-centric velocity
# Conflicts: # TeamCode/build.gradle # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSLocalizer.java
# Conflicts: # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
rbrott
left a comment
rbrott
left a comment
There was a problem hiding this comment.
Has this been tested on a real vehicle?
There was a problem hiding this comment.
Doesn't this driver now ship with the SDK? Are there any differences between that driver and this one?
| The OctoQuad IMU needs to be tuned before use to ensure the output heading is accurate. | ||
| Run AngularScalarTuner and follow the instructions to get this value. | ||
| */ | ||
| public double angularScalar = 1.0415; |
| public int odometryPortX = 1; | ||
|
|
||
| public int odometryPortY = 2; |
There was a problem hiding this comment.
I'd prefer to stick with parallel/perpendicular names to match the other localizers (I think X and Y is more confusing)
| public OctoQuadFWv3.EncoderDirection xDirection = OctoQuadFWv3.EncoderDirection.FORWARD; | ||
| public OctoQuadFWv3.EncoderDirection yDirection = OctoQuadFWv3.EncoderDirection.REVERSE; |
There was a problem hiding this comment.
And why should the yDirection be reversed? I'd prefer them both to be forward to match the other localizers.
| // Reset the localization, IMU, and encoders | ||
| octoquad.resetEverything(); |
There was a problem hiding this comment.
Does this not reset the parameters as well? From the manual:
This command resets quadrature encoder counts or measured pulse width to zero, and sets all parameters to their factory defaults. NOTE: this does not save the newly reset parameters to flash.
| ElapsedTime timeout = new ElapsedTime(); | ||
| OctoQuadFWv3.LocalizerStatus currentStatus = octoquad.getLocalizerStatus(); | ||
| while (currentStatus != OctoQuadFWv3.LocalizerStatus.RUNNING) { | ||
| try { | ||
| Thread.sleep(10); | ||
| } catch (InterruptedException e) { | ||
| throw new RuntimeException(e); | ||
| } | ||
| if (timeout.seconds() > 3) { | ||
| Log.println( | ||
| Log.WARN, | ||
| "OctoQuadLocalizer", | ||
| "init: Calibration timeout reached, OQ still not ready. OctoQuad reports " + currentStatus | ||
| ); | ||
| RobotLog.addGlobalWarningMessage( | ||
| "OctoQuad still not ready, timeout reached. OctoQuad reports " + currentStatus + | ||
| " Continuing anyway, good luck!" | ||
| ); | ||
| break; | ||
| } | ||
| currentStatus = octoquad.getLocalizerStatus(); | ||
| } |
There was a problem hiding this comment.
Does this normally need to run for a while? I don't really like the idea of blocking the rest of initialization for 3s here. If this is really necessary I'd rather build a "localizer ready" signal that's used to block trajectory actions from starting (maybe just the first one?).
I'm also dubious about continuing with this being unavailable. It doesn't really make sense to run a trajectory action when there is no localization (teams concerned about possibility should make their own fallback localizer).
| octoquad.setLocalizerPose( | ||
| (int) DistanceUnit.MM.fromInches(currentPose.position.x), | ||
| (int) DistanceUnit.MM.fromInches(currentPose.position.y), | ||
| (float) currentPose.heading.toDouble() | ||
| ); |
There was a problem hiding this comment.
Interesting that it provides a setLocalizerPose(). I'd rather not call this and use the pinpoint scheme to avoid penalizing users that call setPose() often (this occasionally comes up with distance sensors and whatnot).
| if (localizer.localizerStatus != OctoQuadFWv3.LocalizerStatus.RUNNING || !localizer.crcOk | ||
| ) { | ||
| Log.println( | ||
| Log.WARN, | ||
| "OctoQuadLocalizer", | ||
| "update: Bad data recieved from OctoQuad." + | ||
| " Localizer status: " + localizer.localizerStatus + | ||
| " CRC OK: " + localizer.crcOk | ||
| ); | ||
| // throw away bad data (don't change pose) | ||
| // have to return something for velocity; hopefully this won't cause a sudden jump? need to test | ||
| return new PoseVelocity2d(new Vector2d(0,0),0); | ||
| } |
There was a problem hiding this comment.
I'd rather a tight retry loop here for 50ms instead of immediately returning bad data. Continuing the discussion from the initialization check loop, if there are common failures then it may be worth plumbing some signal into the action level to stop the robot.
|
Yes, though not recently. |
It sounds like this isn't a very common case so I'm not too concerned with exactly what happens. I might have a slight preference for just keeping the pose the same & returning no velocity instead of crashing to shift blame from RR to the sensor/hardware/user setup.
It seems like the BNO055 IMU has a 2s delay for chip ID plus the other initialization stuff which makes a 3s timeout not seem so bad for users used to waiting for IMU init. The calibration procedure is interesting. I would have expected a device like this to rely solely on gyro z for yaw and use the initialization period to determine a fixed bias. It's a little extra to get gravity from the accel -- I would be worried that the accel bias variation is enough to affect the chosen yaw axis. I don't think that really affects the sensitivity to servo/other initialization movement. Fine to leave in the timeout though. |
3 participants
This adds Octoquad support in the style of the existing localizers, and expands the existing tuning opmodes to support it. Also see acmerobotics/road-runner-ftc#27