Improve Teleop Driving

[jay-j]

• Change drivetrain to DcMotorEx class objects, then setVelocityPIDFCoefficients(), and only setVelocity() to command.
• "squared" curve to joystick inputs for improved precision maneuvering at slow speeds. For example: joystick_fwd = gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y);
• If testing is still "twitchy", filter the joystick input commands. The smooth_delay "class" here is what I'd recommend for drivetrain, since it is easy to adapt to (as a driver) and takes off rough edges rather then outright preventing extreme inputs.

[ToothbrushB]

• Change drivetrain to DcMotorEx class objects, then setVelocityPIDFCoefficients(), and only setVelocity() to command.

Velocity control would be good for consistency and predictability, but I'm not sure how well the drivers would be able to get used to the new system.

• "squared" curve to joystick inputs for improved precision maneuvering at slow speeds. For example: joystick_fwd = gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y);

Again, I feel this would be helpful, but I'm not sure if the drivers will be able to get used to it. Perhaps we could try different functions or maybe even use piecewise functions (some curve, some linear).

• If testing is still "twitchy", filter the joystick input commands. The smooth_delay "class" here is what I'd recommend for drivetrain, since it is easy to adapt to (as a driver) and takes off rough edges rather then outright preventing extreme inputs.

This will be very useful if we need to smooth some rough edges/transitions. I will first need to port the C code into Java to use it though.

Thank you so much for your input and ideas. I really appreciate it.

[jay-j]

I can all but guarantee that drivers will immediately like a change to a velocity feedback controlled drivetrain. There is less "fighting with the controls" to get the robot to do what you want.

Curving the joystick input can be more subjective/preference based, but it's easy enough to do I'd still recommend trying it out.

[DeeWBee]

Of all the proposed changes, this is one of the easiest wins/adjustments. At least try it out @ToothbrushB

[jay-j]

The new joystick curves (EnhancedTeleOp.java : 109 and 110) are losing the sign of the joystick input. My go-to solution is x*abs(x) instead.

