Robot Can't Do Tasks in Order

Programming Support VEXcode V5 Tech Support
,
1

Hello dear programmers, I did a lot of research in the forum and in AURA but I couldn’t find any answers to my problem. I wrote 2 PID control loops. One for straight drive and the second is for turning. My PID control loops are like this:
bool enableDrivePID = true;

bool resetDriveSensors = false;

int driveStraightPID(){//cm
  while(enableDrivePID)
  {
     /////////////////////////
    //Turn PID
    /////////////////////////
    if(resetDriveSensors) {
      resetDriveSensors = false;
      LeftMotor.setPosition(0,degrees);
      RightMotor.setPosition(0,degrees);
    }
    Brain.Screen.printAt( 10, 50, "distError %6.2f", distError);
    Brain.Screen.printAt( 10, 80, "diffError %6.2f", diffError);
    double leftEncoder = (LeftMotor.position(degrees) / 360) * 31.92;
    double rightEncoder = (RightMotor.position(degrees) / 360) * 31.92;

    distError = desiredValue - ((leftEncoder + rightEncoder)/2);
    diffError = leftEncoder - rightEncoder;
 
    // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
    if( std::abs(diffError) < 60 && diffError != 0)
    {
      diffIntegral = diffIntegral + diffError;
    }
    else
    {
      diffIntegral = 0; //Otherwise, reset the integral
    }
 
    distDerivative = distError - prevDistError; //Calculate distance derivative
    diffDerivative = diffError - prevDiffError; //Calculate difference derivative
 
    prevDistError = distError; //Update previous distance error
    prevDiffError = diffError; //Update previous difference error
 
    distSpeed = (distError * distkP) + (distDerivative * distkD); //Calculate distance speed
    diffSpeed = (diffError * diffkP) + (diffIntegral * diffkI) + (diffDerivative* diffkD); //Calculate difference (turn) speed
    
    LeftMotor.spin(forward, distSpeed - diffSpeed, voltageUnits::volt); //Set motor values
    RightMotor.spin(forward, distSpeed + diffSpeed, voltageUnits::volt); //Set motor values
    vex::task::sleep(20);
      
  }
  return 1;
}

int turnPID(){
  while(enableDrivePID){

    if(resetDriveSensors) {
      resetDriveSensors = false;
      LeftMotor.setPosition(0,degrees);
      RightMotor.setPosition(0,degrees);
    }
     /////////////////////////
    //Turn PID
    /////////////////////////
    Brain.Screen.printAt( 10, 50, "turnError %6.2f", turnError);
    int turnDifference = LeftMotor.position(degrees) / 10 - RightMotor.position(degrees) / 10;

    //Turn Potential
    turnError = desiredTurnValue - turnDifference;
    //Turn Derivative
    turnDerivative = turnError - prevTurnError;

    double turnMotorPower = turnError * turnkP + turnDerivative * turnkD;
    ///////////////////////////////////////////////////////////////////////

    LeftMotor.spin(forward, turnMotorPower, voltageUnits::volt);
    RightMotor.spin(forward, -turnMotorPower, voltageUnits::volt);

    prevTurnError = turnError;
    vex::task::sleep(20);
  } 
  return 1;
} 
And the autonomous code:
         /////////////////////////
        //Autonomous
        /////////////////////////
    void autonomous(void) { 
      Brain.Timer.reset();
    while(Brain.Timer.value() < 1.0){
       Brain.Screen.printAt( 10, 50, "Zaman %6.2f", Brain.Timer.value() );
        this_thread::sleep_for(20);
    } 
      desiredValue = 200;
      vex::task strDrive(driveStraightPID);
  
  resetDriveSensors = true;

  vex::task::sleep(1000);

  desiredTurnValue = 90;
  vex::task turn1(turnPID);
  wait(20,msec);
}

The problem is the robot is doing tasks simultaneously but I don’t want that. In here our scenario is that the robot will drive straight for 200 cm then turn 90 degrees. But the robot is stopping for a sec when the auton started then it is turning right for 90 degrees. Could you please help me?

2

The vex::task creates a separate thread to execute. This is nice if you want something else to happen while driving, but it’s probably not helpful right now.

Probably a good start would be to change this section to:

driveStraightPID(); 
desiredTurnValue = 90; 
turnPID();

Since your driveStraightPID() thread didn’t seem to do anything for 1 second, it is likely there is a bug in that as well, but take the task stuff out first and see what it does.

6 Likes
3

When I do what you said, the robot is staying on the driveStraightPID loop. It is not passing to turnPID. I think this is happening because we don’t have a break condition?

4

That looks like the right path to investigate next. Your turnPID function probably has the same issue

3 Likes
5

Yes, I solved the break condition problem :blush: Thank you for the help. It was really helpful.