FAQ • Search • Login
View unanswered posts | View active topics It is currently Sat Jan 25, 2020 8:47 pm


Board index » Competitions & Partners » FTC Programming

All times are UTC - 5 hours [ DST ]




Reply to topic  Page 1 of 6
  [ 80 posts ]  Go to page 1, 2, 3, 4, 5, 6  Next
Print view Previous topic | Next topic 
PID Control 
Author Message
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post PID Control
Hey RobotC! Been trying to learn PID Control with our encoders and came across Titan Robotic's library and had some trouble calling for it. I keep having errors for it...I was able to narrow it down to 2 but i'm not sure how to fix them.

Code:
#pragma config(StandardModel, "RVW RANGER")
//*!!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"


task main()
{
   PIDDriveSetTarget(pidDrive, 36.0, 0.0);
}


with these errors:

    File "C:\Users\Jt Buckner\Desktop\PIDTest1.c" compiled on Feb 02 2015 20:08:59
    *Warning*:Substituting similar variable 'playImmediateTone' for 'PlayImmediateTone'. Check spelling and letter case.
    **Error**:No body declared for referenced procedure 'PIDCtrlGetInput'
    *Warning*:Substituting similar variable 'PIDDRIVE' for 'pidDrive'. Check spelling and letter case.
    **Error**:Procedure call Parameters don't match declaration for 'PIDDriveSetTarget(PIDDRIVE & pidDrive, float distSetPoint, float angleSetPoint, bool fHoldTarget, SM * sm, long evtType, unsigned long timeout)'

any and all help would be appreciated :D
Mon Feb 02, 2015 10:10 pm
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
I am not familiar with the robot configuration provided by RVW but here is a code that will use our library to do PID drive on a typical 2-motor Tetrix drive train with a gyro sensor for driving straight or turning.
Code:
#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

#define ENC_KP              3.6
#define ENC_KI              0.0
#define ENC_KD              0.0
#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200

#define GYRO_KP             5.0
#define GYRO_KI             0.0
#define GYRO_KD             0.0
#define GYRO_TOLERANCE      1.0
#define GYRO_SETTLING       200

#define CLICKS_PER_INCH     76.0

GYRO        g_gyro;         //create the gyro 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

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

    if (IsSameReference(pidCtrl, g_yPidCtrl))
    {
        inputValue = (nMotorEncoder[leftMotor] + nMotorEncoder[rightMotor])/2.0/CLICKS_PER_INCH;
    }
    else if (IsSameReference(pidCtrl, g_turnPidCtrl))
    {
        inputValue = GyroGetHeading(g_gyro);
    }
    return inputValue;
}

task main()
{
    GyroInit(g_gyro, gyroSensor);   //initialize gyro object.
    DriveInit(g_drive, leftMotor, rightMotor);  //initialize drive object
    PIDCtrlInit(g_yPidCtrl,         //initialize yPidCtrl object
                ENC_KP, ENC_KI, ENC_KD,
                ENC_TOLERANCE, ENC_SETTLING);
    PIDCtrlInit(g_turnPidCtrl,
                GYRO_KP, GYRO_KI, GYRO_KD,
                GYRO_TOLERANCE, GYRO_SETTLING);
    PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl);

    PIDDriveSetTarget(g_pidDrive, 36.0, 0.0);

    while (true)
    {
        GyroTask(g_gyro);
        PIDDriveTask(g_pidDrive);
        DriveTask(g_drive);
    }
}


Last edited by MHTS on Mon Feb 02, 2015 11:33 pm, edited 1 time in total.
Mon Feb 02, 2015 10:47 pm
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Thanks! Any idea how to do it without gyro's? we don't have one only encoders however we did just order a compass sensor
Mon Feb 02, 2015 10:50 pm
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
Thanks! Any idea how to do it without gyro's? we don't have one only encoders however we did just order a compass sensor

Sure, the library is very flexible. Without gyro, it could use the encoder to approximate the gyro.
Code:
#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

#define ENC_KP              3.6
#define ENC_KI              0.0
#define ENC_KD              0.0
#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200

#define TURN_KP             5.0
#define TURN_KI             0.0
#define TURN_KD             0.0
#define TURN_TOLERANCE      1.0
#define TURN_SETTLING       200

#define CLICKS_PER_INCH     76.0
#define CLICKS_PER_DEGREE   20.0  //??? you need to determine this number, a random number for now.

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

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

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

task main()
{
    DriveInit(g_drive, leftMotor, rightMotor);  //initialize drive object
    PIDCtrlInit(g_yPidCtrl,         //initialize yPidCtrl object
                ENC_KP, ENC_KI, ENC_KD,
                ENC_TOLERANCE, ENC_SETTLING);
    PIDCtrlInit(g_turnPidCtrl,
                TURN_KP, TURN_KI, TURN_KD,
                TURN_TOLERANCE, TURN_SETTLING);
    PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl);

    PIDDriveSetTarget(g_pidDrive, 36.0, 0.0);

    while (true)
    {
        PIDDriveTask(g_pidDrive);
        DriveTask(g_drive);
    }
}


Last edited by MHTS on Mon Feb 02, 2015 11:40 pm, edited 1 time in total.
Mon Feb 02, 2015 11:33 pm
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
so for turns just you default encoder codes such as (sudocode)

