PID and Preset Arm Heights Request

Ok,

I’ve taken a few C classes 20 years ago and I’m helping our team right now to determine how to achieve a couple of things. I know I’m all around this thing but I cannot seem to figure out how to make it work. I feel like I’ve read a hundred threads on it.

We have the pot and we know our three measurement stages:

High = 495
Med = 863
Low = 1017

We also know that current our joystick works well as configured:

Channels 4 and 3 = controlling our four wheel motors for primary locomotion

Channel 1 = a straffing motor for gliding side to side locomotion

Channel 6 Button 1 and 2 = controls the manual motion of the arm up and down

Channel 5 Button 1 and 2 = controls the direction of spin for our induction roller (eats the sacks)

Now, what I really need is the ability to have the arm raised and held at a preset height using Channel 8 Buttons 1, 2 and 4.

I’m all around this thing but I cannot get it written/called right. Can anyone help me with a simple primer written that would work with EasyC?

We need the ‘I’ since the arm even with elastic doesn’t hold correctly at the higher positions.

Thanks!

I would like to se this answered. We also tried to use an potentiometer and buttons on the controller, but it would stutter when it was going up. Other than that, a quick fake code that you can use would probably be something like:

While (1)
int potentiometer //make a variable for the potentiometer
//put in your other driver control blocks here
if (lowest position button is pressed)
  if (potentiometer <= 1017 )
  //have the lift motors go down
  else
  //stop motors
if (medium position button is pressed)
  if (potentiometer >= 863)
  //have the lift motors go up
  else
  //stop motors
if (high position button is pressed)
  if (potentiometer >= 495)
  //have lift motors go up
  else
  //stop motors

This is something like what our code looked like. Also, I apologize for any errors in my syntax, as this was just a quick write up. Also, one of the problems with this system is that the arm will jitter at that position because I haven’t figured out a way to keep the motors slightly powered in order to account for different amounts of sacks.

That code will work but it does not take into consideration the fact that you may be above the middle and it does not hold it there. I would incoperate a PID controller or just a P controller at least. The P part can be created with this code:
{
if(Lower button is pressed) TargetHeight = 1070;
else if(Middle button is pressed)TargetHeight = 863;
else if(Hight button is pressed)TargetHeight = 463;

motor[YourLiftMotor] = (TargetHeight - PotetiometerReading)*Constant;
//The constant should be a float value set to around 0.08
}
This is how we do it,there are other ways but this one works well and it holds it in position. This code does not incoperate the manual lift controls but I’m sure you can figure out how to work those in there.

There are so many posts on this topic…

Here is one thread
https://vexforum.com/t/preset-arm-heights-in-easyc-v4/20223/1

and in particular, there is code is this post
https://vexforum.com/showpost.php?p=238942&postcount=14
and this post
https://vexforum.com/showpost.php?p=239353&postcount=25
and this one
https://vexforum.com/showpost.php?p=239365&postcount=28

Now none of these are PID but they could easily be extended to be so. To be honest, all you really need is to add a bias to account for gravity and the weight of the sacks and it will work almost as well, there is so much mechanical slop and delay in controlling motors that a really good closed loop control is hard to achieve. It’s even harder in EasyC than ROBOTC due to the excessive use of blocking wait calls in the EasyC runtime library when using print to screen or the LCD display.

Write some code, post it up if it does not work and we will try and get you going. I would also reverse the pot if you can before you start so higher values indicate the arm is higher up and need positive motor commands to get there, the logic of the code will be cleaner if you do that.

Ok, thanks for the links - most of these I’ve seen already - as I said I get the context of what to do but perhaps am having some syntax errors and procedure issues with getting it done in easyC.

Anyway, using the code posted here just to get my started I came up with the examples below.

However, it won’t compile without giving me an error “void value not ignored as it ought to be”. I think perhaps I’m not correctly passing values back/forth from functions but I’m stuck.

I know it probably needs to be more complicated but I need to at least get something working to start from. Thanks!

