Constant speed drive

[jtechau]

The DriveTrainSubsystem has methods to command the drive train, such as curvatureDrive(), tankDrive(), etc. Implement these additional methods:

• constantSpeedDrive(speed) - Drive forward at the specified speed (drive reverse, if speed is negative). Speed is specified in feet per second (or meters per second, or whatever is used as the standard speed unit), rather than the "speed" parameters on curvatureDrive() and tankDrive() which actually specify a power level and not a speed.

  If desired, a second parameter curve could be added. This would cause the robot to curve right or left at the specified rate while driving forward or reverse at a the specified speed. Alternatively, parameters leftSpeed and rigthSpeed could be used (similar to tankDrive(), except using speed instead of power).

  Use case: This drive method can be used for any auto-drive command that would benefit from driving at a constant speed (e.g. when slow speeds are needed for precise positioning, driving on an incline, etc).

• stop() - Stop the drive motors.

[jv65537]

The motor controller must be used directly to support constant drive speed. Falcon 500 motor controller is Talon FX. The Java class for this is WPI_TalonFX.

WPI_TalonFX has a method called:

public void set​(ControlMode mode, double value)

One of the ControlModes is Velocity. In Velocity mode, output value is in position change / 100ms.

com.ctre.phoenix.motorcontrol.ControlMode.Velocity

[jtechau]

We've had issues getting the PID controller tuned. Here are some of my recent thoughts.

The most obvious issue is that the motor power oscillates. Attempts at tuning (especially P) have not provided acceptable results. One theory is that the play in the drive train results in an uneven load on the motor, at least at startup. Since velocity is measured at the motor, the uneven load is another potential source of oscillation that may interact with the natural tendency of a PID to want to oscillate, and the interaction may be too much to tune out.

(As a side note, measuring velocity at the wheels should dampen the effects due to play in the power transmission, although that's much less convenient and not implemented on this robot.)

To reduce the effects of oscillating feedback, more constant PID terms might help. This is where feed forward comes in. The CTRE documentation has some good advice in this area - establish a kF, then add some kP to make up the difference. We've tried kF, but haven't gotten it dialed in. This might help:

The Talon FX implements two types of feed forward:

1. kF, which is proportional to the set point
   • output[F] = setpoint * kF
   • kF is in units of motor "percent" / velocity unit
2. Arbitrary feed forward, which simply adds output directly to the motor
   • motor output += ArbitraryFeedForward
   • applied via: set(ControlMode.Velocity, setpoint, DemandType.ArbitraryFeedForward, feedforward)

Since kF is proportional to the setpoint, this is the more obvious approach to achieving constant velocity with a variable setpoint. kF can be determined approximately by employing the formula above:

• kF = output / setpoint
• In our common test case (using approximate numbers): kF = 0.08 / 1500 = 0.00005
• While the above equation may look correct, the motor output for feed forward is actually scaled to 0-1023. So the above equation should be: kF = (0.08 * 1023) / 1500 = 0.055 This is quite close to the value we've been using. According to the theory, applying a proper kP along with this kF should maximize responsiveness while avoiding oscillations. But we're still seeing oscillations, Other tricks may be needed to achieve the desired results (e.g. applying a "boost" when starting up to overcome stiction).

Note also that the kF relationship will not be proportional over the range of setpoints and output values, due to a number of factors. The above characterization would need to be determined at various setpoints to determine a better overall value for kF - or a variable value of kF which is a function of the setpoint.

Arbitrary feed forward is simply adding a fixed value to the motor power output of the PID controller. This may be useful in overcoming predictable variations in motor output loading. For example, moving uphill requires more power than moving on a flat surface. This could be accounted for by applying an "arbitrary" feed forward which is a function of the slope (robot Y angle).

These are my thoughts at the moment. They may or may not be accurate and/or applicable to the problem at hand.

[jtechau]

This has been implemented and tested.

We might be able to improve it by setting a variable ramp rate based on target speed. TBD