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!