View unanswered posts | View active topics It is currently Sat Apr 20, 2019 2:39 pm






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

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: PID Control
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).


Thu Feb 05, 2015 5:30 am
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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?


Thu Feb 05, 2015 12:49 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post 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.


Thu Feb 05, 2015 3:14 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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);
    }
}


Thu Feb 05, 2015 3:40 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post 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;


Thu Feb 05, 2015 3:48 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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


Fri Feb 06, 2015 10:14 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post 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

_________________
Thanks!


Fri Feb 06, 2015 10:37 am
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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;

}


Fri Feb 06, 2015 12:13 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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?


Fri Feb 06, 2015 8:31 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post 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).


Sat Feb 07, 2015 4:05 am
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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?


Sat Feb 07, 2015 12:38 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post 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.


Sat Feb 07, 2015 1:23 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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 :)


Sat Feb 07, 2015 1:30 pm
Profile
Rookie

Joined: Sun Feb 01, 2015 2:27 am
Posts: 3
Post 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


Sat Feb 07, 2015 2:03 pm
Profile
Rookie

Joined: Sat Jan 31, 2015 1:14 am
Posts: 38
Post 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


Sat Feb 07, 2015 2:28 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 80 posts ]  Go to page Previous  1, 2, 3, 4, 5, 6  Next

Who is online

Users browsing this forum: No registered users and 2 guests


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

Search for:
Jump to:  



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