ARM_HI_POS, etc. are constants that we’ve set from the known pentiometer values

http://s17.postimage.org/potyo5ajx/code_example_1.png

http://s18.postimage.org/6fy9jxzzr/code_example_1_c.png

It was EasyC but written as a user function rather than the flowchart method.

Images did not show for me (perhaps .png not supported by the forum) , here are links for others. EDIT: Actually I removed the links from this post as they have inappropriate material displayed with them.

This line

SetMotor( 8, arm_target - arm_position ) * 0.8;

should be more like

SetMotor( 8, (arm_target - arm_position) * 0.8);

That’s the P part of the PID

You could improve this by saying

// error and drive need to be declared as a variables
error = arm_target - arm_position;
drive = error * 0.8;
SetMotor(8, drive);

Then gradually add the integral term into that

error = arm_target - arm_position;
integral = integral + error;
drive = (error * 0.8) + (integral * 0.01);
SetMotor(8, drive);

There is more to it than this so get the P part working first.

Hey man, thanks for all the help!!

Actually I found some code that I think you wrote and I was trying to work with that while I was waiting and guess what, I got it to work!! Woohoo!

The only thing it’s not doing is the ‘I’ which I think is needed for the HOLD function to hold the arm at the desired height.

Here is my code which as I said, is working great, except for the HOLD part.

How do I adapt what you mentioned above about the integral into this?

void MyUserControl()
{
    int joyCh1, joyCh2, joyCh3, joyCh4;
    int but8U, but8D, but8R, but8L;
    int butPressed;
    int buttonIsPressed = 0;
    int autoArmControl = 0;
    int desiredArmPosition = 0;
    int armPot;
    int motor[10]; // for debugging
    
    // Init LCD
    InitLCD ( 1 ) ; 
    SetLCDLight ( 1 , 1 ) ; 

    // Do forever
    while(1)
        {
        // simple arcade drive
        joyCh3 = GetJoystickAnalog( 1 , 3 ) ; 
        joyCh4 = GetJoystickAnalog( 1 , 4 ) ;
        joyCh1 = GetJoystickAnalog( 1 , 1 ) ;
        SetMotor ( 1 ,  (joyCh3 + joyCh4) / 2 ) ; 
        SetMotor ( 10 , (joyCh3 - joyCh4) / 2 ) ;
 
        Arcade4 ( 1 , 3 , 4 , 2 , 1 , 4 , 3 , 1 , 1 , 0 , 0 ) ; // Primary 4 Wheel Drive Controls
        JoystickToMotor ( 1 , 1 , 5 , 1 ) ; //Strafing Controls for Motor 5
        JoystickDigitalToMotor (1 , 5 , 1 , 127 , 2 , -127 , 6 ) ; //Induction Roller Controls - Fill
        JoystickDigitalToMotor (1 , 5 , 1 , -127 , 2 , 127 , 7 ) ; //Induction Roller Controls - Release

        // find out which buttons are down
        but8D = GetJoystickDigital ( 1 , 8 , 1 ) ; 
        but8U = GetJoystickDigital ( 1 , 8 , 2 ) ; 
        but8L = GetJoystickDigital ( 1 , 8 , 3 ) ; 
        but8R = GetJoystickDigital ( 1 , 8 , 4 ) ; 

        // combine into a single variable
        butPressed = (but8D << 3) + (but8U << 2) + (but8L << 1) + but8R;

        // Decide what to do
        switch( butPressed )
            {
            case    0:
                // No buttons
                buttonIsPressed = 0;
                break;

            case    1:
                // Right button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_MID_POS;
                    autoArmControl = 1;
                    }
                break;

            case    2:
                // Left button
                break;

            case    4:
                // Up button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_HIGH_POS;
                    autoArmControl = 1;
                    }
                break;

            case    8:
                // Down button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_LOW_POS;
                    autoArmControl = 1;
                    }
                break;

            default:
                // All other cases
                break;
            }

        // See if we are overiding any buttons
        joyCh2 = GetJoystickAnalog( 1 , 2 ) ; 
        
        // don't use joystick near 0
        if( Abs( joyCh2 ) > 10 )
            {
            // no automatic control
            autoArmControl = 0;

            // manual control of motor
            SetMotor ( 8 ,  motor[2] = joyCh2 ) ;
            SetMotor ( 9 ,  motor[2] = joyCh2 ) ;

            // for debugging
            desiredArmPosition = 9999;
            }
        else
        if( !autoArmControl )
            {
            // stop motor
            SetMotor ( 8 ,  motor[2] = 0 ) ;
            SetMotor ( 9 ,  motor[2] = 0 ) ;
            }

        // Read potentiometer on arm
        armPot = GetAnalogInput ( 1 ) ; 

        // Control arm automatically
        if( autoArmControl )
            {
            // Automatic control
            if( Abs( desiredArmPosition - armPot ) < 10 )
                {
                // done
                autoArmControl = 0;
                SetMotor ( 8 ,  motor[2] = 0 ) ;
                SetMotor ( 9 ,  motor[2] = 0 ) ;
                } 
            else
            if( desiredArmPosition < armPot )
                {
                SetMotor ( 8 ,  motor[2] = 100 ) ;
                SetMotor ( 9 ,  motor[2] = 100 ) ;
                } 
            else
            if( desiredArmPosition > armPot )
                {
                SetMotor ( 8 ,  motor[2] = -100 ) ;
                SetMotor ( 9 ,  motor[2] = -100 ) ;
                } 
            }
        
        // display arm pot,requested position and motor speed
        SetLCDText ( 1 , 1 , "pot %d mot %d", armPot, motor[2] ) ; 
        SetLCDText ( 1 , 2 , "req %d", desiredArmPosition ) ; 

        // wait
        Wait ( 25 ) ; 
        }
}

