Programming Problem, or wheel size misconfiguration

Programming Support VEXcode Pro V5 Text Tech Support
#1

I have some code that takes the rotation of a mechanum wheel and converts it to inches traveled. However, the code overshoots by about twice the specified distance and reports total distance that it has traveled as the distance I told it to move, I have the wheel size set at 4.125, can anyone look at my code and tell me if it is my code, or do I have the wrong wheel size. I have no way to measure wheel size / diameter currently.

Main.cpp

//Drivebase motor ports LF, LB, RF, RB, Gyro, Wheel Size
Drive base(PORT1,PORT2,PORT10,PORT9,PORT19,4.125);

Auton command

//Distance in inches 2 is very close 50 mm
base.DriveFor(1.97);

Wheel Ratio Calculation:

Drive::Drive(int LF_port, int LB_port, int RF_port, int RB_port, inertial Gyro, float wheel_size) : LF(LF_port, true), LB(LB_port, true), RF(RF_port), RB(RB_port), Gyro(Gyro), wheel_ratio((wheel_size*M_PI)/360) {
  wheel_ratio = wheel_size/360;
}

DriveFor Function

int Drive::DriveFor(float distance){
  PID drivePID(drive_kp,drive_ki,drive_kd,drive_starti,drive_settle_error,drive_settle_time,drive_timeout);
  PID turnPID(turn_kp,turn_ki,turn_kd,turn_starti,turn_settle_error,turn_settle_time,turn_timeout);
  
  avg_left_wheel_rotation = (LF.rotation(degrees)+LB.rotation(degrees))/2;
  avg_right_wheel_rotation = (RF.rotation(degrees)+RB.rotation(degrees))/2;
  avg_wheel_rotation = (avg_left_wheel_rotation+avg_right_wheel_rotation)/2;
  
  avg_starting_postion = avg_wheel_rotation;
  starting_angle = Gyro.rotation();
  turn_volt = 0;  
  

  while(!drivePID.is_settled()){
    avg_left_wheel_rotation = (LF.rotation(degrees)+LB.rotation(degrees))/2;
    avg_right_wheel_rotation = (RF.rotation(degrees)+RB.rotation(degrees))/2;
    avg_wheel_rotation = (avg_left_wheel_rotation+avg_right_wheel_rotation)/2;
    
    starting_angle = Gyro.rotation();
    


    
    rotation = distance/wheel_ratio;
    drive_volt = drivePID.compute((rotation+avg_starting_postion)-avg_wheel_rotation);

    if (!turnPID.is_settled()){
    turn_volt = turnPID.compute(starting_angle-Gyro.rotation());
    } else {
      turn_volt = 0;
    }
    
    

    LF_volt = drive_volt - turn_volt;
    LB_volt = drive_volt - turn_volt;

    RF_volt = drive_volt + turn_volt;
    RB_volt = drive_volt + turn_volt;

    LF.spin(forward,LF_volt,volt);
    RF.spin(forward,RF_volt,volt);
    LB.spin(forward,LB_volt,volt);
    RB.spin(forward,RB_volt,volt);
    Brain.Screen.clearScreen(); Brain.Screen.setCursor(1, 1); Brain.Screen.print(rotation); Brain.Screen.print(", "); Brain.Screen.print(avg_wheel_rotation);
    Brain.Screen.newLine(); Brain.Screen.print(rotation*wheel_ratio); Brain.Screen.print(", "); Brain.Screen.print(avg_wheel_rotation*wheel_ratio);
    Drive::drive_odometry();
    wait(10,msec);

  
  }
  return 0;

}

drive_odometry function:

int Drive::drive_odometry(){
    Odom odom;
    odom_is_enabled = true;
    //while(odom_is_enabled){
    current_rot = avg_wheel_rotation*wheel_ratio; //((LF.rotation(deg)+LB.rotation(deg))/2 + (RF.rotation(deg)+RB.rotation(deg)))/2*wheel_ratio;
    x = x + odom.calculate_x(Gyro.angle(), current_rot-prev_rot);
    y = y + odom.calculate_y(Gyro.angle(), current_rot-prev_rot);
    prev_rot = current_rot;
    /*Brain.Screen.clearScreen(); Brain.Screen.setCursor(1, 1);*/ Brain.Screen.newLine(); Brain.Screen.print("X:");  Brain.Screen.print(x); Brain.Screen.print(", Y:"); Brain.Screen.print(y);
    Brain.Screen.newLine(); Brain.Screen.print(current_rot);
    //wait(10,msec);
    //}
    return 0;
}

Ignore the commented-out lines, I was trying to figure out what the problem is
Thank You for the help!

#2

Is your drive geared and if so, is that accounted for somewhere? That is the only thing I can think of to have such a drastic effect on the mentioned situation.

#3

About is not good enough in this scenario. Measure how much distance it goes actual vs. what the code expects. If it’s very close to 2 you could simply multiply expected times 2. X-drives mode √2 faster than normal drive bases and a mecanum drive is similar so that could be your problem.

#4

That worked, although I ended up needing to divide by 2 instead of multiply. Also, what do you mean by an X-drives mode √2 is faster? I’ve never used an X-drive so I am curious.

#5

I’m noticing this looks a lot like JAR Template. Did you just draw inspiration, or is this code within the larger template project? Or maybe you’ve never heard of it idk

In your Drive constructor initialize list, you’re setting the wheel ratio to size*pi/360, but then on the next line you set it to size/360. Maybe look at that.

#6

Yes, I did draw inspiration from it, thank you for publishing it! I was able to teach myself about classes
and some more advanced programming from it, But I wanted to code something myself so I could better understand it.

Thanks for spotting that, I actually don’t remember my reasoning of having two, that will help me clean up my code