Autonuous program only execute last function

We coded a simple skill auton program calling functions sequentially. However, only the last function get executed.
Here is our drive function:
void Drive(double distance, int speed) {
FLMotor.move_relative(distance, speed);
BLMotor.move_relative(distance, speed);
FRMotor.move_relative(distance, speed);
BRMotor.move_relative(distance, speed);
while (!((FLMotor.get_position() < distance + 5) && (FLMotor.get_position() > distance -5))) {
// Continue running this loop as long as the motor is not within ±5 units of its goal
pros::delay(2);
}
}
Here is our SkillAuton program:
void SkillsAuton(){
Drive(1000.0, 100);
pros::delay(20);
Drive(-1000.0, 100);
pros::delay(20);
Drive(1000.0, 100);
}
Only the last “Drive()” function got executed (robot moved forward). However, the first 2 “Drive()” functions were never executed. Not sure why?

Check the units that pros::delay() uses here.
Then ask yourself if the Drive function can complete in that time?

We added a delay loop after each “drive()” function. Now it becomes more weird. Now only the 2nd and 4th “drive()” got executed, meaning robot went backwards twice but never went forward.

Drive(1000.0, 100);
do {
  pros::delay(20);
} while (!AtDistanceDriveGoal(5));

pros::delay(20);
Drive(-1000.0, 100);
do {
  pros::delay(20);
} while (!AtDistanceDriveGoal(5));

pros::delay(20);
Drive(1000.0, 100);
do {
  pros::delay(20);
} while (!AtDistanceDriveGoal(5));

pros::delay(20);
Drive(-1000.0, 100);
do {
  pros::delay(20);
} while (!AtDistanceDriveGoal(5));

How do you know that it is only last command that gets executed?

Could it be the first command that starts but never reaches the target distance?

Also, could it be that wheels move unevenly and some reach target while FL doesn’t?

Could you, please, try this code:

void Drive(double distance, int speed)
{
  FLMotor.move_relative(distance, speed);
  BLMotor.move_relative(distance, speed);
  FRMotor.move_relative(distance, speed);
  BRMotor.move_relative(distance, speed);
  while(1)
  {
     double avgPos = (
             FLMotor.get_position() +
             BLMotor.get_position() +
             FRMotor.get_position() +
             BRMotor.get_position() ) / 4.0;

     if( fabs(avgPos-distance)<5.0 ) break;

     pros::delay(2);
  }
}
1 Like

There are relative position and absolute position. You may want to make sure which one you use.

I would suggest you print out your motor position on the brain screen to debug.

2 Likes