No problem.

That’s what I linked for you originally.

If all you want is hold for the auto arm stuff then you could simply send a non zero hold value.


<<snip>>
        if( !autoArmControl )
            {
            // stop motor
            SetMotor ( 8 ,  motor[8] = 10 ) ;
            SetMotor ( 9 ,  motor[9] = 10 ) ;
            }

        // Read potentiometer on arm
        armPot = GetAnalogInput ( 1 ) ; 

        // Control arm automatically
        if( autoArmControl )
            {
            // Automatic control
            if( Abs( desiredArmPosition - armPot ) < 10 )
                {
                // done
                autoArmControl = 0;
                SetMotor ( 8 ,  motor[8] = 10 ) ;
                SetMotor ( 9 ,  motor[9] = 10 ) ;
                } 
            else
<<end snip>>

(get rid of all the debugging motor[X] = YY code if you want, it’s not needed).

Don’t go above perhaps 20 or you will have motor overheat issues.

Well if you really want PI control then try this. It only implements for the button presents and not for manual.

I did not compiled this and have not tested either, may have bugs, but it gives you an idea of PI control.

void MyUserControl()
{
    int joyCh1, joyCh2, joyCh3, joyCh4;
    int but8U, but8D, but8R, but8L;
    int butPressed;
    int buttonIsPressed = 0;
    int autoArmControl = 0;
    int desiredArmPosition = 0;
    int armPot;
    int motor[10]; // for debugging
    int error = 0;
    int drive = 0;
    int integral = 0;
    float Kp, Ki;
    
    // Init LCD
    InitLCD ( 1 ) ; 
    SetLCDLight ( 1 , 1 ) ; 

    // Do forever
    while(1)
        {
        // simple arcade drive
        joyCh3 = GetJoystickAnalog( 1 , 3 ) ; 
        joyCh4 = GetJoystickAnalog( 1 , 4 ) ;
        joyCh1 = GetJoystickAnalog( 1 , 1 ) ;
        SetMotor ( 1 ,  (joyCh3 + joyCh4) / 2 ) ; 
        SetMotor ( 10 , (joyCh3 - joyCh4) / 2 ) ;
 
        Arcade4 ( 1 , 3 , 4 , 2 , 1 , 4 , 3 , 1 , 1 , 0 , 0 ) ; // Primary 4 Wheel Drive Controls
        JoystickToMotor ( 1 , 1 , 5 , 1 ) ; //Strafing Controls for Motor 5
        JoystickDigitalToMotor (1 , 5 , 1 , 127 , 2 , -127 , 6 ) ; //Induction Roller Controls - Fill
        JoystickDigitalToMotor (1 , 5 , 1 , -127 , 2 , 127 , 7 ) ; //Induction Roller Controls - Release

        // find out which buttons are down
        but8D = GetJoystickDigital ( 1 , 8 , 1 ) ; 
        but8U = GetJoystickDigital ( 1 , 8 , 2 ) ; 
        but8L = GetJoystickDigital ( 1 , 8 , 3 ) ; 
        but8R = GetJoystickDigital ( 1 , 8 , 4 ) ; 

        // combine into a single variable
        butPressed = (but8D << 3) + (but8U << 2) + (but8L << 1) + but8R;

        // Decide what to do
        switch( butPressed )
            {
            case    0:
                // No buttons
                buttonIsPressed = 0;
                break;

            case    1:
                // Right button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_MID_POS;
                    autoArmControl = 1;
                    }
                break;

            case    2:
                // Left button
                break;

            case    4:
                // Up button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_HIGH_POS;
                    autoArmControl = 1;
                    }
                break;

            case    8:
                // Down button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_LOW_POS;
                    autoArmControl = 1;
                    }
                break;

            default:
                // All other cases
                break;
            }

        // See if we are overiding any buttons
        joyCh2 = GetJoystickAnalog( 1 , 2 ) ; 
        
        // don't use joystick near 0
        if( Abs( joyCh2 ) > 10 )
            {
            // no automatic control
            autoArmControl = 0;

            // manual control of motor
            SetMotor ( 8 ,  motor[8] = joyCh2 ) ;
            SetMotor ( 9 ,  motor[9] = joyCh2 ) ;

            // for debugging
            desiredArmPosition = 9999;
            }
        else
        if( !autoArmControl )
            {
            // stop motor
            SetMotor ( 8 ,  motor[8] = 0 ) ;
            SetMotor ( 9 ,  motor[9] = 0 ) ;
            }

        // Read potentiometer on arm
        armPot = GetAnalogInput ( 1 ) ; 

        // Control arm automatically
        if( autoArmControl )
            {
            // error is positive if desiredArmPosition > armPot
            error = desiredArmPosition - armPot;

            // calculate integral and limit to avoid windup
            integral = integral + error;
            if( integral > 100 )
                integral = 100;
            if( integral < (-100) )
                integral = -100;
            
            // I put these here for clarity
            // edit as needed
            Kp = 1.0;
            Ki = 0.1;
            
            // calculate motor drive, PI control
            drive = (error * Kp) + (integral * Ki);

            // limit out of habit
            if(drive > 127)
                drive = 127;
            if(drive < (-127))
                drive = (-127);
                
            // but the pot is installed backwards so we invert
            drive = -drive;            
            
            SetMotor ( 8 ,  motor[8] = drive ) ;
            SetMotor ( 9 ,  motor[9] = drive ) ;
            
            // autoArmControl will never be cleared in this version            
            }
        
        // display arm pot,requested position and motor speed
        SetLCDText ( 1 , 1 , "pot %d mot %d", armPot, motor[8] ) ; 
        SetLCDText ( 1 , 2 , "req %d", desiredArmPosition ) ; 

        // wait
        Wait ( 25 ) ; 
        }
}

