View unanswered posts | View active topics It is currently Mon Jul 23, 2018 11:27 am






Reply to topic  [ 80 posts ]  Go to page Previous  1, 2, 3, 4, 5, 6  Next
PID Control 
Author Message
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
Yes, just drive your elevator all the way down, we use a touch sensor to detect when it reaches the bottom. This is call zero calibration. Once it reaches the bottom reset the encoder and in TeleOp, you can use the joystick to drive the elevator up to the desired height while reading and printing the value of the encoder. Then you know what the encoder value of a certain height is.
BTB1337 is also right that our library does have support for elevator. It's the PIDMotor module. We can call PIDMotorSetTarget to set the height and it will use PID control to get there.


Sun Feb 08, 2015 2:51 am
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Hey this compass code doesn't seem to work, it takes like 10, 360 rotations to hit 90....the debug screen on the nxt shows that it moved on to state 2, but just keeps turning and it shows T 0, E 0, I -1, -2, -3 (increasing) for the top set of values and the bottom is T 90, E (can't remember but it might have been 0), I 0, it was really strange...would it be fixed if I used the gyro? we just ordered one to be here sometime this week...I don't really want to use just encoders if i don't have to because it seemed inaccurate and veered off to the right a little which concerned me, but that seemed to be fixed with the compass...

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     irSeeker,       sensorI2CCustom)
#pragma config(Sensor, S3,     compassSensor,  sensorI2CHiTechnicCompass)
#pragma config(Motor,  mtr_S1_C1_1,     leftWheel,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightWheel,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     leftlifter,    tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     rightlifter,   tmotorTetrix, PIDControl)
#pragma config(Servo,  srvo_S1_C4_1,    rightpan,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    leftpan,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    irServo,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// include files
#include "drivers/hitechnic-irseeker-v2.h"
#include "JoystickDriver.c"
#include "ftclib/trcdefs.h"
#include "ftclib/dbgtrace.h"
#include "ftclib/sm.h"
#include "ftclib/compass.h"
#include "drivers/hitechnic-compass.h"
#include "ftclib/drive.h"
#include "ftclib/pidctrl.h"
#include "ftclib/piddrive.h"

// define distance with encoders
#define ENC_KP              1.8 // change this to meet target distance
#define ENC_KI              0.0
#define ENC_KD              0.0
#define ENC_TOLERANCE       0.5
#define ENC_SETTLING        100

// define turns with compass
#define COMP_KP             0.5 // change this to meet turn angle
#define COMP_KI             0.0
#define COMP_KD             0.0
#define COMP_TOLERANCE      0.5
#define COMP_SETTLING       100

#define CLICKS_PER_INCH     118.5 // change this to tune distance

#define EVTTYPE_PIDDRIVE    (EVTTYPE_NONE + 1)

COMPASS       g_compass;      //create the compass object.
DRIVE       g_drive;        //create the drive object.
PIDCTRL     g_yPidCtrl;     //create the Y PID controller
PIDCTRL     g_turnPidCtrl;  //create the turn PID controller
PIDDRIVE    g_pidDrive;     //create the PID drive object
SM          g_autoSM;       //create the autonomous state machine object

float PIDCtrlGetInput(PIDCTRL &pidCtrl)
{
    float inputValue = 0.0;

    if (IsSameReference(pidCtrl, g_yPidCtrl))
    {
        inputValue = (nMotorEncoder[leftWheel] + nMotorEncoder[rightWheel])/2.0/CLICKS_PER_INCH;
    }
    else if (IsSameReference(pidCtrl, g_turnPidCtrl))
    {
        inputValue = CompassGetHeading(g_compass);
    }
    return inputValue;
}

