View unanswered posts | View active topics It is currently Mon Nov 12, 2018 1:40 pm






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

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
MHTS wrote:
There are two issues. When you said +/- 20 degrees, are they from the reading of E on the NXT display? Or are they the degrees error from your physical observation? If it's the E value from the NXT display, PID is supposed to make error down to within tolerance (whatever it was). If your tolerance was 1 degree, then E should read 1 or less at the end. If it doesn't, then you need to tune Kp to the correct value until E ended up within tolerance. If it is your physical observation that the robot is 20 degree from where you want it to be, then you need to tune the DEGREES_PER_CLICK until the robot physically turn 90 degree if you set the target to 90.
Am I understanding your issue correctly?
If by changing to a fresh battery that the robot turns correctly, that's just the way PID behaves. I think one of my previous replies have talked about this subject. You need to change to a fresh battery for competition. If the battery is too weak, then the robot can't reach target. You can fix this somewhat by increasing Kp to a larger number. +/- 20 degree seems to be a lot. Usually, a weak battery will leave the error around a few degrees. In any case, play around with Kp to see if it improves things.


Ok I got the encoders to work pretty well and we finally got our gyro delivered on saturday, (the day we needed it by), what good that did for us haha

As for our ir program i have 2 programs, one that had all the PID info and stuff for each 3 positions (i just put a bunch of blocks of Pid ctrl set target) and commented out each position if that make sense, like i would have the state machine and do state 1 forward state 2 left state 3 kickstand then comment it out and put position 1, then i put state 1 forward state 2 right state 3 kickstand and comment it out, if that makes sense. i'll show the code in a minute.

Then I have another with an if statement ( and i know that i did the while improperly but i'm not quite sure the proper way to break the loop, would it be break; or return;? i don't know) it was basically a while loop with 3 if statements that said set irservo = 130 then if irseeker ==6 then run position 1, else set irserver = 145 (or whatever the number is) if irseeker = 6 then run position 2, etc for all 3 positions.

The way our irseeker works is that we mounted it on a servo and used a debug program to adjust the servo to be perfectly in the middle of zone 6 on the IR if the center goal was in position 1 position 2 and position 3 rather than having the robot move or using 3 separate numbers.

So basically i attempted to combine the irSeeker program with the PID program with state machine and I got it past the ir where it cycled through the positions and if it found zone 6 it would go to the specific if statement, the problem was getting it out of the if statement and moving to the correct state. hopefully when you read the code you can try to figure out how it works because i'm awful at explaining stuff and i appologize for this.

irSeeker.c combined with pid enc...the commented out while loop at the bottom is the basic idea of the origional irSeeker.c
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#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,    rightDoor,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    leftDoor,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    goalHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_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"
#include "RobotCDrivers/Drivers/hitechnic-irseeker-v2.h"
#include "JoystickDriver.c"
 
// define distance with encoders
#define ENC_KP              1.7 // 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             1.5 // change this to meet turn target
#define TURN_KI             0.0
#define TURN_KD             0.5
#define TURN_TOLERANCE      1.8
#define TURN_SETTLING       200
 
#define CLICKS_PER_INCH     118.5 // change this to adjust actual distance
#define CLICKS_PER_DEGREE   17.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
 
// 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 = (nMotorEncoder[leftWheel] - nMotorEncoder[rightWheel])/2.0/CLICKS_PER_DEGREE;
    }
    return inputValue;
}
 
// declaring variables
int on;
 
void initializeRobot()
{
    servoTarget[rightDoor] = 0;
        servoTarget[leftDoor] = 180;
        servoTarget[irServo] = 130;
  return;
}
 
int timeLeft;
int runTime;
 