Code:
while(nEncoderValue != 1440
{
  motor[leftwheel = 70;
  motor[leftwheel = -70;
}


or is there a way using the compass sensor? the gyro seems useful we'll have to look into getting one
Mon Feb 02, 2015 11:39 pm
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
so for turns just you default encoder codes such as (sudocode)

Code:
while(nEncoderValue != 1440
{
  motor[leftwheel = 70;
  motor[leftwheel = -70;
}


or is there a way using the compass sensor? the gyro seems useful we'll have to look into getting one

I just changed the above code to use the encoder for turn as well. See above.
Mon Feb 02, 2015 11:42 pm
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
In other words, you do the following for turns:
Code:
task main()
{
    DriveInit(g_drive, leftMotor, rightMotor);  //initialize drive object
    PIDCtrlInit(g_yPidCtrl,         //initialize yPidCtrl object
                ENC_KP, ENC_KI, ENC_KD,
                ENC_TOLERANCE, ENC_SETTLING);
    PIDCtrlInit(g_turnPidCtrl,
                TURN_KP, TURN_KI, TURN_KD,
                TURN_TOLERANCE, TURN_SETTLING);
    PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl);

    PIDDriveSetTarget(g_pidDrive, 0.0, -90.0);  // Turn left 90 degrees.

    while (true)
    {
        PIDDriveTask(g_pidDrive);
        DriveTask(g_drive);
    }
}
Mon Feb 02, 2015 11:44 pm
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Thank you so much for your time and help! I'll try this tomorrow and see if I can get it to work, if not i'll post here with whatever further questions. Thanks!
Mon Feb 02, 2015 11:47 pm
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
Thanks! Any idea how to do it without gyro's? we don't have one only encoders however we did just order a compass sensor

The library doesn't care what kind of sensor you use, a gyro, a compass or even just encoders. It will just call you back (i.e. PIDCtrlGetInput) when it needs to get the robot position or robot heading. As long as you can read some sort of sensors and translate it back to inches or degrees, the PID control module is happy.
Mon Feb 02, 2015 11:53 pm
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
so to go forward then straight you put the setup then

Code:
 PIDDriveSetTarget(g_pidDrive, 36.0, 0.0); // Forward 36 inches.

while (true)
{ while loop info }

PIDDriveSetTarget(g_pidDrive, 0.0, -90.0);  // Turn left 90 degrees.


or could you just put the while loop once at the end of the program and just put say a 100 msec wait or does it require a wait? ex:

Code:
 main task
{
setup;

PIDDriveSetTarget(g_pidDrive, 36.0, 0.0); // Forward 36"

wait10msec(10); // with or without the wait?

PIDDriveSetTarget(g_pidDrive, 0.0, -90); // turn left 90 degrees

while (true)
{ while loop info }
}


BTW sorry for pseudo code
Tue Feb 03, 2015 12:04 am
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
No, the library supports multi-tasking so you need to use a state machine like this:
Code:
#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
//*!!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 ENC_KP              3.6
#define ENC_KI              0.0
#define ENC_KD              0.0
#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200

#define TURN_KP             5.0
#define TURN_KI             0.0
#define TURN_KD             0.0
#define TURN_TOLERANCE      1.0
#define TURN_SETTLING       200

#define CLICKS_PER_INCH     76.0
#define CLICKS_PER_DEGREE   20.0

#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[leftMotor] + nMotorEncoder[rightMotor])/2.0/CLICKS_PER_INCH;
    }
    else if (IsSameReference(pidCtrl, g_turnPidCtrl))
    {
        inputValue = (nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2.0/CLICKS_PER_DEGREE;
    }
    return inputValue;
}

task main()
{
    // Initialize the drive object.
    DriveInit(g_drive, leftMotor, rightMotor);
    // 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)
    {
        // Check if state machine is ready.
        if (SMIsReady(g_autoSM))
        {
            int currState = SMGetState(g_autoSM);
            switch (currState)
            {
                case SMSTATE_STARTED:
                    // Drive forward for 36 inches.
                    PIDDriveSetTarget(g_pidDrive,
                                      36.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 + 1:
                    // Turn right 90 degrees.
                    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);
    }
}


Tue Feb 03, 2015 2:00 am
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Wow, very good Library, haha my programs just got 100% more confusing and my skill tripled, haha, thank you so very much for your help! Good luck this season!
Tue Feb 03, 2015 10:47 am
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
Does your compass library have a calibration method to call? or is it mixed in with

Code:
 CompassInit


i've searched through compass.h but couldn't find anything that looked like calibration
and I could just take the gyro sample you gave and replace gyro with compass and it would work that way if i understand it right?
Tue Feb 03, 2015 1:14 pm
Profile
MHTS
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
Does your compass library have a calibration method to call? or is it mixed in with

Code:
 CompassInit


i've searched through compass.h but couldn't find anything that looked like calibration
and I could just take the gyro sample you gave and replace gyro with compass and it would work that way if i understand it right?

My understanding of the compass sensor is that it is quite different from gyro regarding calibration. For a gyro, you just have the robot sit still for a second while it reads the value in a loop to determine the offset and noise. But calibrating a compass requires you to turn the compass around in 360 degrees under 20 seconds. So essentially, it's a manual calibration process and only needs to do it once at a location. That's why the library does not contain the calibration procedure for compass.
Tue Feb 03, 2015 1:24 pm
Profile
BTB1337
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
oh ok so i could just use say, xanders calibration program and run it when i set down the robot on the field or could i say mount it on a 360 servo and have it run during initialization in autonomous
Tue Feb 03, 2015 1:32 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   Page 1 of 6
  [ 80 posts ]  Go to page 1, 2, 3, 4, 5, 6  Next

Board index » Competitions & Partners » FTC Programming

All times are UTC - 5 hours [ DST ]

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:  



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