task main()
{
    CompassInit(g_compass, compassSensor);   //initialize compass object.
    DriveInit(g_drive, leftWheel, rightWheel);  //initialize drive object
    PIDCtrlInit(g_yPidCtrl,         //initialize yPidCtrl object
                ENC_KP, ENC_KI, ENC_KD,
                ENC_TOLERANCE, ENC_SETTLING, PIDCTRLO_ABS_SETPT);
    PIDCtrlInit(g_turnPidCtrl,
                COMP_KP, COMP_KI, COMP_KD,
                COMP_TOLERANCE, COMP_SETTLING);
    PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl);
    // Initialize the autonomous state machine and start it.
      SMInit(g_autoSM);
      SMStart(g_autoSM);

    while (true)
    {
          displayTextLine(0, "state=%d", SMGetState(g_autoSM));
        PIDCtrlDisplayInfo(1, g_yPidCtrl);
        PIDCtrlDisplayInfo(3, g_turnPidCtrl);
        // Check if state machine is ready.
        if (SMIsReady(g_autoSM))
        {
            int currState = SMGetState(g_autoSM);
            switch (currState)
            {
                case SMSTATE_STARTED:
                    // Drive forward for 24 inches.
                    PIDDriveSetTarget(g_pidDrive,
                                      24.0,
                                      CompassGetHeading(g_compass) + 0.0,
                                      false,
                                      &g_autoSM,
                                      EVTTYPE_PIDDRIVE);
                    // Tell state machine to wait until done.
                    SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
                    // Tell state machine to go to next state when done.
                    SMWaitEvents(g_autoSM, currState + 1);
                    break;

                case SMSTATE_STARTED + 1:
                    // Turn right 90 degrees.
                    PIDDriveSetTarget(g_pidDrive,
                                      0.0,
                                      CompassGetHeading(g_compass) + 90.0,
                                      false,
                                      &g_autoSM,
                                      EVTTYPE_PIDDRIVE);
                    // Tell state machine to wait until done.
                    SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
                    // Tell state machine to go to next state when done.
                    SMWaitEvents(g_autoSM, currState + 1);
                    break;

                default:
                    // We are done, stop the state machine.
                    SMStop(g_autoSM);
                    break;
            }
        }
        //
        // Execute all periodic tasks.
        //
        PIDDriveTask(g_pidDrive);
        DriveTask(g_drive);
        wait1Msec(20);
    }
}


Also it is no long moving on to the next state...this is using only encoders to go forward and turn
It shows the following:

    state=1
    T= 24.0 E= 0.2
    I= 23.8 O= 3.7
    T= 0.0 E= 4.0
    I= -4.0 O= 2.0

Code:
// define distance with encoders
#define ENC_KP              1.75 // change this to meet distance target
#define ENC_KI              0.0018
#define ENC_KD              0.6
#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200


Tue Feb 10, 2015 9:45 am
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Another problem came up, no matter what i put at kp ki kd and clicks per inch, for whatever reason it just keeps driving forward and from the brief time i have to look at the nxt it doesn't seem to be counting up for whatever reason...it's the strangest thing. I checked that all the encoders and stuff are connected and they are. I'm very confused by this and could really use some help.

EDIT: It seems to be working now, all I did was change out the battery...? could that happen because the batter is so low that it just wants to drive forward and never stop?


Wed Feb 11, 2015 10:12 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
No, battery should not have anything to do with it. However, it is a well known fact that ESD (Electro-Static Discharge) will cause the Tetrix motor controller or the NXT brick to hang. When that happens, you lose control of the motors (i.e. it will not stop). When that happens, you need to either reboot the NXT (if NXT was hung) by removing and re-installing the NXT battery. Or if the Tetrix motor controller was hung, then power the 12V main power OFF and ON.


Wed Feb 11, 2015 3:30 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Oh OK makes sense! Thanks

Any ideas on why it won't move to the nest state?


Wed Feb 11, 2015 3:39 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
Oh OK makes sense! Thanks

Any ideas on why it won't move to the nest state?