Then lines 112,113 you are scaling. From 2 to the maximum motor rpm. The result is that if the driver command full forwards, the robot will only move at 50% speed (but you'll have lots of steering authority available). I'd recommend changing so that maximum forward is maximum speed (saturate to 1, then map from 1 to 165), and at maximum speed accept that turns will be more gradual.

[jay-j]

Java port of smooth delay looks functional. (code readability opinion - I would have everything in just the SmoothDelay class in SmoothDelay.java and not have separate files and extending classes. The extra abstraction doesn't seem to buy you anything here).

Your teleop loop time (dt) we looked at today was ~12 ms (nice!!). So the "calling loop to operate at constant frequency" is good enough. The step value is all driver preference. Probably values below 5 are not useful, and values above 20 too sluggish. Remember that the goal is not to eliminate extreme/sudden behaviors, but to force the driver to be really intentional about triggering them.

[jay-j]

Potential feature: Drive the Claw
Purpose: avoid claw collisions when raising/lowering in close proximity to obstacles
double armRadius = [measure it, m];
double theta = [arm angle, radians, horizontal=0, increasing down];
double thetadot = [arm velocity, radians/second];
// then with drive converted to m/s...
drive -= -armRadius*sin(theta)*thetadot; // double negative intentional; cancel out body relative motion

My first instinct is to use armTargetSmooth and trap.current_velocity instead of measured encoder values as the data source to calculate theta and thetadot to avoid latency issues (best synchronization by not wait for confirmed arm motion to even start trying to get the drivetrain to move).

[ToothbrushB]

Java port of smooth delay looks functional. (code readability opinion - I would have everything in just the SmoothDelay class in SmoothDelay.java and not have separate files and extending classes. The extra abstraction doesn't seem to buy you anything here).

I know the abstraction doesn’t really make too much sense, but I wanted to model the .h and .c files as closely as possible and didn’t know how else to model the .h file. I will definitely combine the two when I have the time.

Potential feature: Drive the Claw Purpose: avoid claw collisions when raising/lowering in close proximity to obstacles double armRadius = [measure it, m]; double theta = [arm angle, radians, horizontal=0, increasing down]; double thetadot = [arm velocity, radians/second]; // then with drive converted to m/s... drive -= -armRadius*sin(theta)*thetadot; // double negative intentional; cancel out body relative motion

My first instinct is to use armTargetSmooth and trap.current_velocity instead of measured encoder values as the data source to calculate theta and thetadot to avoid latency issues (best synchronization by not wait for confirmed arm motion to even start trying to get the drivetrain to move).

I am a little confused as to what the purpose of this is. I can see that you’re trying to figure out where the arm is/how it’s moving by finding radius, angle, etc. However, I don’t understand how that can help us avoid collisions when raising/lowering. How would the robot know that there is an obstacle in its path by just knowing where the arm is/how it’s moving? I am also confused as to whether you are trying to avoid collisions when raising/lowering or when driving (you used the variable “drive”). My misunderstanding and confusion might come from me not knowing what math you are writing at the bottom; I should probably brush up on/learn more trigonometry.

It makes sense to use values provided by the profiler, but I don’t think it will harm loop time, synchronization, or latency if we use the encoder values. The loop time we calculated today included getting the encoder values for the arm at the very beginning (before any driving), and since that loop time was pretty fast, I don’t believe using the encoder values will hurt us.

[jay-j]

I know the abstraction doesn’t really make too much sense, but I wanted to model the .h and .c files as closely as possible and didn’t know how else to model the .h file. I will definitely combine the two when I have the time.

The .h file is an artifact of C and how its compiling/linking system works that as far as I'm aware doesn't apply to Java.

However, I don’t understand how that can help us avoid collisions when raising/lowering. How would the robot know that there is an obstacle in its path by just knowing where the arm is/how it’s moving? I am also confused as to whether you are trying to avoid collisions when raising/lowering or when driving (you used the variable “drive”).

You are correct, the robot doesn't know if there is an obstacle in its path or not. But the human driver does. So currently, if the robot pulls up close to an obstacle and tries to raise its arm, the driver will have to backup first (read: drive the robot, thus modifying the drive variable; the robot's commanded speed) to make sure the arm doesn't collide with the obstacle. My proposed enhancement will automatically modify the drive speed command to compensate for arm movement. So the effect is that the driver can really focus on just the claw and has (as much as possible) independent control inputs to move it forward/backward, rotate, and up/down (previously, foward/backward and up/down were coupled). Will this mess with the driver too much when not near anything? TBD, it's simple enough that I think it's worth trying out.

It makes sense to use values provided by the profiler, but I don’t think it will harm loop time, synchronization, or latency if we use the encoder values.

If using the encoder values, then the response time of the electromechanical system (mass moving, encoder mechanical design) becomes part of the feedback loop. The mechanics are very sluggish relative to the electrical speeds, and your low count encoder has low velocity bandwidth (slow to detect changes in velocity). So my hunch is that waiting for these systems to respond to start commanding drive motors (which have to get through their own mechanical latency) is a worse problem than using ideal (profiler output) data instead of real (encoder measured) data.

[jay-j]

Need to have separate smoothDelay objects for each variable being smoothed.

[ToothbrushB]

I implemented the drive with claw feature in EnhancedDecoupledTeleOp.java, but I'm not sure if I did it correctly. Could you please check my code?

Notes:

• Line 52 is used to convert the angle difference between the tower and the resting position of the arm (the angle offset) to ticks
• The 15 on line 52 is the placeholder angle offset and should be in radians, I just forgot to convert it
• The arm length on line 53 needs to be adjusted
• I chose to use trap.current_position instead of keeping track of armTargetSmooth. It should output the same number though.
• I know there are many expressions that can be simplified, I will keep them in this "verbose" form to make troubleshooting easier.

[jay-j]

Only have time for a quick glance now.

• The 'driveMetersPerSec' alteration for arm movement should NOT be affected in any way by fastMode scaling.
• Move the drivetrain code to after the arm code so you have the latest arm commands (only saving one loop, which is not much, but it's an easy change).
• ARM_ENCODER_OFFSET I think can just be a measured value; put a level on top of the arm, print out the tics when the level reads 0 degrees. Probably something like -500 or -600 tics? based on the other arm setpoints.
• I'm not familiar with the angleUnit and distanceUnit systems, so

A comment on "style" - it is a better practice to convert values into engineering units "meters, radians, seconds, etc." as soon as they are input, perform all calculations in those units, and then only convert to required output units at the last possible point.

Verbose for easier debugging is perfect.

[ToothbrushB]

I think that fast mode scaling is necessary in order to calculate the "real velocity" that gets sent to the motors. If we just subtract from the non-scaled velocity, we wouldn't move at the right speed because the velocity would be scaled down.

Move the drivetrain code to after the arm code so you have the latest arm commands (only saving one loop, which is not much, but it's an easy change).

Done

ARM_ENCODER_OFFSET I think can just be a measured value; put a level on top of the arm, print out the tics when the level reads 0 degrees. Probably something like -500 or -600 tics? based on the other arm setpoints.

I might try both methods and see if they come up with around the same answer. Although, the measurement option probably is more accurate than trying to measure lengths and angles, it would still be a good learning experience to play with trigonometry in the real world.

I'm not familiar with the angleUnit and distanceUnit systems, so

They are just utility enums that help you convert between different units. The angle unit enum has methods like fromRadians(), toDegrees(), etc. The distance unit enum has methods that convert distances. It wouldn't be hard to implement these methods by hand, but since the rest of the SDK uses AngleUnit and DistanceUnit, I was inclined to use them too.

A comment on "style" - it is a better practice to convert values into engineering units "meters, radians, seconds, etc." as soon as they are input, perform all calculations in those units, and then only convert to required output units at the last possible point.

Drive and turn are now in m/s from the very beginning. They get scaled by fast mode too so that they accurately resemble the velocity in real life. They get converted back into tics/s when calculating the combined velocity (drive and turn).

[jay-j]

Looking great, nice work! Grab a side view video when you try it out.