View unanswered posts | View active topics It is currently Thu Jul 31, 2014 1:45 pm






Reply to topic  [ 6 posts ] 
Trouble with PID control 
Author Message
Rookie

Joined: Wed Feb 18, 2009 4:06 pm
Posts: 12
Post Trouble with PID control
I am having great difficulty with PID control. I am using the simple sample program below:

#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, PIDControl)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

task main()
{
motor[motorD] = 100;
motor[motorE] = -100;
wait1Msec(5000);

motor[motorD] = 0;
motor[motorE] = 0;

PlaySound(soundBeepBeep);
}

Note: motor E is reversed from motor D so I need to use different sign.

When I run with power 100 (and -100) the robot goes straight
When I run with power 50 (and -50) the robot curves noticeably

It seems the pid control is not working properly (or I don't know how to use it)

do I need a command such as this? nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg; (I can't find any documentation on it)

the encoders seem to be wired correctly (although I wonder about clockwise vs counterclockwise mounting?). Also, could they be slipping on the motor shaft?

Any help would be great.

thanks
peter


Wed Feb 18, 2009 4:32 pm
Profile
Novice

Joined: Fri Oct 24, 2008 8:58 am
Posts: 87
Post Re: Trouble with PID control
For what its worth, I never got the built in PID control to work on a real robot either.
I've posted several times both here and even emailed Hitechnic directly.

I think the PID implementation is severely broken or at best crippled by not being tunable.
The PID parameters are apparently not accessible from RobotC user code.

I'm not sure how any "stock" parameters would work for every possible size/weight of robot.

Most of what is available seems to have been written for the NXT motors. I think the RobotC developers got the Hitechnic controllers too late to have everything ready for this season's code release.

I've totally abandoned the built in PID controller and gone to using my own fuzzy logic controllers.
I use them for arm control, heading control, distance traveled control. They are working very well.

I wrote some python scripts that automagically generate the RobotC code for a fuzzy logic controller from a simple Python based description of it (membership functions, logic inference rules, etc)

I'd be happy to post them, but its really more advanced stuff for people who know what they're getting into.


Wed Feb 18, 2009 5:07 pm
Profile
Rookie

Joined: Wed Feb 18, 2009 4:06 pm
Posts: 12
Post Re: Trouble with PID control
Wow...

I also noticed that one of the encoders counts down while the other counts up. I wonder if this is confusing it?


Thu Feb 19, 2009 9:17 am
Profile
Novice

Joined: Fri Oct 24, 2008 8:58 am
Posts: 87
Post Re: Trouble with PID control
okay, here's some more useful suggestions than just ranting about it not working right.

First, reverse the motor in the Motor Setup screen rather than by using - signs.

Second, double and triple check that the RED motor wire is connected to the + terminal and the BLACK motor wire is connected to the - terminal. The motors are labeled and the PID controller will really really care. Also make sure that the correct encoders are plugged into the correct spots for the motors they are on.

Third, watch the NXT devices debug screen and see what the command PWM value vs the actual PWM values are for the motor. This will tell you want the PID is actually doing vs what you told it to.

Fourth, (4th...thats a weird looking word isn't it).

Fifth, I had lots of problems going > 75% power when my batteries were even a little bit low. I was told some excuse about the PID controller needing headroom...for me it just went completely crazy so I'm not sure if that is headroom or a little bit of psychosis.


Good luck.


Thu Feb 19, 2009 10:44 am
Profile
Rookie

Joined: Thu Oct 09, 2008 7:59 pm
Posts: 3
Post Re: Trouble with PID control
chadgeorge wrote:
For what its worth, I never got the built in PID control to work on a real robot either.
I've posted several times both here and even emailed Hitechnic directly.

I think the PID implementation is severely broken or at best crippled by not being tunable.
The PID parameters are apparently not accessible from RobotC user code.

I'm not sure how any "stock" parameters would work for every possible size/weight of robot.

Most of what is available seems to have been written for the NXT motors. I think the RobotC developers got the Hitechnic controllers too late to have everything ready for this season's code release.

I've totally abandoned the built in PID controller and gone to using my own fuzzy logic controllers.
I use them for arm control, heading control, distance traveled control. They are working very well.

I wrote some python scripts that automagically generate the RobotC code for a fuzzy logic controller from a simple Python based description of it (membership functions, logic inference rules, etc)

I'd be happy to post them, but its really more advanced stuff for people who know what they're getting into.



I would LOVE to see these python scripts and/or the C code generated by them.
I have two motors working together to raise a bucket but they need to be sync'd up with little margin of error


Sat Feb 28, 2009 4:34 pm
Profile
Novice

Joined: Fri Oct 24, 2008 8:58 am
Posts: 87
Post Re: Trouble with PID control
I've tried many times to get the attach files to work and it never does. So its hard to upload the python scripts (since there are quite a few files...python itself doesn't have any limits so I guess I'm spoiled and use too many)

Here's the python script that "defines" a fuzzy controller. Notice the fuzzy rules are in the doc string for the controller class definition. This one uses a special table form for automatically generating a full set of 2 variable if-then rules. You can add if-then rules manually here as well.

Also be aware that this controller uses 10 functions and 2 files. This is 10% of available user functions (100) and 15% of available user files (14). Not that its really a problem since only a ridiculous programmer would waste the remaining 90%. Just don't fall into the trap that you have an unlimited number.

I should really re-factor my code generator to use fewer functions...since its auto-generated anyway it doesn't have to actually be legible. And you certainly don't ever want to use a function in RobotC unless you absolutely have to.

Sometimes programming with RobotC feels like a real-time strategy game where you're given a certain number of resources and you have to figure out where to use them best... what fun :)

Code:
from fuzzy import *

error_range = (-250, 250)  # 1/10 in error
class error(Fuzzify):
    nb      = ScaleMember( (0.0, 0.0, .10, .35), error_range )
    ns      = ScaleMember( (.25, .38, .48), error_range )
    ze      = ScaleMember( (.40, .50, .60), error_range )
    ps      = ScaleMember( (.52, .63, .75), error_range )
    pb      = ScaleMember( (.65, .90, 1.0, 1.0), error_range )

    _crisp = Int16(-1000, 1000)

chgerr_range = (-1000, 1000)   # change error / sec
class chgerr(Fuzzify):
    nb      = ScaleMember( (0.0, 0.0, .10, .35), chgerr_range )
    ns      = ScaleMember( (.25, .30, .40, .45), chgerr_range )
    ze      = ScaleMember( (.40, .45, .55, .60), chgerr_range )
    ps      = ScaleMember( (.55, .63, .70, .75), chgerr_range )
    pb      = ScaleMember( (.65, .90, 1.0, 1.0), chgerr_range )

    _crisp = Int16(100, 100)
   
output_range = (-100, 100)   # % Full Speed
class output(Defuzzify):
    nb      = ScaleMember( (0.0, 0.0, .10, .35), output_range )
    ns      = ScaleMember( (.25, .35, .50),      output_range )
    ze      = ScaleMember( (.45, .50, .55),      output_range )
    ps      = ScaleMember( (.50, .65, .75),      output_range )
    pb      = ScaleMember( (.65, .90, 1.0, 1.0), output_range )
   
    _crisp = Int16(-1000, 1000,null = 0, clip_output = False)
       
class motor_control(RuleBase):
    """
    TABLE : output
    -----------------------------------
    | error  | nb | ns | ze | ps | pb |
    | chgerr |    |    |    |    |    |
    -----------------------------------
    |   pb   | ze | ps | pb | pb | pb |
    -----------------------------------
    |   ps   | ns | ze | ps | pb | pb |
    -----------------------------------
    |   ze   | nb | ns | ze | ps | pb |
    -----------------------------------
    |   ns   | nb | nb | ns | ze | ps |
    -----------------------------------
    |   nb   | nb | nb | nb | ns | ze |
    -----------------------------------

    """

    # Input Variables
    error   = error()
    chgerr  = chgerr()
   
    # Output Variables
    output = output()
   
    # Type to use for variable storage
    _fuzzy = Int32(min=0, max=1000)
   
if __name__ == "__main__":
    r1 = motor_control()
    from fuzzygen import makeRobotC
   
    f = open("drv_pos_ctrl.c", "wt")
    result = makeRobotC(r1, prefix="drvpos");
    f.write(result)
    f.close()
    print result


And here's the resulting RobotC code.
Code:
#if !defined(FUZZY_H)
#include "fuzzy.c"
#endif


/**************************************************
 * Define Linguistic Variables
 **************************************************/

typedef struct {
    int16 _crisp;
    int32 nb;
    int32 ns;
    int32 pb;
    int32 ps;
    int32 ze;
    int32 _area;
    int32 _cx;
    int32 _s0_;
    int32 _s1_;
    } tDRVPOS_OUTPUT;

typedef struct {
    int16 _crisp;
    int32 nb;
    int32 ns;
    int32 pb;
    int32 ps;
    int32 ze;
    } tDRVPOS_ERROR;

typedef struct {
    int16 _crisp;
    int32 nb;
    int32 ns;
    int32 pb;
    int32 ps;
    int32 ze;
    } tDRVPOS_CHGERR;

typedef struct {
    tDRVPOS_OUTPUT output;
    tDRVPOS_ERROR error;
    tDRVPOS_CHGERR chgerr;
    int32 _s0_;
    } tDRVPOS_VARS;
tDRVPOS_VARS DRVPOS;


/**************************************************
 * Fuzzification Functions
 **************************************************/

/**************************************************
 * Fuzzify error
 **************************************************/

void fuzzifyDRVPOS_error()
{
    fzy_crisp = DRVPOS.error._crisp;
    fzy_fmax = 1000;
    calcLeftRamp(DRVPOS.error.nb, -200,-75);
    calcTriangle(DRVPOS.error.ns, -125,-60,-10);
    calcRightRamp(DRVPOS.error.pb, 75,200);
    calcTriangle(DRVPOS.error.ps, 10,65,125);
    calcTriangle(DRVPOS.error.ze, -50,0,50);
}

/**************************************************
 * Fuzzify chgerr
 **************************************************/

void fuzzifyDRVPOS_chgerr()
{
    fzy_crisp = DRVPOS.chgerr._crisp;
    fzy_fmax = 1000;
    calcLeftRamp(DRVPOS.chgerr.nb, -800,-300);
    calcTrapezoid(DRVPOS.chgerr.ns, -500,-400,-200,-100);
    calcRightRamp(DRVPOS.chgerr.pb, 300,800);
    calcTrapezoid(DRVPOS.chgerr.ps, 100,260,400,500);
    calcTrapezoid(DRVPOS.chgerr.ze, -200,-100,100,200);
}


/**************************************************
 * Fuzzy Inference Rules
 **************************************************/
void calcDRVPOS_FuzzyRules()
{
    /* IF (error IS nb) AND (chgerr IS pb) THEN output IS ze */
    calcSimpleAND(DRVPOS.error.nb, DRVPOS.chgerr.pb, DRVPOS.output.ze);

    /* IF (error IS ns) AND (chgerr IS pb) THEN output IS ps */
    calcSimpleAND(DRVPOS.error.ns, DRVPOS.chgerr.pb, DRVPOS.output.ps);

    /* IF (error IS ze) AND (chgerr IS pb) THEN output IS pb */
    calcSimpleAND(DRVPOS.error.ze, DRVPOS.chgerr.pb, DRVPOS.output.pb);

    /* IF (error IS ps) AND (chgerr IS pb) THEN output IS pb */
    calcSimpleAND(DRVPOS.error.ps, DRVPOS.chgerr.pb, DRVPOS.output.pb);

    /* IF (error IS pb) AND (chgerr IS pb) THEN output IS pb */
    calcSimpleAND(DRVPOS.error.pb, DRVPOS.chgerr.pb, DRVPOS.output.pb);

    /* IF (error IS nb) AND (chgerr IS ps) THEN output IS ns */
    calcSimpleAND(DRVPOS.error.nb, DRVPOS.chgerr.ps, DRVPOS.output.ns);

    /* IF (error IS ns) AND (chgerr IS ps) THEN output IS ze */
    calcSimpleAND(DRVPOS.error.ns, DRVPOS.chgerr.ps, DRVPOS.output.ze);

    /* IF (error IS ze) AND (chgerr IS ps) THEN output IS ps */
    calcSimpleAND(DRVPOS.error.ze, DRVPOS.chgerr.ps, DRVPOS.output.ps);

    /* IF (error IS ps) AND (chgerr IS ps) THEN output IS pb */
    calcSimpleAND(DRVPOS.error.ps, DRVPOS.chgerr.ps, DRVPOS.output.pb);

    /* IF (error IS pb) AND (chgerr IS ps) THEN output IS pb */
    calcSimpleAND(DRVPOS.error.pb, DRVPOS.chgerr.ps, DRVPOS.output.pb);

    /* IF (error IS nb) AND (chgerr IS ze) THEN output IS nb */
    calcSimpleAND(DRVPOS.error.nb, DRVPOS.chgerr.ze, DRVPOS.output.nb);

    /* IF (error IS ns) AND (chgerr IS ze) THEN output IS ns */
    calcSimpleAND(DRVPOS.error.ns, DRVPOS.chgerr.ze, DRVPOS.output.ns);

    /* IF (error IS ze) AND (chgerr IS ze) THEN output IS ze */
    calcSimpleAND(DRVPOS.error.ze, DRVPOS.chgerr.ze, DRVPOS.output.ze);

    /* IF (error IS ps) AND (chgerr IS ze) THEN output IS ps */
    calcSimpleAND(DRVPOS.error.ps, DRVPOS.chgerr.ze, DRVPOS.output.ps);

    /* IF (error IS pb) AND (chgerr IS ze) THEN output IS pb */
    calcSimpleAND(DRVPOS.error.pb, DRVPOS.chgerr.ze, DRVPOS.output.pb);

    /* IF (error IS nb) AND (chgerr IS ns) THEN output IS nb */
    calcSimpleAND(DRVPOS.error.nb, DRVPOS.chgerr.ns, DRVPOS.output.nb);

    /* IF (error IS ns) AND (chgerr IS ns) THEN output IS nb */
    calcSimpleAND(DRVPOS.error.ns, DRVPOS.chgerr.ns, DRVPOS.output.nb);

    /* IF (error IS ze) AND (chgerr IS ns) THEN output IS ns */
    calcSimpleAND(DRVPOS.error.ze, DRVPOS.chgerr.ns, DRVPOS.output.ns);

    /* IF (error IS ps) AND (chgerr IS ns) THEN output IS ze */
    calcSimpleAND(DRVPOS.error.ps, DRVPOS.chgerr.ns, DRVPOS.output.ze);

    /* IF (error IS pb) AND (chgerr IS ns) THEN output IS ps */
    calcSimpleAND(DRVPOS.error.pb, DRVPOS.chgerr.ns, DRVPOS.output.ps);

    /* IF (error IS nb) AND (chgerr IS nb) THEN output IS nb */
    calcSimpleAND(DRVPOS.error.nb, DRVPOS.chgerr.nb, DRVPOS.output.nb);

    /* IF (error IS ns) AND (chgerr IS nb) THEN output IS nb */
    calcSimpleAND(DRVPOS.error.ns, DRVPOS.chgerr.nb, DRVPOS.output.nb);

    /* IF (error IS ze) AND (chgerr IS nb) THEN output IS nb */
    calcSimpleAND(DRVPOS.error.ze, DRVPOS.chgerr.nb, DRVPOS.output.nb);

    /* IF (error IS ps) AND (chgerr IS nb) THEN output IS ns */
    calcSimpleAND(DRVPOS.error.ps, DRVPOS.chgerr.nb, DRVPOS.output.ns);

    /* IF (error IS pb) AND (chgerr IS nb) THEN output IS ze */
    calcSimpleAND(DRVPOS.error.pb, DRVPOS.chgerr.nb, DRVPOS.output.ze);
}


/**************************************************
 * Defuzzification Functions
 **************************************************/

void defuzzifyDRVPOS_output()
{
    DRVPOS.output._cx = 0;
    DRVPOS.output._area = 0;

    /* nb */
    /* Left_Ramp(-100,-100,-80,-30) */
    DRVPOS.output._s0_ = 45;
    DRVPOS.output._s0_ *= DRVPOS.output.nb;
    DRVPOS.output._s1_ = -100;
    DRVPOS.output._s1_ *= DRVPOS.output._s0_;
    DRVPOS.output._cx += DRVPOS.output._s1_;
    DRVPOS.output._area += DRVPOS.output._s0_;

    /* ns */
    /* Triangle(-50,-30,0) */
    DRVPOS.output._s0_ = 25;
    DRVPOS.output._s0_ *= DRVPOS.output.ns;
    DRVPOS.output._s1_ = -80;
    DRVPOS.output._s1_ *= DRVPOS.output._s0_;
    DRVPOS.output._s1_ /= 3;

    DRVPOS.output._cx += DRVPOS.output._s1_;
    DRVPOS.output._area += DRVPOS.output._s0_;

    /* pb */
    /* Right_Ramp(30,80,100,100) */
    DRVPOS.output._s0_ = 45;
    DRVPOS.output._s0_ *= DRVPOS.output.pb;
    DRVPOS.output._s1_ = 100;
    DRVPOS.output._s1_ *= DRVPOS.output._s0_;
    DRVPOS.output._cx += DRVPOS.output._s1_;
    DRVPOS.output._area += DRVPOS.output._s0_;

    /* ps */
    /* Triangle(0,30,50) */
    DRVPOS.output._s0_ = 25;
    DRVPOS.output._s0_ *= DRVPOS.output.ps;
    DRVPOS.output._s1_ = 80;
    DRVPOS.output._s1_ *= DRVPOS.output._s0_;
    DRVPOS.output._s1_ /= 3;

    DRVPOS.output._cx += DRVPOS.output._s1_;
    DRVPOS.output._area += DRVPOS.output._s0_;

    /* ze */
    /* Triangle(-10,0,10) */
    DRVPOS.output._s0_ = 10;
    DRVPOS.output._s0_ *= DRVPOS.output.ze;
    DRVPOS.output._area += DRVPOS.output._s0_;


    if (DRVPOS.output._area == 0)
       DRVPOS.output._crisp = 0;
    else
    {
        DRVPOS._s0_ = DRVPOS.output._cx;
        DRVPOS._s0_ /= DRVPOS.output._area;
        DRVPOS.output._crisp = DRVPOS._s0_;
    }
}


/**************************************************
 * Fuzzy Logic Controller
 **************************************************/
void evalDRVPOS()
{
    /* Reset Fuzzy Outputs */
    DRVPOS.output.nb = 0;
    DRVPOS.output.ns = 0;
    DRVPOS.output.pb = 0;
    DRVPOS.output.ps = 0;
    DRVPOS.output.ze = 0;

    /* Fuzzify Inputs */
    fuzzifyDRVPOS_error();
    fuzzifyDRVPOS_chgerr();

    /* Perform Fuzzy Inference */
    calcDRVPOS_FuzzyRules();

    /* Defuzzify Outputs */
    defuzzifyDRVPOS_output();
}


and here's the RobotC helper file.
Code:
#define FUZZY_H

#if !defined(int16)
#define int16 int
#endif

#if !defined(int32)
#define int32 long
#endif


int32 fzy_fuzzy;
int32 fzy_crisp;
int32 fzy_fmax;

// Fuzzification - Standard Membership Functions
void calcTriangle(int32 &outvar, int32 pt0, int32 pt1, int32 pt2)
{
    if (fzy_crisp <= pt0)
        fzy_fuzzy = 0;
    else if (fzy_crisp <= pt1)
    {
        fzy_fuzzy = fzy_crisp;
        fzy_fuzzy -= pt0;
        fzy_fuzzy *= fzy_fmax;
        fzy_fuzzy /= (pt1 - pt0);
    }
    else if (fzy_crisp <= pt2)
    {
        fzy_fuzzy = fzy_crisp;
        fzy_fuzzy -= pt1;
        fzy_fuzzy *= fzy_fmax;
        fzy_fuzzy /= (pt1 - pt2);
        fzy_fuzzy += fzy_fmax;
    }
    else
        fzy_fuzzy = 0;

    outvar = fzy_fuzzy;
}

void calcLeftRamp(int32 &outvar, int32 pt0, int32 pt1)
{
    if (fzy_crisp <= pt0)
        fzy_fuzzy = fzy_fmax;

    else if (fzy_crisp <= pt1)
    {
        fzy_fuzzy = fzy_crisp;
        fzy_fuzzy -= pt0;
        fzy_fuzzy *= fzy_fmax;
        fzy_fuzzy /= (pt0 - pt1);
        fzy_fuzzy += fzy_fmax;
    }
    else
        fzy_fuzzy = 0;

    outvar = fzy_fuzzy;
}

void calcRightRamp(int32 &outvar, int32 pt0, int32 pt1)
{
    if (fzy_crisp <= pt0)
        fzy_fuzzy = 0;

    else if (fzy_crisp <= pt1)
    {
        fzy_fuzzy = fzy_crisp;
        fzy_fuzzy -= pt0;
        fzy_fuzzy *= fzy_fmax;
        fzy_fuzzy /= (pt1 - pt0);

    }
    else
        fzy_fuzzy = fzy_fmax;

    outvar = fzy_fuzzy;
}

void calcTrapezoid(int32 &outvar, int32 pt0, int32 pt1, int32 pt2, int32 pt3)
{
    if (fzy_crisp <= pt0)
        fzy_fuzzy = 0;
    else if (fzy_crisp <= pt1)
    {
        fzy_fuzzy = fzy_crisp;
        fzy_fuzzy -= pt0;
        fzy_fuzzy *= fzy_fmax;
        fzy_fuzzy /= (pt1 - pt0);

    }
    else if (fzy_crisp <= pt2)
    {
        fzy_fuzzy = fzy_fmax;
    }
    else if (fzy_crisp <= pt3)
    {
        fzy_fuzzy = fzy_crisp;
        fzy_fuzzy -= pt2;
        fzy_fuzzy *= fzy_fmax;
        fzy_fuzzy /= (pt2 - pt3);

        fzy_fuzzy += fzy_fmax;
    }
    else
        fzy_fuzzy = 0;

    outvar = fzy_fuzzy;
}

// Common Fuzzy Inference Rules
void calcSimpleAND(int32 varA, int32 varB, int32 &outvar)
{
   if (varA < varB)
   {
      if (varA > outvar)
         outvar = varA;
   }
   else
   {
      if (varB > outvar)
         outvar = varB;
   }
}


Mon Mar 02, 2009 3:28 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 6 posts ] 

Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.