This is a much simpler way to preset the heights and hold it there as well as incorporate the manual control. We use buttons to lift instead of a joystick but the idea still works.

void LiftControl()
{
	if ((BtnLiftHigh == 1)||(BtnLiftHigh2 == 1)) LiftTarget = TLPos;
//TroughLiftPosition
	if ((BtnLiftMid == 1)||(BtnLiftMid2 == 1))	LiftTarget = MLPos;
//MiddleLiftPosition
	if ((BtnLiftGround == 1)||(BtnLiftGround2 == 1)) LiftTarget = GLPos;
//GroundLiftPosition

	if ((BtnLiftUp == BtnLiftDown) && (BtnLiftUp2 == BtnLiftDown2))
	{
		if(LiftTarget == 0) LiftTarget = SenLiftPot;
		SenLiftPotError = LiftTarget-SenLiftPot;
		SenLiftPotIntegral = SenLiftPotIntegral*(3/4)+SenLiftPotError;
		SenLiftPotDerivative = SenLiftPotError - SenLiftPotErrorLast;
		MtrLift = ((float)SenLiftPotError*SenLiftKp)+(SenLiftPotIntegral*SenLiftKi)+(SenLiftPotDerivative*SenLiftKd);
		SenLiftPotErrorLast = SenLiftPotError;
	}
	else if ((BtnLiftUp == 1) || (BtnLiftUp2 == 1))   			{MtrLift = Up;			LiftTarget = 0;}//defaults to driver 1
	else if ((BtnLiftDown == 1) || (BtnLiftDown2 == 1)) 		{MtrLift = Down;		LiftTarget = 0;}
}

