Pid code help

#1

basically, when I run the loop, the timer doesn’t stop the loop like its supposed to and when the loop reaches the target distance it doesn’t stop either, so it goes on infinitely. pls help.

void driveStraight(int dist, int maxSpeed, int time){
  SensorValue(dgtl1) = 0;
  SensorValue(dgtl3) = 0;
  clearTimer(T1);

  float distance = 14.3239448783 * dist;
  float prevDistError = 0;
  float prevDiffError = 0;
  float distError = 0;
  float diffError = 0;
  float distI = 0;
  float diffI = 0;
  float distD = 0;
  float diffD = 0;
  float distSpeed = 0;
  float diffSpeed = 0;
  float distPk = 0.09;
  float diffPk = -0.05;
  float distIk = 0;
  float diffIk = 0;
  float distDk = 0;
  float diffDk = 0;

  while (time1[T1] < time)
  {
    distError = distance - ((SensorValue(dgtl1) + SensorValue(dgtl3))/2); //Calculate distance error
    diffError = SensorValue(dgtl1) - SensorValue(dgtl3); //Calculate difference error

    // Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
    if(abs(distError) < 143.239448783 && distError != 0)
    {
      distI = distI + distError;
    }
    else
    {
      distI = 0; //Otherwise, reset the integral
    }
    // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
    if( abs(diffError) < 30 && diffError != 0)
    {
      diffI = diffI + diffError;
    }
    else
    {
      diffI = 0; //Otherwise, reset the integral
    }
    distD = distError - prevDistError; //Calculate distance derivative
    diffD = diffError - prevDiffError; //Calculate difference derivative

    prevDistError = distError; //Update previous distance error
    prevDiffError = diffError; //Update previous difference error

    distSpeed = (distError * distPk) + (distI * distIk) + (distD * distDk); //Calculate distance speed
    diffSpeed = (diffError * diffPk) + (diffI * diffIk) + (diffD * diffDk); //Calculate difference (turn) speed

    if(distSpeed > maxSpeed) //Check that the speed is not exceeding the maximum set speed
    {
      distSpeed = maxSpeed;
    }

    if(distSpeed < -maxSpeed) //Check that the speed is not exceeding the maximum set speed
    {
      distSpeed = -maxSpeed;
    }

    motor[port2] = distSpeed + diffSpeed; //Set motor values
    motor[port3] = distSpeed - diffSpeed; //Set motor values

    wait1Msec(25); //Give the robot a (quick) break
  }
}
void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks
  // running between Autonomous and Driver controlled modes. You will need to
  // manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;
  //reset shaft encoders
SensorValue[dgtl1] = 0;
SensorValue[dgtl3] = 0;
	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                  task contr            Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

//* Top Square (closer to the flag)

task autonomous()
{

slaveMotor(port8,port3);//right
slaveMotor(port4,port2);//left
driveStraight(18,127,1000);
}

***This code is in robotc for Cortex. not v5

0 Likes

#2

You should probably tag it as RobotC instead of as VEXcode (which is V5 only).

0 Likes

#3

sorry about that lol

0 Likes