Simple P controller for arm position

VEX V5 General Discussion Technical Discussion
#1

So for the start of a new year I thought I would post some code for a simple (and I mean simple) proportional controller. The question of arm control seems to come up a lot so this can be a thread for posting different techniques of achieving that. This is what we are implementing.

(click for original)

A potentiometer connected to the arm measures position, this can vary between 0 and 4095 but will in practice be a smaller range, in the example code I use 400 to 3000 but you need to determine this by experimentation. A single motor is driving the arm as the example uses a clawbot as it’s basis. The assumption is that positive command values will make the arm raise and the potentiometer values increase, I did not check the example code on an actual clawbot, perhaps I will do this over the weekend. If potentiometer values decrease then remove the pot and attach it the other way around so they do.

Here is the example code in ROBOTC, this code uses one task to run the P controller, the constant Kp will need adjusting depending on the arm weight and the gear ratio. I made no attempt to tune it here, again, perhaps I will test on a clawbot this weekend.

#pragma config(Sensor, in1,    armPot,        sensorPotentiometer)
#pragma config(Motor,  port1,  motorA,        tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// These could be constants but leaving
// as variables allows them to be modified in the debugger "live"
static float  pid_Kp = 0.5;
static float  pidRequestedValue;

/*-----------------------------------------------------------------------------*/
/*  pid control task                                                           */
/*-----------------------------------------------------------------------------*/

task pidController()
{
    float  pidSensorCurrentValue;
    float  pidError;
    float  pidDrive;

    while( true )
        {
        // Read the sensor value and scale
        pidSensorCurrentValue = SensorValue armPot ];

        // calculate error
        pidError =  pidRequestedValue - pidSensorCurrentValue;

        // calculate drive
        pidDrive = (pid_Kp * pidError);

        // limit drive
        if( pidDrive > 127 )
            pidDrive = 127;
        if( pidDrive < (-127) )
            pidDrive = (-127);

        // send to motor
        motor motorA ] = pidDrive;

        // Don't hog cpu
        wait1Msec( 25 );
        }
}

/*-----------------------------------------------------------------------------*/
/*  main task                                                                  */
/*-----------------------------------------------------------------------------*/

task main()
{
    // set initial position as the value of the pot
    pidRequestedValue = SensorValue armPot ];

    // start the PID task
    StartTask( pidController );

    // use joystick to modify the requested position
    while( true )
        {
        // presets - assume joystick is centered
        if( vexRT Btn8U ] == 1 )
            pidRequestedValue = 1800;
        else
        if( vexRT Btn8D ] == 1 )
            pidRequestedValue = 800;

        // manual control
        if( abs(vexRT Ch2 ]) > 10 )
            {
            pidRequestedValue = pidRequestedValue + (vexRT Ch2 ]/2);

            // crude limiting to upper and lower values
            if( pidRequestedValue > 3000 )
                pidRequestedValue = 3000;
            
            if( pidRequestedValue < 400 )
                pidRequestedValue = 400;
            }

        // don't hog cpu
        wait1Msec(50);
        }
}

The same code in EasyC, I did not use a repeating timer for this example, this is the most basic code with everything in one loop.

#include "Main.h"

void OperatorControl ( unsigned long ulTime )
{
    float pidRequestedValue; 
    float pid_Kp = 0.5; 
    float pidSensorCurrentValue; 
    float pidError; 
    float pidDrive; 

    pidRequestedValue = GetAnalogInputHR ( 1 ) ;
    while ( 1 ) // Insert Your RC Code Below
        {
        pidSensorCurrentValue = GetAnalogInputHR ( 1 ) ;
        pidError = pidRequestedValue - pidSensorCurrentValue ;
        pidDrive = pidError * pid_Kp ;
        if ( pidDrive > 127 )
            {
            pidDrive = 127 ;
            }
        if ( pidDrive < -127 )
            {
            pidDrive = -127 ;
            }
        SetMotor ( 1 , (int)pidDrive ) ;
        // User input 
        if ( GetJoystickDigital( 1, 8, 2 ) )
            {
            pidRequestedValue = 1800 ;
            }
        else if ( GetJoystickDigital( 1, 8,1 ) )
            {
            pidRequestedValue = 800 ;
            }
        if ( ( GetJoystickAnalog( 1, 2 )  > 10) || (GetJoystickAnalog( 1, 2 )  < -10) )
            {
            pidRequestedValue = pidRequestedValue +  (GetJoystickAnalog( 1, 2 ) / 2) ;
            if ( pidRequestedValue > 3000 )
                {
                pidRequestedValue = 3000 ;
                }
            if ( pidRequestedValue < 400 )
                {
                pidRequestedValue = 400 ;
                }
            }
        Wait ( 25 ) ;
        //PrintToScreen ( "drive %d %d %d\n" , (int)pidRequestedValue, (int)pidSensorCurrentValue, (int)pidDrive ) ;
        }
}

