Quote:
Originally Posted by team-4480
Hi,
I tried searching this problem all over and was surprised that I couldn't find anything. Maybe I had the wrong keywords or something....
Anyways, I want to be able to control our PG71 with the included encoder with servo like control. On a servo, you can set it to go to angle 162 and it would be really accurate. I would use a servo, but we need a lot of torque and the servos that are legal are not even close to powerful enough. So basically, I want to be able to say to the motor, go to position 5 and it would go there accurately each time.
From researching this, I did find in FTC they have a "setTargetPosition" for their encoders, but I couldn't find any information on the FRC documentation of a similar function. Any help would be greatly appreciated!
|
There are two sides to this, the sensor side and the programming side.
Sensor side
---------------
You won't be able to do this solely with the PG71. The encoder on the PG71 is an incremental encoder, it only measures how much the shaft has moved since the robot has turned on. You can use this to tell the motor "turn X degrees", but you cannot tell the motor to "go to angle Y" because it has no sense of an absolute 0. 0 is wherever it was when the robot turned on.
In order to resolve this, you have two options. The first is to add some sort of switch somewhere along the mechanisms path. This way, you know that whenever the arm hits the switch, it's at x angle. Now since you know that specific angle, and how much it has moved from that angle, you know exactly where the arm is at.
The other solution would be to use an absolute encoder, such as
http://www.andymark.com/product-p/am-2899.htm. This type of encoder keeps the same zero position, even though robot reboots. If you turn the robot off with the arm at 60 degrees, when you turn the robot back on, the sensor will still report 60 degrees (instead of zero like the built-in PG71 encoder would).
Programming side
-----------------------
You will now need to use the encoder in some sort of feedback loop to control the mechanism. One of the simplest forms of this is a bang-bang controller. Here's some psuedocode.
Code:
if (armAngle > target) {
setMotor(-1);
}
else if (armAngle < target) {
setMotor(1);
}
else {
setMotor(0);
}
Now obviously this would cause the arm to rapidly oscillate around the target value. However, you can see the concept; we are choosing what value to send the motor based on the value of the encoder. The most common type of algorithm to use to control the position of something is a PID loop. This is what the servo is doing internally to control it's position. PID loops look something like this.
Code:
//These are constants
Kp = xxx.xxx;
Ki = yyy.yyy;
Kd = zzz.zzz;
lastError = 0;
sumOfErrors = 0;
while (error > tolerance) {
error = position - target;
P = Kp * error;
I = Ki * sumOfErrors;
D = Kd * (error - lastError);
setMotor(P + I + D);
lastError = error;
sumOfErrors = sumOfErrors + error;
}
There should be a PID function built into the WPI library. This doesn't really work straight out of the box, you'll need to tune those initial constants at the beginning, as they will be unique to the mechanical system. Some of them might be zero as well (just running P, PI, PD, etc).