This year, our shooter direction is opposite of our front of our robot. I decided that we should use the NavX we just got to semi-accurately turn ourselves around. We don’t have any encoders on our motors so I just made a simple function that would have the motors turn until it hit a 10 degree range in which case it would stop.
The problem is that it doesn’t always work. It works 9/10 times but that one time it just spins endlessly in a circle and never stops. This, of course, is not something you want to happen. I am asking if there is a better way to go about programming this so it ALWAYS works? Here is the current function:
#if button pressed, start
if self.turn_button.get():
self.turn_state=0
yaw=self.navx.getYaw()
#Our NavX is not completely centered on the robot so
#that is why it isn't 180 degrees for turning around
if yaw > 0:
self.desired=self.navx.getYaw()-227
else:
self.desired=self.navx.getYaw()+133
current=self.navx.getYaw()
if self.turn_state==0:
self.turn_state=1
elif self.turn_state==1:
if current < (self.desired+10) and current > self.desired:
self.turn_state=2
else:
self.drive2.set(.7)
self.drive1.set(.5)
This example shows how to implement this, using Mecanum drive.
Some PID Tuning is required, so you’ll want to read up on how to use PID (there are lots of links to good info here on ChiefDelphi). You should be able to implement something that works reasonably well by only using a P coefficient, though to get it to work well w/your specific drive system you may need to do additional tuning of the I and D coefficients.
Oh I guess I just under the assumption we needed encoders to do PID. Cool that we don’t!
So the online description is not the full code? If not, do you have a Github page or something because we use Python and don’t have the necessary software to install nax-mxp example.
You would need encoders if you wanted to have PID control of each individual wheel’s speed. Note that this can be a useful thing to have (kind of like traction control on a car).
But for rotate-to-angle, your feedback that drives the PID controller is the Yaw angle from the navX-MXP.
Anyways, the RotateToAngle Java example code is available on GitHub at:
My understanding is that the java and python are very similar, so this should be a good starting point for you. There are some comments in the example code that discuss tuning the PID a bit.
PID is a very useful thing to learn and get used to for programming control systems like those used in FIRST and in the real world, so I encourage you to spend the time to learn it. Focus first on understanding how the P (proportional) coefficient works. Then, once you understand that and the concept of undershoot and overshoot well, begin working on the I (integral) coefficient. Finally, if you find this is not good enough for your needs, begin looking at the D (derivative) coefficient.
A final bit of advice when tuning PID coefficients is to only change one coefficient at a time. The different coefficients “mix” together in ways that aren’t always intuitive, so go slowly and take careful notes as you tune them.
I am revisiting this topic and am having trouble using the example with arcadeDrive. What values, besides the joysticks, do I pass to arcadeDrive so it will turn if the button is pressed? I pass in the currentRotationRate into arcade Drive but in the simulator, the motors still don’t do anything.
We have found that a simple piece wise function is accurate enough to use for rotating to angles. We have a tolerance of two degrees. It works very well. We call this little set of functions Tigerdrive.
We are using a well tuned PID loop on the angle returned by the navX in order to turn accurately. An issue that we ran into, which I think might explain what you described (your code working only 9/10 times), is that at certain absolute angles, if you simply add or subtract 180 (or in your case the slightly altered values), then you might get a value which is outside the range of the navX. We use the navX getFusedHeading() method as opposed to the getYaw() method, so I’m not sure how the values returned by these two methods differ. But essentially, since the navX returns values modulo 360 (for the getFusedHeading() method), if you want any value you calculate to be within the range 0-360, you can perform some mathematical modulo function on it. For instance, ((desired angle + 360) % 360) will map any angle greater than -360 to an angle between 0 and 360. On the other hand, (((desired angle + 540) % 360) - 180) will map any angle greater than -360 to an angle between -180 and 180 if you would prefer that. You can work out what would be best for you, but I hope this might help explain your issue.
In regards to PID, I would highly suggest, like most everyone else replying here, that you look into it. It is fairly simple and can drastically increase the accuracy, speed, and smoothness of your turning.
EDIT: I actually looked at the reference, and found that getYaw() returns values between -180 and 180. Notice that if it is greater than 0, you subtract 227. This would suggest that you could possibly be trying to turn to the angle -226, which the getYaw() method will never return. Instead you might want to take the angle and map it to -180 to 180 using a function like the second one I described. This way, -226 will be turned into +134, which is a value which will work with what the navX returns.
In theory, PID is great. In practice, not so much.
In this exercise, you are trying to turn a robot to a set angle. PID will increase the speed until you get there, and then oscillate back and forth trying to hit the mark. The easier way to do it is to turn at a controlled speed until you get there.
In the original code, I’m guessing that the robot is rotating too fast, and overshoots the error range (self.desired to self.desired+10) that one time in 10. The solution is to turn slower, check the angle more frequently, wider error range. Maybe when you get close, you slow down the turn.
We use the Analog Devices supplied gyro without encoders with a PI controller for an algorithm to turn the robot to a set absolut degree. With a PI controller you can easily get within a half of a degree or better.
I’ve added a navx_rotate_to_angle_arcade example to the set of samples. If you look at the diff, there’s basically two changes that I made:
Changed the physics implementation to call the appropriate functions for a 4 wheel drivebase instead of mecanum (2 small changes)
instead of calling mecanumDrive_Cartesian it calls arcadeDrive and passes the rotation rate as the second argument
You’ll note that arcade drive does not accept the same arguments as mecanum drive – in particular, it does not accept extra ‘rotation’ or ‘gyroangle’ arguments.
One can easily cap the max speed of a P loop (let’s be honest, for turn-to-angle you’re probably not going to need any I or D) or tune the P term to avoid overshoot. In fact, one of the appeals of a P loop is precisely that the output ramps down as you approach the setpoint, such that you do not overshoot. In this capacity, it is quite a bit nicer than bang-bang control (which is what you are describing).
This is unnecessary with the NavX and WPILib, as one can simply call AHRS pidGet() (which always ranges from -180 to 180) and ensure that the PIDController object is set to have a continuous range and not have to ever explicitly do any of the math.
I was responding to DanielHa’s post, to show a simple way to compute shortest angle… which would be useful to know for situations where the library functions or the hardware don’t do the math for you.