Joystick channel 2 is used for manual control.
Buttons 8 up and down are used for two presets.

Code is attached to save you typing it in.

enjoy.
p_controller.zip (5.35 KB)

2 Likes
Code to hold clawbot in up position
#2

Thanks for the post. The diagram is very useful for understanding the idea behind P(ID), and the code is simple to follow.

If the arm is carrying a relatively heavy load, wouldn’t the code result in the arm oscillating around pidError = 0?

#3

Several factors will affect oscillation, if the arm is tuned without a load then when the load is added it will tend to fall short of its target with a resulting constant error if raising and perhaps overshoot if falling (due to gravity).

The proportional constant also effects greatly how much overshoot there will be. Here is the response of an idealized system where the motor has a linear acceleration, several different values of Kp are shown. The green trace shows the change in requested position, the various other traces show how the arm moves, with Kp = 1 it does not achieve a stable position (although it may do eventually).

response.jpg

Large values of Kp will cause oscillation, small values give a slow response.

2 Likes
#4

If the Kp is too high, yes you get oscillation since you go past the target. But as you tune Kp down, the effect is more like approaching the target kind of slowly. Tune too low you probably will not reach the target and stay below a bit.

Taken from the paper “PID without a PHD”. http://www.embedded.com/design/embedded/4211211/PID-without-a-PhD or other places but a PDF link is right in the article too.

http://i.cmpnet.com/embedded/gifs/2000/0010/0010feat3fig7.gif

Mass on an arm like this has the nastiness of gravity working for you one way and against you the other. So the force effect when in downward motion is greater than that of the upward one so you blow by the target worse on the way down. But it’s coded as just one value of Kp regardless of direction. Darn.

Examples of various Kp values on top of each other. See how some blow by the target and oscillate, while some go under.
http://i.cmpnet.com/embedded/gifs/2000/0010/0010feat3fig8.gif

Which leads you down the road of making a neutral weighted arm if possible (rubber bands, black tubing, counter weights, lighter arm, etc). Or the road of harder to learn control mechanisms like PID and other tricks control-wise which is not necessarily the point of this tutorial.

1 Like
#5

Thanks for the details.

My reasoning for the use of PID for controlling the simple arm is as follows. Am I on the right track? Or, am I missing something?

Let’s say we have designed and painstakingly tuned an almost perfect PID control for the robot arm using P, I and D terms. Then, the control signal consists of three terms:

U = kP * Error + kI * (sum of errors at time steps) + kD * (change in error over time).

When the arm reaches the state of equilibrium; it reaches the target position and stays at that position for the duration of at least one time interval, the first and third terms of the above equation vanish. However, the second term (integral of the error) most likely will not reach zero. Therefore, the motor continues to stay on preventing the arm from moving down due to gravity. Without the integral term, PID will not be very effective for robot arm control of this sort.

#6

I’m going to cover full PID in another post, however, P control can be acceptable if a small error in position is also acceptable. I also often use a term I call Kbias which I use to compensate for gravity, a fixed offset that is always added to the drive. In general I use PD control for arms, I try and avoid I if possible due to changing arm weight. You may want to have a look at the library I published a few weeks ago.

https://vexforum.com/t/pid-library/23257/1

and also this code from 18 months back (pre robotc pointers)

https://vexforum.com/t/a-pid-controller-in-robotc/20105/1

2 Likes
#7

May I suggest a generic clamp function?

