ROBOTC.net forums
http://www.robotc.net/forums/

PID Control
http://www.robotc.net/forums/viewtopic.php?f=52&t=10442
Page 3 of 6

Author:  MHTS [ Thu Feb 05, 2015 5:30 am ]
Post subject:  Re: PID Control

Code:
      // Calculate runTime
     // Calculate the Time left to engage test mode
      runTime = nPgmTime*.001;
      timeLeft = abs(runTime-10);

This doesn't always work because nPgmTime is a running timestamp since powering up and doesn't get reset to zero. You are better off using the time1[T1] which you can reset.
BTB1337 wrote:
also i was thinking, when you go down the ramp even with pid, you always end up getting orientated wrong, is there a way with the compass say that when you are at the bottom of the ramp rotate back to your origional heading? i thought through it and to my knowledge couldn't find a logical way to make it work.

If you have a gyro, it will keep your robot going straight when running down the ramp. Even if you are slightly off, the gyro will tell you how much and your PID can compensate it. We just let the library handle that (i.e. PIDDriveSetTarget with an angleSetPoint of 0.0 will make sure it runs the distance without turning).

Author:  BTB1337 [ Thu Feb 05, 2015 12:49 pm ]
Post subject:  Re: PID Control

Oh ok, I thought nPgmTime was the time the program has been running, and nSysTime, was how long the nxt has been running? That was my understanding after watching the Timers debugger in robotc

and yeah we definitely need a gyro then haha, wish I had known that before we ordered a bunch of sensors not including a gyro haha. So with the gyro and you go down the ramp you never have the problem of it sliding down sideways or something like it always does? As of last qualifier we ended up using normal set motor power with a wait and had to go down backwards to go down the straightest, that or since we haven't had time to fix the weight problem, if it didn't slide down going forwards it would just stop randomly and tip over. but until we can get our hands on a gyro, would the code i posted earlier using the compass, work well enough from the floor to run our ir program accurately?

Author:  MHTS [ Thu Feb 05, 2015 3:14 pm ]
Post subject:  Re: PID Control

In theory, if the compass sensor responses fast enough, it could be used to keep the robot to go straight. We've never tried that, so I don't know if it will work. With the gyro sensor, we never have problems sliding down the ramp sideways. Having said that, you said your robot has weight problem, I assume you meant the weight on the robot is distributed unevenly. If that's the case, the gyro can compensate to a certain extent. But if the weight is too heavily biased to one side, you may have a hard time tuning the Kp that works well turning in both directions. Or are you just saying your robot is top heavy that when going down the slide the front way, it may tip over? If that's what you meant, gyro won't save you from tipping over but going down the slide slower may do the trick.
I went through the entire thread and didn't see any code that uses the compass, so I can't tell you if it will work.

Author:  BTB1337 [ Thu Feb 05, 2015 3:40 pm ]
Post subject:  Re: PID Control

My bad i thought i posted it earlier...and what i'm saying is there is more weight in the front than the back the left to right weight is good with a weight on the right wheel(countering the battery)

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, PIDControl, encoder, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     rightWheel,    tmotorTetrix, PIDControl, 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 "ftclib/drive.h"
#include "ftclib/pidctrl.h"
#include "ftclib/piddrive.h"

#define ENC_KP              10.0 // change this to meet target distance
#define ENC_KI              0.0
#define ENC_KD              0.0

#define ENC_TOLERANCE       1.0
#define ENC_SETTLING        200

#define COMP_KP             0.25 // change this to meet turn angle
#define COMP_KI             0.0
#define COMP_KD             0.0
#define COMP_TOLERANCE      1.0
#define COMP_SETTLING       200

#define CLICKS_PER_INCH     175.0 // 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

// declaring variables
int on;

void initializeRobot()
{
    servoTarget[rightpan] = 0;
      servoTarget[leftpan] = 180;
      servoTarget[irServo] = 130;
  return;
}

int timeLeft;
int runTime;

