Set Motor position with Encoder?

Technical Programming
1

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!

2

You can’t do this without the use of some other kind of sensor, as the encoder value resets to zero when the code is rebooted.

What you could do, and many teams did in the past with Recycle Rush elevators, is use a limit switch that resets the encoder count to zero (wherever the arm is when it hits the switch would theoretically be zero degrees), and use closed loop control to move and hold the arm at a certain number of encoder ticks.

Or if you’re always going to be booting the robot up with the arm at zero degrees, you wouldn’t even need a limit switch, just a way to reposition it to the same place every time you want to boot or reboot the robot.

3

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.


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.


//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).

1 Like
4

Yes, you can do that with the hardware you described, but you’ll have to have a reference position. If you always boot the robot with the mechanism in a known position (like against a hard stop), that can be your reference. That reference can be any value you choose.

Then you need to do what’s called “closed-loop position control”.

Most teams use a PID closed-loop controller for this. Try searching for those keywords and you’ll get lots of relevant hits.

5

This is a common need for swerve drives, I’m not a programmer at all but if you can find some swerve drive code (I know FRC 1640 posts a bunch of swerve drive stuff) you can probably figure out how they do it. The AndyMark swerve module actually uses this exact motor for steering.

6

Note the following from the Swerve & Steer product page.

This is the same issue that Ether and I described. Since the encoder is incremental, there is a mechanical feature to make sure that all of your modules are aligned in the same direction at the beginning of the match (so that zero is always in the same place). Alternatively, there are holes on the plate and an additional gear available as a separate purchase to mount the MA3 absolute encoder to the module.