We have a need to filter joy stick movement to avoid having jerky starts and stops that...
- threaten to tip over the robot
- reduce our ability to accurately move the robot
- reduce our ability to accurately set the heading of the robot (for shooting azimuth angle)
There are two useful behaviors we naturally look for...
- a gentle response curve between the joystick and actual motor RPM. This means that at the slowest speeds, where accuracy of positioning is important, one degree of joystick motion has a small effect on motor RPM; at medium speed, where you need maneuverability, but not critical accuracy, one degree has medium effect; at high speeds, where you want to get from point A to point B quickly, in more or less a straight line, one degree has a large effect.
- a maximum envelope for acceleration. This is for safety. If the driver accidentally pushes the joystick full forward or backward, or does a rapid reverse operation, the stress on the drive system is very high, and the robot can do a wheelie and fall over.
This is a filter that simply transforms an input value to an output value through a curve. The simplest and most flexible way to express the curve is though an interpolated piecewise linear vector of points. Here are some examples, assuming input and output ranges from 0 to 1:
- output = input: input=[0, 0] output=[1, 1]. This is just a line from 0,0 to 1,1, so the output is equal to the input.
- two speed curve: input=[0,.5,1] output =[0,.25,1]. This will divide the input values by 2 for the lower half, and multiply input values by 1.5 after adding .25 for the second half.
- three speed curve: input=[0, .35, .65, 1] output =[0, .1, .4, 1]. For the first third, the output goes up by only a tenth of the input, then it shifts to about 1 to 1 for the second third, then shifts to about 2 to 1 for the higher speeds.
These examples only show positive numbers, but for full flexibility, having a range with zero at the center probably makes more sense. Also, these examples are not proposals for actual curves, which should be determined experimentally. The class for this would be constructed with an input array for the input and one for the output to define the curve. The main filtering method would take the input value and return the output value. There could also be a range array to scale the output.
I have seen people use polynomial functions, usually cubic curves, for this function, and this basically works. But it is hard to get it right, and the piecewise linear approach allows direct manipulation of the inflection points and makes it easy to deal with dead zones and the non-linearity of the motor controllers. We do not need smooth curves for this function.
Low Pass Filter
The output of the joystick is a curve that corresponds to its physical position plotted against time. The output of the response curve mechanism described above is a transformation of that curve. In either case, the curve is plotted against time, eventually manifesting as acceleration of the wheels on the robot. The full system of floor/drive-train/robot can only tolerate a certain maximum slope of that acceleration curve. A low pass filter serves as a mechanism for limiting that slope. The intent would be to use this only for safety, so when the driver is doing exactly the right thing, the input to the motor controller would be controlled by the joystick positions as filtered by the response curve mechanism above.
The low pass filter is only concerned with the slope of the motor controller input value plotted against time. Its inputs are the output value of the response curve class, the current time, and the maximum allowed acceleration slope. Its output is a "limited" version of the number to be sent to the motor controller. As internal state, it remembers the time it was last called and the last motor controller value it delivered.
The low pass filter class takes the input value (requested speed value) and a time snapshot. It subtracts the old values from the new values to get DeltaValue and DeltaTime. It compares DeltaValue/DeltaTime against the maximum allowed acceration and returns the one with the smaller absolute value. Along the way, it updates it old values from the output value and time snapshot. Note: the input values used by this method must always be up the date, even when the robot is standing still, so the value it uses needs to be updated for every loop of the scheduler. Otherwise the first slope value will be wrong. Behavior examples:
- Driver jams both joysticks to maximum: The input goes from 0.0 to 1.0 very quickly. Say the maximum acceleration value has been determined experimentally to be 1.0 per second. For this example, say the last sampled value was 0.0 at 12:00:00.000 and the current sampled value is 1.0 at 12:00:00.010, which is .01 seconds later. The slope of this is 1/.01, or 100, which is 100 times the allowed value. The method will return .01, which is the value the motor will see. Assuming .01 second between calls, it will take 99 calls before the motor controller will see full power.
- Driver gently moves the joystick forward. Say the input to the low pass filter goes from 0.0 to 0.1 in .2 seconds. The slope of this is .1/.2, which is only .5 per second, so the low pass filter will not limit the acceleration.
This discussion does not specify exactly how stuff would be stored, etc. The object could store the old values, the constructor could take the max acceleration value as an argument and store it in the object, or these values could be stored elsewhere and passed to the object with the method call. We might also find that the acceleration value limit needs to be different at different speeds. In this case, passing an acceleration limit list as a pair of vectors could replace the single number.