int clamp(int val) {
	return ((val > 127) ? 127 : (val < -127) ? -127 : val) + 127;
}

It just seems like one of those things that you might want to use elsewhere. Although your example is easier to read, which may be a big plus.

-Cody

#8

Well, sure, but that was not the point of the post. In general I always avoid small functions this this unless I can either inline them or write as a macro, the overhead of the function call can become significant. The above is exactly the same as I wrote if I had included the additional else between the two tests (as both cannot be true). In ROBOTC they generate exactly the same code.


// limit drive
if( pidDrive > 127 )
0022: 27015700002D08000C00         TestAndBranchFloat(127.000000 >= pidDrive:S08(float), L0036)
    pidDrive = 127;
002C: 292D08000000FE42             pidDrive:S08(float) = 127           // Assign 4-byte constant to 'long/float'
0034: 7913                         BranchNear(L0048)                   // If end
else
if( pidDrive < (-127) )
0036: 27005704002D08000A00 L0036:  TestAndBranchFloat(-127.000000 <= pidDrive:S08(float), L0048)
    pidDrive = (-127);
0040: 292D08000000FEC2             pidDrive:S08(float) = -127          // Assign 4-byte constant to 'long/float'

//


pidDrive = (pidDrive > 127) ? 127 : (pidDrive < -127) ? -127 : pidDrive;
0022: 27015700002D08000C00         TestAndBranchFloat(127.000000 >= pidDrive:S08(float), L0036)
002C: 292D08000000FE42             pidDrive:S08(float) = 127           // Assign 4-byte constant to 'long/float'
0034: 7913                         BranchNear(L0048)                  
0036: 27005704002D08000A00 L0036:  TestAndBranchFloat(-127.000000 <= pidDrive:S08(float), L0048)
0040: 292D08000000FEC2             pidDrive:S08(float) = -127          // Assign 4-byte constant to 'long/float'

//

The limiting is also not strictly necessary in ROBOTC but it makes the code more portable. I also used floats for some of the variables which was unnecessary but for example code does not really matter.

2 Likes
#9

If the weight of the are changes( with or without game pieces) will it change the tuning of the p control? Obviously it depends on the wight if the object and other factors, but in general will it make a difference?

#10

Testing shows Kp needs to be about 0.3.

It will have some effect, when going up the error at rest will be larger, when going down it will have more overshoot (but then again going down only works marginally with this simple controller).

2 Likes
#11

Very interesting post as I’m trying to re-learn for myself and explain to some middle schoolers how PID helps improve control. I’m curious why this line of code is included. Why do you need to divide the joystick input by half and add it to the pidRequestedValue ?

#12

It allows manual control of the arm, as the joystick is pushed forwards or backwards the arm will move up or down. The divide by 2 is just to make the movement slower. The loop this code is in runs 20 times per second, maximum change per second would be 50*63 (127/2) or 3150 counts.

1 Like
#13

Oh, okay. I thought maybe it was something deeper than that. Thanks. :slight_smile:

#14

I know this is an old line but I was having trouble keeping my arm at a specific destination. To be accurate, the lower position of my arm. I have a PID on the arm to help me control it better. I have a similar PID to the code above. Could anyone help me out with this? Thanks.

#15

How would one go about maintaining the alignment of a claw using PID and two potentiometers on either side (this claw would be without any mechanical gearing keeping it aligned)?

#16

Not sure if this question is really related to the topic in this old thread, perhaps start a new one.

#17

like our claw? :wink:

How we did it was having two separate PI loops on each claw. (just P loops work fine) and then we had multiple triggers throughout the program that would call an “align” function. Basically, whenever we put our lift all the way down, or if we’ve been sitting still for a couple seconds, or even if we manually click a button then it averages the two targets of each of the loops. Works pretty well, we prefer it to having code actively try to synchronize our claws.

If you have any more questions dm me or hit me up at my email fam :slight_smile:

#18

I’m new to PID. I have read the sample code(thanks jpearman). The arm motor will generally always be moving; correct(very little once it has reached the target)? Unless the unlikely event that the arm is in equilibrium?

#19

@pietrofesar please avoid reviving long dead threads. Next time just create a new thread and link back to this thread. It makes threads much easier to follow.

#20