I am using the quadrature encoder to measure the distance my robot drives, but when I use the encoder in the autonomous program, the actual distance my robot moves will be different from my manually recorded values.
Here are the steps to reproduce the error. I put a measure tape beside the robot to measure the metric distance the robot moves, all the values are recorded when the robot was completely stopped. I used two ways of both manually pushing & joystick controlling to record the encoder values (in the debugger in robotC). The calculated proportion of encoder ticks to metric distance in centimeters is about 10.6, which means 530 ticks for 50 cm. I used the code below to run the robot for multiple times for a certain speed, which is set in variable ‘s_drive_spd’. I set the speed to 127(full speed of), 80, 50, respectively. And here are the results I got.
Speed = 127:
drive(50) ----> LAST error (-32, -11), actual distance of about 57 cm
drive(-50) ----> LAST error (33, 53), actual distance of about -(57-6) cm
Speed = 80:
drive(50) ----> LAST error (-24, 7), actual distance of about 53 cm
drive(50) ----> LAST error (30, 28), actual distance of about -(53-2) cm
Speed = 50:
drive(50) ----> LAST error (10, 7), actual distance of about 49 cm
drive(50) ----> LAST error (-10, -4), actual distance of about -(49-0) cm
Note: the ‘LAST error (left_encoder_stop_err, right_encoder_stop_err)’ is an average from three independent runs the program prints.
As can be seen, when the speed is set to a higher value, the more error it would produce.
Since I have been stuck here for almost a week, I really appreciate if you can provide some hints for me to look for the error.
#pragma config(Sensor, in8, gyro, sensorGyro)
#pragma config(Sensor, dgtl1, base_quad_r, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, jump7, sensorDigitalIn)
#pragma config(Sensor, dgtl4, jump8, sensorDigitalIn)
#pragma config(Sensor, dgtl5, lift_quad, sensorQuadEncoder)
#pragma config(Sensor, dgtl7, base_quad_l, sensorQuadEncoder)
#pragma config(Sensor, dgtl9, arm_pump, sensorDigitalOut)
#pragma config(Sensor, dgtl10, gripper_pump, sensorDigitalOut)
#pragma config(Sensor, dgtl11, cube_pump, sensorDigitalOut)
#pragma config(Motor, port1, frontLeftMotor, tmotorVex393HighSpeed_HBridge, openLoop, driveLeft)
#pragma config(Motor, port2, backLeftMotor, tmotorVex393HighSpeed_MC29, openLoop, driveLeft)
#pragma config(Motor, port3, lift1, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, lift2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, lift3, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, lift4, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port7, lift5, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, lift6, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, frontRightMotor, tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight)
#pragma config(Motor, port10, backRightMotor, tmotorVex393HighSpeed_HBridge, openLoop, reversed, driveRight)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void setTurnSpeed2(int left, int right)
{
motor[frontLeftMotor] = left;
motor[backLeftMotor] = left;
motor[frontRightMotor] = right;
motor[backRightMotor] = right;
}
void drive(int dist_cm)
{
static int s_error_thres = 40;
static int s_drive_spd = 50;
float set_point = dist_cm * 10.6;
SensorValue[base_quad_l] = 0;
SensorValue[base_quad_r] = 0;
float errorl = set_point - SensorValue[base_quad_l];
float errorr = set_point - SensorValue[base_quad_r];
float error_avg = (errorl + errorr) / 2;
float drive_spd = error_avg >0? s_drive_spd:-s_drive_spd;
setDriveSpeed(drive_spd);
while (true)
{
float l_spd = drive_spd, r_spd = drive_spd;
if (abs(errorl - errorr) > 10)
{
if (errorl > errorr)
l_spd += 10;
else
r_spd += 10;
}
setTurnSpeed2(l_spd, r_spd);
if (abs(error_avg) < s_error_thres)
{
// brake
setDriveSpeed(set_point>0? -10:10);
writeDebugStream("===> Drive MID error%d,%d], set point%.2f]\n", set_point - SensorValue[base_quad_l], set_point - SensorValue[base_quad_r], set_point);
break;
}
writeDebugStreamLine("Drive in progress: error%.2f,%.2f], speed%.2f,%.2f]", errorl, errorr, l_spd, r_spd);
wait1Msec(20);
errorl = set_point - SensorValue[base_quad_l];
errorr = set_point - SensorValue[base_quad_r];
error_avg = (errorl + errorr) / 2;
}
wait1Msec(300);
stopDriveMotors();
writeDebugStreamLine("===> Drive LAST error%d,%d], set point%.2f]\n", set_point - SensorValue[base_quad_l], set_point - SensorValue[base_quad_r], set_point);
}
#define LEN(array) sizeof(array)/sizeof(array[0])
void testBase()
{
clearDebugStream();
int dists] = {50, -50, 50, -50, 50, -50, 80, -80, 100, -100};
for (unsigned int i = 0; i < LEN(dists); ++i)
{
while (true)
if (vexRT[Btn7U] == 1)
break;
writeDebugStreamLine("Drive dist %d]", dists*);
drive(dists*);
//driveQuadPid(dists*);
wait1Msec(500);
}
}