void runMode()
{
    // reset Timer1
        clearTimer(T1);
        eraseDisplay();
    while(true)
    {
        // Calculate runTime
      // Calculate the Time left to engage test mode
        runTime = time1[T1]*.001;
        timeLeft = abs(runTime-10);
 
        // display instuctions
        displayTextLine(1, "   Press Enter");
        displayTextLine(3, "    to Engage");
        displayTextLine(5, "    Test Mode");
        displayTextLine(7, "  Time Left: %d", timeLeft);
 
        // if statement to switch waitForStart()
        if(timeLeft <= 10 && nNxtButtonPressed == 3)
        {
            eraseDisplay();
            displayTextLine(2, "   Test");
            displayTextLine(4, "   Mode");
 
            sleep(5000);
 
            return;
        }
        else if(timeLeft == 0)
        {
            eraseDisplay();
            displayTextLine(2, "   Competition");
            displayTextLine(4, "      Mode");
 
            sleep(3000);
 
            waitForStart();
 
            return;
        }
    }
}
 
 
task main()
{
  initializeRobot();
 
  waitForStart();
 
  //runMode();
 
   // ac directional value.
   int _dirAC;
 
   // we are going to set DSP mode to 1200 Hz.  By the way, DSP_1200 is a constant = 0, DSP_600 = 1.
   /*tHTIRS2DSPMode _mode = DSP_1200;
 
   // attempt to set to DSP mode.
   if (HTIRS2setDSPMode(irSeeker, _mode) == 0)
   {
      // unsuccessful at setting the mode. Make a noise to get their attention.
      PlaySound(soundBeepBeep);
      return;
    }*/
 
   // read the current modulated signal direction
   _dirAC = HTIRS2readACDir(irSeeker);
 
   // Now you can write your own code using _dirAC.  Also, you should only need to set the mode once, so
   // from now on, you just need to use the above line to poll/display the signal direction, perhaps in an endless while loop.
 
   // 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);
 
    // error: PgmCnt: 0001BB
    //              Type: 34
 
    while (true)
    {
        // display debug info on nxt
        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:
 
                    on = 1;
 
                    while (true)
                    {
 
                    servoTarget[irServo] = 130;
 
                    wait10Msec(200);
 
                    if (HTIRS2readACDir(irSeeker) == 6)
                {
                // position 1
                        SMSetState(g_autoSM, SMSTATE_STARTED + 1);
                        break;
                }
 
                // if irSeeker = 6
                else // move irServo to 140
                {
                    servoTarget[irServo] = 140;
                }
 
                wait10Msec(200);
 
                if (_dirAC = HTIRS2readACDir(irSeeker) == 6)
                {
                //position 2
                    SMWaitEvents(g_autoSM, SMSTATE_STARTED + 7;
                    break;
                }
 
                // if left ir seeker = 5 and right ir seeker = 2
                else
                    {
                    servoTarget[irServo] = 157;
                }
 
                wait10Msec(200);
 
                if (_dirAC = HTIRS2readACDir(irSeeker) == 6)
                {
                    //position 3
                        SMWaitEvents(g_autoSM, SMSTATE_STARTED + 12;
                    break;
                }
            }
 
                SMWaitEvents(g_autoSM, currState);
                break;
 
            // POSITION 1
            case SMSTATE_STARTED + 1:
                // Drive forward for 24 inches.
                PIDDriveSetTarget(g_pidDrive,
                24.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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:
                // Turn left 75 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -75.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 3:
                // Drive forward 30 inches.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 4:
                // Turn left 55 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -55.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 6:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -27.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 17);
                break;
 
            // POSITION 2
            case SMSTATE_STARTED + 7:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                24.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 8:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -110.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 9:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                15.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 10:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -65.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 11:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -40.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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, SMSTATE_STARTED + 17);
                break;
 
            // POSITION 3
            case SMSTATE_STARTED + 12:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 13:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                90.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 14:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                16.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 15:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                85.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 16:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 17:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                1000);
                // 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);
    }
}
}
 /*on = 1;
 
  while(on == 1) // while var on (var to close while loop) = 1
    {
        // check sensor value to determine center goal position
        // then run appropriate case program
 
        // move irServo to 130
        servoTarget[irServo] = 130;
 
        wait10Msec(200);
 
        if (_dirAC = HTIRS2readACDir(irSeeker) == 6)
    {
      // position 1
 
            on = 0;
    }
 
    // if irSeeker = 6
    else // move irServo to 140
    {
        servoTarget[irServo] = 140;
    }
 
    wait10Msec(200);
 
    if (_dirAC = HTIRS2readACDir(irSeeker) == 6)
    {
      //position 2
 
        on = 0;
    }
 
    // if left ir seeker = 5 and right ir seeker = 2
    else
        {
        servoTarget[irServo] = 157;
    }
 
    wait10Msec(200);
 
    if (_dirAC = HTIRS2readACDir(irSeeker) == 6)
    {
        //position 3
 
        on = 0;
    }
    }*/