It is simple to write and it works well once you set Ki, Kp, and Kd to correct values. I also includes the second joystick controls.

Ok, thanks guys. Now, how do I calculate Ki, Kp etc? Is there a range or some accepted value to use?

Thanks!

Tuning the PID loop is the hard part, it depends a lot on your mechanical design and the gear ratios etc. used. My favorite paper on PID is here.

I cleaned up the code a little, there was some residual arcade drive code left in addition to the arcade4 function you were calling. I also added the derivative term and something I call Kbias which compensates for gravity. I don’t think you will really need the integral term, the problems is that this will cause overshoot to the position control and then need the error to be of the opposite sign to reduce it.

I would also encourage you to rotate your potentiometer a little to get away from the LOW position of 1017. I’m assuming here you were reading these with 10 bit precision (GetAnalogInput) so 1017 is very close to the maximum value of 1023. Rotate the pot so you values are more like, 295, 663 and 817 (subtracted 200). Also bear in mind that you will always have some small error so rounding them would also work just as well (300, 670, 820 for example).

The PID control is only enabled during preset button control and not when in manual mode. One reason to do this is so that you have a way to completely remove power if the PTC (thermal fuse) in the motor trips. The only way to allow this to reset is to send a value of 0 to the motor, if the PID is running then this will not happen unless you have some other type of override.

I also notice that you have all your drive motors on one bank of the cortex, this can be a source of drive failure (unless you are using a power expander) as the four motors are sharing one 4A PTC in the cortex, you may wish to reconsider this arrangement and split them across the two banks.

Anyway, here is the revised code and also an EasyC project with the same thing, I checked the PID on a robot with an arm geared 8.33:1 but did not test the drive.

// UserFunctions.c : implementation file
#include "Main.h"

// 
#define ARM_HIGH_POS  495
#define ARM_MID_POS   863
#define ARM_LOW_POS  1017
//#define ARM_HIGH_POS  450
//#define ARM_MID_POS   675
//#define ARM_LOW_POS   880


// Motor definitions
#define ARM_MOTOR_1  8
#define ARM_MOTOR_2  9

#define ITK_MOTOR_1  6
#define ITK_MOTOR_2  7

#define DRV_MOTOR_1  2
#define DRV_MOTOR_2  1
#define DRV_MOTOR_3  4
#define DRV_MOTOR_4  3

#define STR_MOTOR_1  5



#define Sgn(x) ((x) > 0 ? 1 : -1)