Alright, I looked at your code carefully. The first set of PID info looks normal. You set your target to 24.0 inches and it went 23.8 so the error was 0.2 inch which was within the 0.5 inch tolerance. However, the second set of PID info is for the compass. It said you set the target to 0.0 and the compass returned -4.0 and the error would be 4.0. I scanned your code and did not see you calling CompassTask. You need to call CompassTask at the beginning of the forever-while loop. That's how the compass heading is updated. If you are not calling it, it means the compass value won't change. When the compass is not changing, PID controlled turn got stuck and it won't move on to the next state until it satisfy the turn request of 0.0. Without the compass moving, you have two outcomes, either the robot will turn round and round going crazy trying to turn the robot to the turn setpoint or it doesn't move because the turn error is small so the power is not large enough to turn the robot but since you have not reach within the turn tolerance, PID got stuck trying forever to turn the robot to your target. So I'd bet it will solve your issue if you add the CompassTask call as the following:
Code:
    while (true)
    {
       displayTextLine(0, "state=%d", SMGetState(g_autoSM));
        PIDCtrlDisplayInfo(1, g_yPidCtrl);
        PIDCtrlDisplayInfo(3, g_turnPidCtrl);
        CompassTask(g_compass);
        // Check if state machine is ready.
        if (SMIsReady(g_autoSM))
        {


Wed Feb 11, 2015 4:06 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
MHTS wrote:
BTB1337 wrote:
Oh OK makes sense! Thanks

Any ideas on why it won't move to the nest state?

Alright, I looked at your code carefully. The first set of PID info looks normal. You set your target to 24.0 inches and it went 23.8 so the error was 0.2 inch which was within the 0.5 inch tolerance. However, the second set of PID info is for the compass. It said you set the target to 0.0 and the compass returned -4.0 and the error would be 4.0. I scanned your code and did not see you calling CompassTask. You need to call CompassTask at the beginning of the forever-while loop. That's how the compass heading is updated. If you are not calling it, it means the compass value won't change. When the compass is not changing, PID controlled turn got stuck and it won't move on to the next state until it satisfy the turn request of 0.0. Without the compass moving, you have two outcomes, either the robot will turn round and round going crazy trying to turn the robot to the turn setpoint or it doesn't move because the turn error is small so the power is not large enough to turn the robot but since you have not reach within the turn tolerance, PID got stuck trying forever to turn the robot to your target. So I'd bet it will solve your issue if you add the CompassTask call as the following:
Code:
    while (true)
    {
       displayTextLine(0, "state=%d", SMGetState(g_autoSM));
        PIDCtrlDisplayInfo(1, g_yPidCtrl);
        PIDCtrlDisplayInfo(3, g_turnPidCtrl);
        CompassTask(g_compass);
        // Check if state machine is ready.
        if (SMIsReady(g_autoSM))
        {


ok so i added that to my compass program but haven't been able to test it, but for the encoder only program is getting stuck and not moving on.
This is what the nxt displays:
    state=1
    T= 24.0 E= 0.2
    I= 23.8 O= 3.7
    T= 0.0 E= 4.0
    I= -4.0 O= 2.0

Here is the full program for encoder's only

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     irSeeker,       sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     leftWheel,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightWheel,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     leftlifter,    tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     rightlifter,   tmotorTetrix, PIDControl)
#pragma config(Servo,  srvo_S1_C4_1,    rightpan,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    leftpan,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    irServo,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// include files
#include "ftclib/trcdefs.h"
#include "ftclib/dbgtrace.h"
#include "ftclib/sm.h"
#include "ftclib/drive.h"
#include "ftclib/pidctrl.h"
#include "ftclib/piddrive.h"

// define distance with encoders
#define ENC_KP              1.6 // change this to meet distance target
#define ENC_KI              0.0015
#define ENC_KD              0.5
#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200

// define turns with encoder
#define TURN_KP             0.5 // change this to meet turn target
#define TURN_KI             0.0
#define TURN_KD             0.0
#define TURN_TOLERANCE      0.5
#define TURN_SETTLING       200

#define CLICKS_PER_INCH     118.5 // change this to adjust actual distance
#define CLICKS_PER_DEGREE   10.0 // change this to adjust actual degree

#define EVTTYPE_PIDDRIVE    (EVTTYPE_NONE + 1)

DRIVE       g_drive;        //create the drive object.
PIDCTRL     g_yPidCtrl;     //create the Y PID controller
PIDCTRL     g_turnPidCtrl;  //create the turn PID controller
PIDDRIVE    g_pidDrive;     //create the PID drive object
SM          g_autoSM;       //create the autonomous state machine object

float PIDCtrlGetInput(PIDCTRL &pidCtrl)
{
   float inputValue = 0.0;

   if (IsSameReference(pidCtrl, g_yPidCtrl))
   {
      inputValue = (nMotorEncoder[leftWheel] + nMotorEncoder[rightWheel])/2.0/CLICKS_PER_INCH;
   }
   else if (IsSameReference(pidCtrl, g_turnPidCtrl))
   {
      inputValue = (nMotorEncoder[leftWheel] - nMotorEncoder[rightWheel])/2.0/CLICKS_PER_DEGREE;
   }
   return inputValue;
}

task main()
{
   // Initialize the drive object.
   DriveInit(g_drive, leftWheel, rightWheel);
   // Initialize the yPidCtrl object.
   PIDCtrlInit(g_yPidCtrl,
   ENC_KP, ENC_KI, ENC_KD,
   ENC_TOLERANCE, ENC_SETTLING);
   // Initialize the turnPidCtrl object.
   PIDCtrlInit(g_turnPidCtrl,
   TURN_KP, TURN_KI, TURN_KD,
   TURN_TOLERANCE, TURN_SETTLING);
   // Initialize the pidDrive object.
   PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl);
   // Initialize the autonomous state machine and start it.
   SMInit(g_autoSM);
   SMStart(g_autoSM);

   while (true)
   {
      displayTextLine(0, "state=%d", SMGetState(g_autoSM));
      PIDCtrlDisplayInfo(1, g_yPidCtrl);
      PIDCtrlDisplayInfo(3, g_turnPidCtrl);
      // Check if state machine is ready.
      if (SMIsReady(g_autoSM))
      {
         int currState = SMGetState(g_autoSM);
         switch (currState)
         {
         case SMSTATE_STARTED:
            // Drive forward for 24 inches.
            PIDDriveSetTarget(g_pidDrive,
            24.0,
            0.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE);
            // Tell state machine to wait until done.
            SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
            // Tell state machine to go to next state when done.
            SMWaitEvents(g_autoSM, currState + 1);
            break;

         case SMSTATE_STARTED + 2:
            // Drive forward for 24 inches.
            PIDDriveSetTarget(g_pidDrive,
            0.0,
            90.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE);
            // Tell state machine to wait until done.
            SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
            // Tell state machine to go to next state when done.
            SMWaitEvents(g_autoSM, currState + 1);
            break;

         default:
            // We are done, stop the state machine.
            SMStop(g_autoSM);
            break;
         }
      }
      //
      // Execute all periodic tasks.
      //
      PIDDriveTask(g_pidDrive);
      DriveTask(g_drive);
      wait1Msec(20);
   }
}


Wed Feb 11, 2015 4:42 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
ok so i added that to my compass program but haven't been able to test it, but for the encoder only program is getting stuck and not moving on.
This is what the nxt displays:
    state=1
    T= 24.0 E= 0.2
    I= 23.8 O= 3.7
    T= 0.0 E= 4.0
    I= -4.0 O= 2.0

According to the PID info, your turn error was 4 and your TURN_KP was 0.5. So it means PID control was trying to correct the turn by applying a power difference between the left and the right wheel of 4*0.5 = 2.0. And since you have reached target on the 24.0 inches, the forward power was pretty much zero. So it means the robot was trying to correct the heading error of 4.0 by applying only 2% power and your turn tolerance was 0.5 degrees. So PID was trying forever to reach within 0.5 degree but couldn't quite do it. That's why it got stuck at state 1. You need to tune your TURN_KP, it needs to be about 10 times bigger (e.g. 5.0). One thing you need to understand about PID controlled drive is that if you set a target and a tolerance and if PID control can't reach the target, it will try forever. The usual fix is to tune Kp correctly so it will have enough power to reach target. However, there are times when you tune Kp correctly on a fresh battery, it may still not work later because the battery has run down and now PID doesn't have enough power to complete the requested target. There are two ways to fix this issue: tune a non-zero Ki term so if it gets stuck, the robot will eventually move if it gets stuck for a certain amount of time such that the time integration of the error will eventually raise the power level of the motors. However, introducing Ki means that it may oscillate especially if your tolerance is only 0.5 degree. When the power built up to move the robot again, it may overshoot and start oscillating. In that case, you may need to introduce a non-zero Kd term and tune it to reduce oscillation. Tuning PID constants take a lot of time. I generally do not prefer that. Instead, I will relax the tolerance to something reasonable (1.0 degree or even 2.0 degrees), tune Kp slightly larger (with a little bit overshooting but not too much), and finally add a timeout on the PIDDriveSetTarget call to give it a maximum time allowed for the operation. So in case, the robot get stuck for a small error but yet outside of tolerance, it will move to the next state after attempting a period of time unsuccessfully. But be careful to set the timeout value. It must be long enough to allow the robot to complete the target request in the worst case scenario (i.e. weak battery) and not abort prematurely but short enough that you don't waste valuable autonomous time. And make sure you change to a fresh battery during competition so PID control will be more consistent.


Wed Feb 11, 2015 6:12 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTW, I just noticed you have a bug in your state machine code:
Code:
         switch (currState)
         {
         case SMSTATE_STARTED:
            // Drive forward for 24 inches.
            PIDDriveSetTarget(g_pidDrive,
            24.0,
            0.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE);
            // Tell state machine to wait until done.
            SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
            // Tell state machine to go to next state when done.
            SMWaitEvents(g_autoSM, currState + 1);
            break;

         case SMSTATE_STARTED + 2:
            // Drive forward for 24 inches.

The second case should be "SMSTATE_STARTED + 1" not "+ 2" because in the previous state, the statement "SMWaitEvent(g_autoSM, currState + 1);" tells the state machine to go to SMSTATE_STARTED + 1 when done not "+2".


Wed Feb 11, 2015 6:40 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
MHTS wrote:
BTW, I just noticed you have a bug in your state machine code:
Code:
         switch (currState)
         {
         case SMSTATE_STARTED:
            // Drive forward for 24 inches.
            PIDDriveSetTarget(g_pidDrive,
            24.0,
            0.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE);
            // Tell state machine to wait until done.
            SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
            // Tell state machine to go to next state when done.
            SMWaitEvents(g_autoSM, currState + 1);
            break;

         case SMSTATE_STARTED + 2:
            // Drive forward for 24 inches.

The second case should be "SMSTATE_STARTED + 1" not "+ 2" because in the previous state, the statement "SMWaitEvent(g_autoSM, currState + 1);" tells the state machine to go to SMSTATE_STARTED + 1 when done not "+2".


ok thanks for the help, i understand PID a lot more now and how to tune it. How do you add the timer you referenced? I'm not familiar with how to add that to PIDDriveSetTarget. Also thanks for pointing out that bug haha! I think i was messing around with that number to try and see if I had confused myself and put the wrong number and forgot to put the correct one there before posting.

Another Issue i noticed earlier, while using the gyro in RVW it seemed to turn left then turn right then turn left and so on like it's swinging, would that be because kp was too large or too small?


Tue Feb 17, 2015 1:18 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
ok thanks for the help, i understand PID a lot more now and how to tune it. How do you add the timer you referenced? I'm not familiar with how to add that to PIDDriveSetTarget. Also thanks for pointing out that bug haha! I think i was messing around with that number to try and see if I had confused myself and put the wrong number and forgot to put the correct one there before posting.

Another Issue i noticed earlier, while using the gyro in RVW it seemed to turn left then turn right then turn left and so on like it's swinging, would that be because kp was too large or too small?

Below shows the function prototype of the function PIDDriveSetTarget. Notice the last parameter is "timeout" in msec. So for example, if you give it a value 3000, it means it will give it up to 3 seconds to complete the operation. If for some reason it doesn't complete, the event will be signaled after 3 seconds has elapsed and the state machine will move on.
Code:
void
PIDDriveSetTarget(
    PIDDRIVE &pidDrive,
    float distSetPoint,
    float angleSetPoint,
    bool fHoldTarget = false,
    SM *sm = NULL,
    int evtType = 0,
    unsigned long timeout = 0
    );

Regarding the swinging, what you described seems to be oscillation. That means Kp was too big and caused the target to overshoot so the robot swing the other way to compensate and the robot went too far the other way and the cycle repeats. To fix this, bring down Kp until it stop oscillating.


Wed Feb 18, 2015 5:37 am
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
MHTS wrote:
BTB1337 wrote:
ok thanks for the help, i understand PID a lot more now and how to tune it. How do you add the timer you referenced? I'm not familiar with how to add that to PIDDriveSetTarget. Also thanks for pointing out that bug haha! I think i was messing around with that number to try and see if I had confused myself and put the wrong number and forgot to put the correct one there before posting.

Another Issue i noticed earlier, while using the gyro in RVW it seemed to turn left then turn right then turn left and so on like it's swinging, would that be because kp was too large or too small?

Below shows the function prototype of the function PIDDriveSetTarget. Notice the last parameter is "timeout" in msec. So for example, if you give it a value 3000, it means it will give it up to 3 seconds to complete the operation. If for some reason it doesn't complete, the event will be signaled after 3 seconds has elapsed and the state machine will move on.
Code:
void
PIDDriveSetTarget(
    PIDDRIVE &pidDrive,
    float distSetPoint,
    float angleSetPoint,
    bool fHoldTarget = false,
    SM *sm = NULL,
    int evtType = 0,
    unsigned long timeout = 0
    );

Regarding the swinging, what you described seems to be oscillation. That means Kp was too big and caused the target to overshoot so the robot swing the other way to compensate and the robot went too far the other way and the cycle repeats. To fix this, bring down Kp until it stop oscillating.


Ok so the timeout is in milsecs correct? So a 3 second time out would be
Code:
PIDDriveSetTarget(g_pidDrive,
            24.0,
            0.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE,
            3000);


Another question regarding the PIDDriveSetTarget function, I was wondering what exactly does the bool fHoldTarget do?


Wed Feb 18, 2015 4:47 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
Ok so the timeout is in milsecs correct? So a 3 second time out would be
Code:
PIDDriveSetTarget(g_pidDrive,
            24.0,
            0.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE,
            3000);


Another question regarding the PIDDriveSetTarget function, I was wondering what exactly does the bool fHoldTarget do?

That's correct. 3 seconds has a value 3000. fHoldTarget is to tell PIDDrive to maintain the target instead of ending the operation. So if you set it to true, PIDDrive will go forward 24.0 inches and maintain the position, meaning that if another robot is pushing you back, it will try its hardest to push back to maintain the 24.0 inch position. It is seldom used in PIDDrive scenario where once you reach target, you normally just end the operation and turn the motors off. It is a lot more useful for a motor operating an arm, for example. Say if you have an arm that swings up to a certain angle, you don't want to turn the motor off when it has reached target because it will drop back down by gravity. In that case you want to "hold the target". In any case, the library is generic so it allows you that option if you really want to. But if you choose to set it to true, the operation will never complete (i.e. the event will never signal). So if you have a state machine like the one you have in your code waiting for the operation to complete, it will never move to the next state.


Wed Feb 18, 2015 7:38 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
ok great!

I've been messing around with the compass program and it seems to be setting the target at some rediculously high number and then it just keeps turning and turning until the I is at that number and it starts at 0 and goes up...i'm super confused :( Sorry if this makes little sense, i hardly understand what it's doing honestly, it's just freaking out going in circles

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     irSeeker,       sensorI2CCustom)
#pragma config(Sensor, S3,     htComp,         sensorI2CHiTechnicCompass)
#pragma config(Sensor, S4,     htEOPD,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     rightWheel,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     leftWheel,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     rightLifter,   tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     leftLifter,    tmotorTetrix, PIDControl)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    irServo,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    goalHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    leftDoor,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    rightDoor,            tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// include files
#include "ftclib/trcdefs.h"
#include "ftclib/dbgtrace.h"
#include "ftclib/sm.h"
#include "ftclib/drive.h"
#include "ftclib/pidctrl.h"
#include "ftclib/piddrive.h"
#include "ftclib/compass.h"

// define distance with encoders
#define ENC_KP              1.6 // change this to meet distance target
#define ENC_KI              0.0015
#define ENC_KD              0.5
#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200

// define turns with encoder
#define COMP_KP             1.0 // change this to meet turn target
#define COMP_KI             0.0
#define COMP_KD             0.0
#define COMP_TOLERANCE      1.0
#define COMP_SETTLING       200

#define CLICKS_PER_INCH     118.5 // change this to adjust actual distance

#define EVTTYPE_PIDDRIVE    (EVTTYPE_NONE + 1)

COMPASS         g_compass;
DRIVE       g_drive;        //create the drive object.
PIDCTRL     g_yPidCtrl;     //create the Y PID controller
PIDCTRL     g_turnPidCtrl;  //create the turn PID controller
PIDDRIVE    g_pidDrive;     //create the PID drive object
SM          g_autoSM;       //create the autonomous state machine object

// calculate all the math
float PIDCtrlGetInput(PIDCTRL &pidCtrl)
{
   float inputValue = 0.0;

   if (IsSameReference(pidCtrl, g_yPidCtrl))
   {
      inputValue = (nMotorEncoder[leftWheel] + nMotorEncoder[rightWheel])/2.0/CLICKS_PER_INCH;
   }
   else if (IsSameReference(pidCtrl, g_turnPidCtrl))
   {
      inputValue = CompassGetHeading(g_compass);
   }
   return inputValue;
}

task main()
{
   CompassInit(g_compass, htComp);
   // Initialize the drive object.
   DriveInit(g_drive, leftWheel, rightWheel);
   // Initialize the yPidCtrl object.
   PIDCtrlInit(g_yPidCtrl,
                     ENC_KP, ENC_KI, ENC_KD,
                     ENC_TOLERANCE, ENC_SETTLING);
   // Initialize the turnPidCtrl object.
   PIDCtrlInit(g_turnPidCtrl,
                     COMP_KP, COMP_KI, COMP_KD,
                     COMP_TOLERANCE, COMP_SETTLING,
                     PIDCTRLO_ABS_SETPT);
   // Initialize the pidDrive object.
   PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl);
   // Initialize the autonomous state machine and start it.
   SMInit(g_autoSM);
   SMStart(g_autoSM);

   while (true)
   {
      // display debug info on nxt
      displayTextLine(0, "state=%d", SMGetState(g_autoSM));
      PIDCtrlDisplayInfo(1, g_yPidCtrl);
      PIDCtrlDisplayInfo(3, g_turnPidCtrl);
      CompassTask(g_compass);
      // Check if state machine is ready.
      if (SMIsReady(g_autoSM))
      {
         int currState = SMGetState(g_autoSM);
         switch (currState)
         {
         case SMSTATE_STARTED:
            // Drive forward for 24 inches.
            PIDDriveSetTarget(g_pidDrive,
            0.0,
            CompassGetHeading(g_compass) + 90.0,
            false,
            &g_autoSM,
            EVTTYPE_PIDDRIVE);
            // Tell state machine to wait until done.
            SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE);
            // Tell state machine to go to next state when done.
            SMWaitEvents(g_autoSM, currState + 1);
            break;

         default:
            // We are done, stop the state machine.
            SMStop(g_autoSM);
            break;
         }
      }
      //
      // Execute all periodic tasks.
      //
      PIDDriveTask(g_pidDrive);
      DriveTask(g_drive);
      wait1Msec(20);
   }
}


do keep in mind that this program is actively changing while i try and figure this out so it could be different by the time you read it...

EDIT:
i just changed at the top the htComp from "sensorI2CHiTechnicCompass" to "sensorI2CCustom" and now on the top set of debug info it has everything as 0 except I is going up continously as the robot turns and on the bottom set of debug info it has T as 90 and E as 90... strangely I was at 0 only the top I was changing and the bottom O varied on KP


Wed Feb 18, 2015 9:12 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
Since you are playing with the compass on turning, the top set is not very relevant (yPidCtrl). Only the bottom set (turnPidCtrl) is interesting. The first order of business is to make sure the compass sensor actually works as expected. You may want to enter the following code to test the compass sensor. Then turn the robot around manually and check if the numbers actually follows the magnetic north closely (i.e. gives you 90 when pointing East, 180 when pointing South etc). Only then should you start debugging PID control with it. I don't quite understand your description about the behavior so you may want to describe it in more details. Since you set your target at CompassGetHeading(g_compass) + 90.0, if T is 90, it means your robot starts pointing at perfect North! I am a little suspicious of that. That's why I want you to test the compass sensor to see if it really gives you reasonable readings. For example, if the sensor doesn't really work and gives you zero all the time, that would explain why the robot is turning round and round non-stop because your E (error) is always 90.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     irSeeker,       sensorI2CCustom)
#pragma config(Sensor, S3,     htComp,         sensorI2CHiTechnicCompass)
#pragma config(Sensor, S4,     htEOPD,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     rightWheel,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     leftWheel,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     rightLifter,   tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     leftLifter,    tmotorTetrix, PIDControl)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    irServo,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    goalHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    leftDoor,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    rightDoor,            tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// include files
#include "ftclib/trcdefs.h"
#include "ftclib/dbgtrace.h"
#include "ftclib/compass.h"

COMPASS    g_compass;

task main()
{
   CompassInit(g_compass, htComp);
    while (true)
    {
        CompassTask(g_compass);
        nxtDisplayTextLine(3, "heading=%d", CompassGetHeading(g_compass));
        wait1Msec(100);
    }
}


Thu Feb 19, 2015 2:03 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 80 posts ]  Go to page Previous  1, 2, 3, 4, 5, 6  Next

Who is online

Users browsing this forum: No registered users and 2 guests


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:  
cron



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