PIDEnc.c
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#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,    rightDoor,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    leftDoor,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    goalHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_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"
#include "JoystickDriver.c"
 
// define distance with encoders
#define ENC_KP              1.7 // 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             1.5 // change this to meet turn target
#define TURN_KI             0.0
#define TURN_KD             0.5
#define TURN_TOLERANCE      1.8
#define TURN_SETTLING       200
 
#define CLICKS_PER_INCH     118.5 // change this to adjust actual distance
#define CLICKS_PER_DEGREE   17.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
 
// 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 = (nMotorEncoder[leftWheel] - nMotorEncoder[rightWheel])/2.0/CLICKS_PER_DEGREE;
    }
    return inputValue;
}
 
task main()
{
    waitForStart();
 
    // 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)
    {
        // display debug info on nxt
        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)
            {
            // position 1
            /*case SMSTATE_STARTED:
                // Drive forward for 24 inches.
                PIDDriveSetTarget(g_pidDrive,
                24.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 left 75 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -75.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 30 inches.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 3:
                // Turn left 55 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -55.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 4:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -27.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                24.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -110.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                15.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 3:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -65.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 4:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -40.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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;
 
////////////////////////////////////////////////////////////////////////////////////////////////////////*/
 
            // position 3
 
            /*case SMSTATE_STARTED:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                90.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                16.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 3:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                85.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 4:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                72.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -90.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                25.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 3:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                90.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 4:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 5:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                50.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 24, 2015 12:49 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
You got the right idea but you have a few problems in your code. I have corrected those problems with the following code.
First, the whole point of using a state machine is to support multi-tasking. In order to do that, you absolutely cannot have any loop or wait in the state machine loop. Putting while-loops and wait10Msec statements in the state machine while-loop violates that. All the while-loop code is basically trying to determine the center goal position. You can do it outside of the main state machine while loop. I have optimized that code with a for-loop and position tables.
Secondly, I moved all the initialization code into initializeRobot. All initialization should be done before waitForStart so it doesn't waste any of the precious autonomous time (30 seconds). In fact, I am even tempted to move the position detection for-loop into initializeRobot because the code is changing the servo angle and waiting for a whole 2 seconds for each of the 3 positions. This wasted 6 of the 30 seconds. But doing that may not work because the position detection code may run prematurely before the field setup people change the center goal position. So I left it after waitForStart but you should seriously consider not waiting that long. I'd bet you really meant 200 msec not 2 seconds.
Thirdly, when you start the state machine, you can specify the starting state, so you don't have to make a start state that do a bogus SMWaitEvent just to go to the correct state according to the center goal position.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#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,    rightDoor,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    leftDoor,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    goalHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    irServo,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
// include files
#include "JoystickDriver.c"
#include "RobotCDrivers/Drivers/hitechnic-irseeker-v2.h"
#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.7 // 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             1.5 // change this to meet turn target
#define TURN_KI             0.0
#define TURN_KD             0.5
#define TURN_TOLERANCE      1.8
#define TURN_SETTLING       200
 
#define CLICKS_PER_INCH     118.5 // change this to adjust actual distance
#define CLICKS_PER_DEGREE   17.0 // change this to adjust actual degree
 
#define EVTTYPE_PIDDRIVE    (EVTTYPE_NONE + 1)
 
#define NUM_POSITIONS       3

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
int         g_irServoAngleTable[NUM_POSITIONS] = {130, 140, 157};
int         g_startStateTable[NUM_POSITIONS] =
            {SMSTATE_STARTED + 100, SMSTATE_STARTED + 200, SMSTATE_STARTED + 300};
 
// 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 = (nMotorEncoder[leftWheel] - nMotorEncoder[rightWheel])/2.0/CLICKS_PER_DEGREE;
    }
    return inputValue;
}
 
void initializeRobot()
{
    servoTarget[rightDoor] = 0;
    servoTarget[leftDoor] = 180;
    servoTarget[irServo] = 130;

    // 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);
}
 
task main()
{
    int startState = SMSTATE_STARTED;

    initializeRobot();

    waitForStart();

    for (int i = 0; i < NUM_POSITIONS; i++)
    {
        servoTarget[irServo] = g_irServoAngleTable[i];
        wait10Msec(200);  // Do you really want to wait 2 whole seconds???
        if (HTIRS2readACDir(irSeeker) == 6)
        {
            startState = g_startStateTable[i];
            break;
        }
    }

    SMStart(g_autoSM, startState);

    while (true)
    {
        // display debug info on nxt
        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)
            {
            // POSITION 1
            case SMSTATE_STARTED + 100:
                // Drive forward for 24 inches.
                PIDDriveSetTarget(g_pidDrive,
                24.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 101:
                // Turn left 75 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -75.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 102:
                // Drive forward 30 inches.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 103:
                // Turn left 55 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -55.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 104:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -27.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 400);
                break;
 
            // POSITION 2
            case SMSTATE_STARTED + 200:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                24.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 201:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -110.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 202:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                15.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 203:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                -65.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 204:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -40.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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, SMSTATE_STARTED + 400);
                break;
 
            // POSITION 3
            case SMSTATE_STARTED + 300:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 301:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                90.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 302:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                16.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 303:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                85.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 304:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -30.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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 + 400);
                break;
 
            case SMSTATE_STARTED + 400:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                0.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                1000);
                // 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 24, 2015 4:01 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTW, I just looked at the code more carefully, what is the purpose of SMSTATE_STARTED + 400? It is calling PIDDriveSetTarget with 0.0 yTarget and 0.0 turn target. In other words, this state does absolutely nothing.


Tue Feb 24, 2015 4:11 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post Re: PID Control
MHTS wrote:
BTW, I just looked at the code more carefully, what is the purpose of SMSTATE_STARTED + 400? It is calling PIDDriveSetTarget with 0.0 yTarget and 0.0 turn target. In other words, this state does absolutely nothing.


thanks so much, that makes a lot more sense, i'm just glad I was able to get the basic idea, i was concerned I was way way way off haha. State 400 was there because origionally i thought at the end of the state at say in the origional code at the end of position one, end of state 4, it would jump down to state 400 which would stop the robot and from there would move on to the ending break...if that makes sense?


Wed Feb 25, 2015 12:28 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
BTB1337 wrote:
thanks so much, that makes a lot more sense, i'm just glad I was able to get the basic idea, i was concerned I was way way way off haha. State 400 was there because origionally i thought at the end of the state at say in the origional code at the end of position one, end of state 4, it would jump down to state 400 which would stop the robot and from there would move on to the ending break...if that makes sense?

If stopping the robot at the end is your goal, that's not necessary because if the robot did not stop, it would have stayed in a state waiting for PID to complete and would not go to state + 400 anyway. If you really want to make sure the robot stops at the end, you want to call PIDDriveReset instead of PIDDriveSetTarget of 0.0. Also remember to change all the goto SMSTATE_STARTED + 400 to currState + 1 at the end of all the goal positions.
Code:
            case SMSTATE_STARTED + 204:
                // Turn left 50 degrees.
                PIDDriveSetTarget(g_pidDrive,
                -40.0,
                0.0,
                false,
                &g_autoSM,
                EVTTYPE_PIDDRIVE,
                2500);
                // 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.
                PIDDriveReset(g_pidDrive);
                SMStop(g_autoSM);
                break;


Wed Feb 25, 2015 1:50 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 80 posts ]  Go to page Previous  1, 2, 3, 4, 5, 6

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.