void MyUserControl()
{
    int joyCh2;
    int but8U, but8D, but8R, but8L;
    int butPressed;
    int buttonIsPressed = 0;
    int autoArmControl = 0;
    int desiredArmPosition = 0;
    int armPot;
    
    int error = 0;
    int last_error = 0;
    int integral = 0;
    int derivative = 0;
    int drive = 0;   
    float Kp, Ki, Kd, Kbias;
    
    // Constants for PID, adjust as necessary
    Kp    = 0.6;
    Ki    = 0.0;       
    Kd    = 0.5;
    Kbias = 10;

    // Init LCD
    InitLCD ( 2 ) ; 
    SetLCDLight ( 2 , 1 ) ; 

    // Do forever
    while(1)
        {
        // Primary 4 Wheel Drive Controls
        Arcade4 ( 1 , 3 , 4 , DRV_MOTOR_1 , DRV_MOTOR_2 , DRV_MOTOR_3 , DRV_MOTOR_4 , 1 , 1 , 0 , 0 ) ;

        //Strafing Controls for Motor 5
        JoystickToMotor ( 1 , 1 , STR_MOTOR_1 , 1 ) ;

        //Induction Roller Controls - Fill/Release
        JoystickDigitalToMotor (1 , 5 , 1 , 127 , 2 , -127 , ITK_MOTOR_1 ) ;
        JoystickDigitalToMotor (1 , 5 , 1 , -127 , 2 , 127 , ITK_MOTOR_2 ) ;

        // find out which buttons are down
        but8D = GetJoystickDigital ( 1 , 8 , 1 ) ; 
        but8U = GetJoystickDigital ( 1 , 8 , 2 ) ; 
        but8L = GetJoystickDigital ( 1 , 8 , 3 ) ; 
        but8R = GetJoystickDigital ( 1 , 8 , 4 ) ; 

        // combine into a single variable
        butPressed = (but8D << 3) + (but8U << 2) + (but8L << 1) + but8R;

        // Decide what to do
        switch( butPressed )
            {
            case    0:
                // No buttons
                buttonIsPressed = 0;
                break;

            case    1:
                // Right button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_MID_POS;
                    autoArmControl = 1;
                    }
                break;

            case    2:
                // Left button
                break;

            case    4:
                // Up button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_HIGH_POS;
                    autoArmControl = 1;
                    }
                break;

            case    8:
                // Down button
                if( !buttonIsPressed )
                    {
                    buttonIsPressed = 1;
                    desiredArmPosition = ARM_LOW_POS;
                    autoArmControl = 1;
                    }
                break;

            default:
                // All other cases
                break;
            }

        // See if we are overiding any buttons
        joyCh2 = GetJoystickAnalog( 1 , 2 ) ; 
        
        // don't use joystick near 0
        if( Abs( joyCh2 ) > 10 )
            {
            // no automatic control
            autoArmControl = 0;
            drive = joyCh2;

            // for debugging
            desiredArmPosition = 9999;
            }
        else
        if( !autoArmControl )
            {
            // stop motor
            drive = 0;
            }

        // Read potentiometer on arm
        armPot = GetAnalogInput ( 1 ) ; 

        // Control arm automatically using PID
        if( autoArmControl )
            {
            // error is positive if desiredArmPosition > armPot
            error = desiredArmPosition - armPot;

            // calculate derivative
            derivative = error - last_error;
            last_error = error;

            // calculate integral and limit to avoid windup
            integral = integral + error;
            if( Abs(integral) > 500 )
                integral = Sgn(integral) * 500;
                         
            // calculate motor drive, PI control
            drive = (error * Kp) + (integral * Ki) + (derivative * Kd ) + Kbias;

            // limit out of habit
            if( Abs(drive) > 127)
                drive = Sgn(drive) * 127;            

            // but the pot is installed backwards so we invert
            drive = -drive;            
                   
            // autoArmControl will never be cleared in this version            
            }
        
        // send drive to motors
        SetMotor ( ARM_MOTOR_1 , drive );
        SetMotor ( ARM_MOTOR_2 , drive );

        // display arm pot,requested position and motor speed
        SetLCDText ( 2 , 1 , "pot %d mot %d", armPot, drive ) ; 
        SetLCDText ( 2 , 2 , "Kp  %.1f err %d", Kp, error ) ; 

        // wait
        Wait ( 25 ) ; 
        }
}


