Code: #pragma config(Sensor, dgtl1, rightEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, leftEncoder, sensorQuadEncoder)
#pragma config(Motor, port2, rightMotor, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port3, leftMotor, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*----------------------------------------------------------------------------------------------------*\
|* - Moving Straight with Encoders - *|
|* ROBOTC on VEX 2.0 CORTEX *|
|* *|
|* This program implements a self-straightening algorithm that provides a higher power level to the *|
|* motor that has traveled less, determined by comparing the values of the two encoders. *|
|* There is a 2 second pause at the beginning of the program. *|
|* *|
|* ROBOT CONFIGURATION *|
|* NOTES: *|
|* 1) Reversing 'rightMotor' (port 2) in the "Motors and Sensors Setup" is needed with the *|
|* "Squarebot" model, but may not be needed for all robot configurations. *|
|* 2) Whichever encoder is being used for feedback should be cleared just before it starts *|
|* counting by using the "SensorValue(encoder) = 0;". This helps ensure consistancy. *|
|* *|
|* MOTORS & SENSORS: *|
|* [I/O Port] [Name] [Type] [Description] *|
|* Motor - Port 2 rightMotor VEX 3-wire module Right side motor *|
|* Motor - Port 3 leftMotor VEX 3-wire module Left side motor *|
|* Digital - Port 1,2 rightEncoder VEX Shaft Encoder Right side *|
|* Digital - Port 3,4 leftEncoder VEX Shaft Encoder Left side *|
\*----------------------------------------------------------------------------------------------------*/
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
wait1Msec(2000); // Robot waits for 2000 milliseconds before executing program
SensorValue[rightEncoder] = 0; // Set the encoder so that it starts counting at 0
SensorValue[leftEncoder] = 0; // Set the encoder so that it starts counting at 0
while(1 == 1) // Creates an infinite loop, since "true" always evaluates to true
{
if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
{
// Move Forward
motor[rightMotor] = 80; // Right Motor is run at power level 80
motor[leftMotor] = 80; // Left Motor is run at power level 80
}
else if(SensorValue[rightEncoder] > SensorValue[leftEncoder]) // If rightEncoder has counted more encoder counts
{
// Turn slightly right
motor[rightMotor] = 60; // Right Motor is run at power level 60
motor[leftMotor] = 80; // Left Motor is run at power level 80
}
else // Only runs if leftEncoder has counted more encoder counts
{
// Turn slightly left
motor[rightMotor] = 80; // Right Motor is run at power level 80
motor[leftMotor] = 60; // Left Motor is run at power level 60
}
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
| 