void runMode()
{
   // reset Timer1
      resetTimer(T1);
   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;
      }
   }
}

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()
{
   initializeRobot();
   runMode();

    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)
    {
          nxtDisplayTextLine(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 36 inches.
                    PIDDriveSetTarget(g_pidDrive,
                                      56.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;

                case SMSTATE_STARTED + 2:
                      // Go forward 43 inches.
                      PIDDriveSetTarget(g_pidDrive,
                                                 43.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;

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

Author:  MHTS [ Thu Feb 05, 2015 3:48 pm ]
Post subject:  Re: PID Control

Since the compass is an absolute heading device and you have set the PIDCTRLO_ABS_SETPT option, you need to specify absolute heading you want the robot to turn, not relative. If you want to turn 90 degrees from your current heading, you need to:
Code:
                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;

Author:  BTB1337 [ Fri Feb 06, 2015 10:14 am ]
Post subject:  Re: PID Control

ah ok thanks, is there a way to connect two nxt's so i can see the debug info on the nxt on a nxt i'm holding in my hand while the robot runs? our nxt is mounted sideways so it's hard to read when it's standing still, impossible to read when running a program haha

Author:  akash329dsouza [ Fri Feb 06, 2015 10:37 am ]
Post subject:  Re: PID Control

You can use the view remote screen debugging feature in RobotC
http://help.robotc.net/WebHelpArduino/index.htm#page=robotc_debugger/nxt_remotescreen/NXT%20Remote%20Screen.htm

Author:  BTB1337 [ Fri Feb 06, 2015 12:13 pm ]
Post subject:  Re: PID Control

akash329dsouza wrote:


oh ok, i thought that only worked with RVW haha, thanks!

in my pid using gyro program i get this error
Code:
**Error**:Undefined procedure 'SetSensorType'.

but there is no error shown in my program i believe it's in one of the includes?? it's happened a few times and seems to randomly disappear after awhile not sure

gyro.h
Code:
    if (SensorType[sensorID] != sensorAnalogInactive)
    {
        SetSensorType(sensorID, sensorAnalogInactive); //error occurs here
        wait1Msec(100);
    }


Also, i've got a little bit of time on my hands while waiting for parts to come in...so i was thinking to improve my knowledge and skill with robotc, how would i create a simple library? maybe to start, using the waitForStart() program i mentioned earlier (code below) that i would just #include runMode.h and then in task main just put runMode(); like it is currently?

runMode.c
Code:
 #pragma config(StandardModel, "RVW RANGER")
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an TETRIX robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}

int timeLeft;
int runTime;

void runMode()
{
   // reset Timer1
      resetTimer(T1);
   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(3000);

         return;
      }
      else if(timeLeft == 0)
      {
         eraseDisplay();
         displayTextLine(2, "   Competition");
         displayTextLine(4, "      Mode");

         sleep(3000);

         waitForStart();

         return;
      }
   }
}

task main()
{
   initializeRobot();

  runMode();

  motor[leftMotor] = 100;
  motor[rightMotor] = 100;

  sleep(3000);

  motor[leftMotor] = 0;
  motor[rightMotor] = 0;

}

Author:  BTB1337 [ Fri Feb 06, 2015 8:31 pm ]
Post subject:  Re: PID Control

Well, i think i just solved the SetSensorType problem...in the gyro.h file Set should be set so its
Code:
setSensorType
and now the error is gone, so i guess you might wanna update that?

Author:  MHTS [ Sat Feb 07, 2015 4:05 am ]
Post subject:  Re: PID Control

BTB1337 wrote:
Well, i think i just solved the SetSensorType problem...in the gyro.h file Set should be set so its
Code:
setSensorType
and now the error is gone, so i guess you might wanna update that?

Our library was written for RobotC 3.x. Unfortunately, RobotC 4.x has changed a number of things that requires our library to be changed to match. I do have a version that works with RobotC 4.x (e.g. nxtDisplayTextLine is now called displayTextLine).

Author:  BTB1337 [ Sat Feb 07, 2015 12:38 pm ]
Post subject:  Re: PID Control

yeah i noticed that, made a few changes to get rid of the annoying warnings so it's all cleaned up haha, any other changes i should make for robotc 4?

Author:  MHTS [ Sat Feb 07, 2015 1:23 pm ]
Post subject:  Re: PID Control

The syntax change is the easy part. The difficult part is to make sure the whole library still works with EV3. That will not happen until the robotics competition season is over.

Author:  BTB1337 [ Sat Feb 07, 2015 1:30 pm ]
Post subject:  Re: PID Control

right, is FTC going to make the switch to ev3 next season? our middle school FLL team has already switched and it looks so much better than when i was in middle school FLL haha we are actually working with them to start teaching the basics of robotc at the end of the year after FLL is over using robotc's graphical program and rvw.

Anyway, thanks for all the help MHTS, your help has been much appreciated :)

Author:  robofan785 [ Sat Feb 07, 2015 2:03 pm ]
Post subject:  Re: PID Control

I am trying to use encoders in order to move my linear slide up for a certain distance. However I need to get the encoder value of how high the linear slide should go. How do I do this?
The encoder value I am trying to find is for the heights of 30 cm, 60 cm, 90cm, and 120cm.
Thanks

Author:  BTB1337 [ Sat Feb 07, 2015 2:28 pm ]
Post subject:  Re: PID Control

robofan785 wrote:
I am trying to use encoders in order to move my linear slide up for a certain distance. However I need to get the encoder value of how high the linear slide should go. How do I do this?
The encoder value I am trying to find is for the heights of 30 cm, 60 cm, 90cm, and 120cm.
Thanks


i'm not sure if MHTS has a library function for this but my suggestion is to set a variable, let's just call it "arm" and set arm as = to nMotorEncoderValue of your arm and then have the nxt display "%d", arm - so something along the lines of this, i did some tests using RVW and it worked so hopefully it will work for you, just replace "armMotor" with the name of the motor controlling your arm and it should work, so if you press the left arrow on the nxt the arm should move up and if you press the right arrow on the nxt your arm should go down, however, i'm not sure if nMotorEncoder(armMotor) = 0; is the proper way to reset the encoder value but it seemed to work for me. Good luck!

Code:
int arm;

task main()
{   
   nMotorEncoder(armMotor) = 0;
   while(true)
   {
      arm = nMotorEncoderRaw(armMotor);
      displayBigTextLine(2, "Arm:");
      displayBigTextLine(4, "%d", arm);
      
      if (nNxtButtonPressed == 2)
      {
         motor[armMotor] = 50;
      }
      else if (nNxtButtonPressed == 1)
      {
         motor[armMotor] = -50;
      }
      else
      {
         motor[armMotor] = 0;
      }
   }
}


i believe that should work, and don't forget to add your motor and sensor setup at the top, also, i just remembered and i don't know if this will help or not, but i remember reading somewhere that 1 rotation is equal to 1440 on the encoder so in other words, to move 1 rotation of the motor, set the encoder target to 1440 so 2 rotations is 2880 and so on, i guess if you really wanted to you could do some math find how high the arm moves on 1 rotation and calculate it out if you wanted to

EDIT: I found where it talked about the encoder counts per rotation, it's on page 12 http://www.education.rec.ri.cmu.edu/pre ... ntable.pdf

Page 3 of 6 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/