frequency of setPower to motor

[wangxdflight]

in our log, the time interval between setPower to wheel (eg. frontLeft) varies a lot. How long is it supposed to be? What is the expected maximum interval?

I wonder if it is due to our android phone performance issue (we have excessive logcat print over WIFI) or Roadrunner works in that way (based on motion profile, it applies power with different intervals).

We saw very different consistencies when run the code on a S5 and a pixel XL2 phone. It seems that setPower and update() called in the main control loop?

In some of our logs, the interval between update calls is more than 100ms. Will this cause problem?

Thanks.

[rbrott]

There should not be a large time interval between setPower() calls, and they should be consistent. 50ms is a good maximum interval, although it's best to eliminate hardware calls to bring it as low as possible.

Logcat could very well be the culprit. I recommend using TimingLogger to easily measure the timing of various parts of your control loop with low overhead (unlike profiling or heavier instrumentation).

[wangxdflight]

Thanks. Another question, if we increase the maxVel and maxAccel for the follower, are we supposed to re-tune the transitional PIDs?

And we logged the power values set to wheel. Some values are more than 1.0 (especially in strafe move). Should we lower vel and accel to make sure it is always less than 1.0?

[rbrott]

You probably don't have to re-tune the follower PIDs (including the translational ones). What exact constraints do you have? It's possible to give constraints that are unachievable (even theoretically) for the drivetrain (which manifests as powers of magnitude greater than 1).

[wangxdflight]

Vel: 90, accel 45 (straight move). For straight move, its maximum is just around 0.7. Vel: 30, accel 15 (strafe move). Even with such small value for strafe, power values sometime reaches more than 1.5.

If power reaches more than 1.0, won't it easily cause issue described below? https://en.wikipedia.org/wiki/Integral_windup

[rbrott]

Have you modified the kinematics? The mecanum kinematics in 0.4.x are symmetric w.r.t. motion direction. Whether this assumption is correct is for another discussion.

Yes, if the power exceeds 1 it will most likely cause integral windup. Road Runner ensures that this will never happen if you provide the proper constraints to the trajectory generation algorithm. In particular, if the kinematics are modified, the constraints need to be correspondingly modified to obey them.

[wangxdflight]

No. We haven't modified the kinematics. What do you mean "symmetric w.r.t. motion direction."? same power (maybe different directions) on all 4 wheels?

We are trying to optimize the control loop latency. From the log, encoder reading takes about 16 ms and IMU takes 10ms. For encoder, we plan to use bulk read. Do you have idea how much can it improve? As for IMU, we want to disable the use, as we already have 3 odometry wheels. But it seems that external heading is always enabled. Do we have to override getRawExternalHeading to disable it?

private val useExternalHeading: Boolean = true

Thanks.

[rbrott]

I mean forward and strafe motion are equivalent in the model up to wheel re-ordering and perhaps sign changes. Essentially, there's no theoretical difference between going forward and strafing.

There must be something non-standard about your code. Could you try to make a minimal, reproducible example? Having some code will help me narrow things down.

Bulk reads generally take around 3ms. If you set your localizer to a version of ThreeTrackingWheelLocalizer, the IMU will not be used. The useExternalHeading flag is for the default/drive encoder localizer.

[wangxdflight]

Thanks for your reply. I have been trying different methods to reduce the control loop latency, and want to get this right first. Tried to put IMU read to a seperate thread to continuously read to a pingpong buffer, and it saves 10ms. Tried bulk read for wheel encoder, it saves about 10ms. But right now motor setPower for the 4 drive wheels, is taking 25-30ms on average (from the time before the setPower for the 1st motor until the finishing of setPower for the 4th motor). It doesn't look right.

BTW, I am using USING_ENCODER MODE.

Thanks.

[wangxdflight]

Is it possible the abnormal latency in setPower caused by my dedicated IMU polling thread? IMU and 4 wheel motors are connected to the same hub. Polling interval of IMU is 10ms. Not sure about the impact here.

[rbrott]

Is it possible the abnormal latency in setPower caused by my dedicated IMU polling thread?

Yes, only one hardware command can be executed at a time. I advise against multiple threads accessing hardware.

[wangxdflight]

Moved IMU read to the main control loop. On average setPower has better performance. But in some cases, it takes up to 40ms to setPower to 4 wheels. Might it be electrical issue? Each wheels takes 10ms. But on my Pixel XL2 phone, it takes 3ms for each wheel. This may indicate S5 is too slow?

01-29 01:19:33.228 15200 17244 D SampleMecanumDriveREVOptimized: setMotorPowers leftFront: 0.7216236068277854 01-29 01:19:33.238 15200 17244 D SampleMecanumDriveREVOptimized: setMotorPowers leftRear: -0.250282349508273 01-29 01:19:33.248 15200 17244 D SampleMecanumDriveREVOptimized: setMotorPowers rightRear: 0.6479710341239974 01-29 01:19:33.258 15200 17244 D SampleMecanumDriveREVOptimized: setMotorPowersrightFront: -0.32393492221206094 01-29 01:19:33.268 15200 17244 D SampleMecanumDriveREVOptimized: setMotorPowers done:

[rbrott]

A little variance in command latencies is normal, but 10ms each is pretty high. 3ms is typical for an Expansion Hub.

[wangxdflight]

Thanks Ryan.

After reducing the control loop latency, I tried to retune the PIDs. But I am having the same issue again with Y direction error correction. ( I believe strafe will have the same difficulty). X transitional PIDs work quite well, but not for Y. Even I provided large P value for Y direction, it seems that feedback control is not happening. Did I miss something?

Plot for xError, (xError + X) ; yError, (yError + Y); headingError, (headingError + heading) https://docs.google.com/document/d/1Fu1Hj1JNGJDRPtYD8kG28Wpg9A8gEmsxp3O96lAAiac/edit?usp=sharing

xTransitional PID txP: 5.0 txI: 0.5 txD: 1.0E-5 yTransitional PID tyP: 5.0 tyI: 0.5 tyD: 1.0E-5 Heading PID hP: 1.0 hI: 0.5 hD: 1.0E-5 Velocity PID kP: 1.72, kI: 0.172, kD: 0.0 maxVel: 70.000000, maxAccel: 35.000000 Log: https://drive.google.com/file/d/16cTwhFOPZ_AtMX_Wnc-6znN1E3y1xIhr/view?usp=sharing

https://github.com/edsumpena/SkyStone-Clueless-2019-2020/blob/latency_dev/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID/mecanum/SampleMecanumDriveBase.java

[wangxdflight]

Double checked the code. It seems that IMU reading value needs to be in the range of [0, 2*pi]. Right now it is [-pi, pi]. This caused heading PID mess up, and then caused Y dimensional error. Does this make sense?

[rbrott]

The graphs don't seem that bad. If making the coefficients more aggressive doesn't affect the response, you may be saturating the controller (especially with your relatively high maximum velocity). You may want to reduce the constraints and see if that helps.