PidTest.zip (3.94 KB)

Thanks again - I got the one with the ‘hold’ code working yesterday but I’m still going to try and get the full PID working so this is much appreciated. Something was up with the other code - it was reading each of the buttons correctly as a requested arm position level but only the down button actually did anything and it actually tried to go up instead of down. So, anyway, will try this and see if I can get it working. Thanks again!

Hey,

What is the purpose of this piece of code?


Sgn(x) ((x) > 0 ? 1 : -1)

And what is the significance of Kbias?

This latest code you sent is working functionally very well but I need to tune it as the high location seems to stop about 40 points short and the error stays higher - the mid location stops about 20 points short and the low position hits the target with no issue.

Is this where I start playing with the PID constants you mentioned?

EDIT** - I actually played with the P and raised it so now I’m getting to the correct height - however I’m getting some overshoot and it takes it a second to regulate it - I think this is where the I and D comes in based on the article you sent so I’m going to work on that next.

The other code had a couple of small bugs, I did fix them but you had probably downloaded before that.

It’s a macro that tests the sign of the given argument written as a tertiary statement. It does the same thing as a function written like this.

int Sgn( int x )
{
    if( x > 0 )
        return(1);
    else
        return(-1);
}

I’m using it as a shortcut to limit a variable, so


drive = Sgn(drive) * 127; 

expands to

drive = ((drive>0) ? 1 : -1) * 127; 

and basically does the following

if( drive > 0 )
    drive = 1 * 127;
else
    drive = -1 * 127;

I’m not sure where this idea came from but I use Kbias to compensate for the error caused by gravity.

Lets say the arm is going up and the error to the target value is 100, using the Kp constant of 0.6 I had in last nights code, the drive value created would be 60 (if Kbias was 0). If the arm was going down then the error may be -100 and the drive -60, however, a drive of -60 will tend to cause the arm to descend much faster than a value of 60 will cause it to rise. I use Kbias to offset these numbers creating larger values when going up compared to going down. If Kbias is 10 then the drive values created in the example above would be 70 and -50 respectively.

If you are happy with the response of the arm but have a small constant error then the easy way is to adjust the target points and not worry about any residual error. Increasing Kp will create more drive for small errors but in doing this may cause the arm to overshoot the target position and eventually go into oscillation. You also want to monitor the drive value when holding at a given position, if the drive is over a value of say 20 then you will be in danger of overheating the motors if you hold at that position for any significant time. Does you arm have any elastic assistance? Which motors are you using, I assume 393s as I doubt 269s would have sufficient power. What is you gear ratio?

I really would avoid using the integral, the derivative helps control overshoot by adjusting drive based on the change of error. Be aware that it’s not necessary to control the arm so the absolute error is reduced to zero, you want a good response and minimal power sent to the arm in the hold position. If the arm is not quite at the correct height then change the target value until it is.

I’ve posted this link before but it is one of my favorite papers on PID for beginners.

http://igor.chudov.com/manuals/Servo-Tuning/PID-without-a-PhD.pdf

1 Like

Hey jpearman,

Just wanted to say thanks again. With the new PID code the team won the overall at their tournament today. Take care!

You are welcome. Congratulations on winning, did that qualify you for worlds and if so will you be coming to Anaheim?

Hey,

No, they are in a regional league and so this was event 3 out of 5. If they win the overall league they will get the World’s invite. This particular team actually attended World’s last year in their first year as an excellence award winner and they are trying hard to get back. It was an eye-opener last year to how much we didn’t know about how to be successful at an event like World’s.