Exact Distance Programming

So, I am trying to program a robot that when a bump switch is hit it will travel an exact distance. I only know how to do this in the PLTW Natural Language, and not in the VEX 2.0 language. Also, when I was doing this I was running into the issue that when measuring the difference between the desired distance and the distance that the robot traveled derived from the code, the difference was 20 cm. Is there a way to increase the accuracy , so that the robot is able to hit the exact desired distance.

Try using untilEncoderCounts(distance, sensorPort) where distance is the # of encoder ticks you want and sensorPort is the name of your quad encoder.
To figure out the # to put into the function, you need to figure out how many degrees rotation of your wheel equals the distance you want (think circumference of a circle).

What kind of sensor are you using?

20cm is a bit far off. You should get closer even if you cut off the power and it glides to rest. If you go to what you think is the distance and stop the motors momentum will carry your robot a bit further.

So what you want to do is gradually slow down your motor as you approach your goal. You do this by implementing a control loop. That just means adjust your motor power based upon how far away you are using a function.

While this is for an arm, the exact same thing can be done for a driven robot. You just need to measure how far you are away from your goal and apply a proportional constant to glide your way in.


You will know if your P value is too high if the robot goes past the target and starts coming back. This is overshoot and if it wiggle back and forth it is oscillation. The example has 0.5 for the P constant. You might want to start at something like 0.3. You can get pretty darn accurate just with just proportional control.

Once you master the proportional control, you can go on to other control loops if you like.

To get the exact distance, use PID. I just made an example for you in RobotC. This example is using an IME on a motor.

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Motor,  port1,           driveLF,       tmotorVex393HighSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           driveLR,       tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           driveRF,       tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port4,           driveRR,       tmotorVex393HighSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//------------------------PID variables------------------------//
float speed = 1.0;

#define PID_SENSOR_INDEX_GYRO    sensorValue[Gyro]
#define PID_SENSOR_INDEX_DRIVESTRAIGHT    SensorValue[driveL]
#define PID_SENSOR_INDEX_Sonar    nMotorEncoder(driveRR)

#define PID_MOTOR_INDEX_GYRO     driveRF
#define PID_MOTOR_INDEX_Sonar     driveRF
#define PID_MOTOR_INDEX_LAUNCH     flywheelR

#define PID_MOTOR_SCALE     (-1)
#define PID_SENSOR_SCALE    1

#define PID_DRIVE_MAX       127
#define PID_DRIVE_MIN     (-127)


// These variables control the "intensity" of the PID loops //
// Variables for driving straight //
float  pid_Kp_DRIVESTRAIGHT = 0.71; //was 4 for the encoders
float  pid_Ki_DRIVESTRAIGHT = 0.005; //originally 0.02 first 0.04
float  pid_Kd_DRIVESTRAIGHT = 4.4; //originally 0.0. larger the number the more corrections

static int   pidRunning = 1;
static float pidRequestedValueDRIVESTRAIGHT;

//-------------------PID calculation tasks------------------------//

// PID drive straight
task pidControllerDRIVESTRAIGHT ()
	float  pidSensorCurrentValueDrive;

	float  pidErrorDrive, pidLastErrorDrive, pidIntegralDrive;
	float  pidDerivativeDrive, pidDriveDrive;

	// If we are using an encoder then clear it
	//if( SensorType PID_SENSOR_INDEX ] == sensorQuadEncoder )
	//	SensorValue PID_SENSOR_INDEX ] = 0;

	// Init the variables
	pidLastErrorDrive  = 0;
	pidIntegralDrive   = 0;

	while( true )
		// Is PID control active ?
		if( pidRunning )
			//-----------drive calculation-------------//

			// Read the sensor value and scale
			pidSensorCurrentValueDrive = -nMotorEncoder(driveRF) * PID_SENSOR_SCALE;
															  /*^read the IME on a motor^*/

			// calculate error
			pidErrorDrive = pidSensorCurrentValueDrive - pidRequestedValueDRIVESTRAIGHT;

			// integral - if Ki is not 0
			if( pid_Ki_DRIVESTRAIGHT != 0 )
				// If we are inside controlable window then integrate the error
				if( abs(pidErrorDrive) < PID_INTEGRAL_LIMIT_DRIVESTRAIGHT )
					pidIntegralDrive = pidIntegralDrive + pidErrorDrive;
					pidIntegralDrive = 0;
				pidIntegralDrive = 0;

			// calculate the derivative
			pidDerivativeDrive = pidErrorDrive - pidLastErrorDrive;
			pidLastErrorDrive  = pidErrorDrive;

			// calculate drive
			pidDriveDrive = (pid_Kp_DRIVESTRAIGHT * pidErrorDrive)
			+ (pid_Ki_DRIVESTRAIGHT * pidIntegralDrive)
			+ (pid_Kd_DRIVESTRAIGHT * pidDerivativeDrive);

			// limit drive
			if( pidDriveDrive > PID_DRIVE_MAX )
				pidDriveDrive = PID_DRIVE_MAX;
			if( pidDriveDrive < PID_DRIVE_MIN )
				pidDriveDrive = PID_DRIVE_MIN;

			// send to motor //
			motor driveLF ] = ((pidDriveDrive * (PID_MOTOR_SCALE)) * speed);
			motor driveLR ] = ((pidDriveDrive * (PID_MOTOR_SCALE)) * speed);
			motor driveRF ] = -((pidDriveDrive * (PID_MOTOR_SCALE)) * speed);
			motor driveRR ] = -((pidDriveDrive * (PID_MOTOR_SCALE)) * speed);
			// clear all
			pidErrorDrive      = 0;
			pidLastErrorDrive  = 0;
			pidIntegralDrive   = 0;
			pidDerivativeDrive = 0;

		wait1Msec( 5 );

task main()
	while(SensorValue[bumper1] == 0) // whille not pressed
		displayLCDCenteredString(0, "Press Button");
	pidRequestedValueDRIVESTRAIGHT = 2000;

Most of the NL 2.0 commands are just better versions of the PLTW commands. So anything you need should be there.