Robot not moving correctly

Programming Support
#1

We want the robot to move back 36 inches, but it only moves back less than a foot, why?

double dst1,dst2;
double forward_Translation(double distanceTravel) {
  double motorRot = 3.5/2.5;//1 rotation of the wheel
  double dis1 = distanceTravel/(M_PI*(wheel_radius*2))*4/3;//How many times the wheel has to rotate
  double dis2 = motorRot*dis1;//Number rotations to reach destination
  return dis2*360;
}

void reset_Motors() {
  set_Velocity(0);

  L1.setStopping(brake);
  L2.setStopping(brake);
  R1.setStopping(brake);
  R2.setStopping(brake);
}

void linear_PID(double P, double I, double D, double wanted_Position, bool forw) {
  double error = 0;
  double prev_Error = 0;
  double integral = 0;// Integral
  double derivative = 0;

  while (true) {
    // Gathering Necessary Information
    int lPos = (avrL());
    int rPos = (avrR());

    double aPos = (lPos+rPos)/2;
    // P
    prev_Error = error;
    error = wanted_Position- aPos;
    //I
    integral += fabs(error);
    //if (error <= 0||error > 100) {
     if (fabs(error) > 100) { 
      integral = 0;
    }
    double integralS = integral * I;

    if(integralS > 5) {
      integralS = 5;
    }
    //D
    derivative = error - prev_Error;

    double speed = (error * P) + integralS + derivative * D;
    // print statistics to later graph, use desmos
    printf("Wanted Position: %f \n", wanted_Position);
    printf("Average Position: %f \n", aPos);
    printf("Error: %f \n", error);
    printf("Proportion: %f \n", error * P);
    printf("Integral: %f \n", integral*I);
    printf("Derivative: %f \n", derivative*D);
    printf("Speed: %f \n", speed);
    printf("\n");

    // limit speed to not go too fast
    if (speed > 12) {
      speed = 12;
    }
    if(speed<-12)
       speed=-12;
    //if (forw) {
      L1.spin(forward,speed,vex::voltageUnits::volt);
      L2.spin(forward,speed,vex::voltageUnits::volt);
      R1.spin(forward,speed,vex::voltageUnits::volt);
      R2.spin(forward,speed,vex::voltageUnits::volt);
   /* } else {
      L1.spin(reverse,speed,vex::voltageUnits::volt);
      L2.spin(reverse,speed,vex::voltageUnits::volt);
      R1.spin(reverse,speed,vex::voltageUnits::volt);
      R2.spin(reverse,speed,vex::voltageUnits::volt);
    }
   */ vex::task::sleep(15);

    if (fabs(error) <= 5) {
      break;
    } 
  }
  reset_Motors();
}

void activateClawAuton() {
  clawActivate();
 // vex::task::sleep(900);
  while (true) {
    // Gathering Necessary Information
    int lPos = (avrL());
    int rPos = (avrR());

    double aPos = (lPos+rPos)/2;
    if(aPos>=dst1)
    {
     clawActivate();
     break;
    }
    vex::task::sleep(10);
  }
 // clawActivate();
}

void autonLeft() {
 // double
  dst1=forward_Translation(52);
  vex::thread t(activateClawAuton);
  linear_PID(0.08,0.01,0.01,forward_Translation(52),true);
  linear_PID(0.08,0.01,0.01,forward_Translation(-36),false);
  move_Rotation(-70);
  //move_Linear(-12,10);
  //Back.spinFor(forward,250,degrees);
}
``` not all code, just essential functions
#2

Have you made sure that your drivetrain is properly set up? We had this issue, but only because we set up our wheel diameter and drivetrain length wrong.

#3

Yes, it works when it goes forward, but when it clamps on a goal and goes backwards, it doesn’t go as far

#4

looks like it’s using absolute movements rather than relative, in other words, it probably reverses 52-36=16 inches back.

3 Likes
#5

Is the mobile goal sliding against the tile? If so, it may not be moving as far because the friction of the mobile goal causes it to move slower, especially since you’re operating using motor encoder ticks, not shaft encoder ticks as it relates to a free spinning wheel

1 Like
#6

An easy way to test this: run your auton without the mogo. If it goes back as far as you want, it’s friction from the mogo.

2 Likes