[FTC]: Programming a TETRIX Encoder - Wheel Problem

[FRC Team CC]

Hey Chief Delphi,

Today we were testing the beginning portion of our autonomous program, where the robot drives off the ramp. The robot was not going down straight, so we added PIDControl to the pragmas. Although it went down straight this time, it kept going for an unlimited amount of time. In addition, it did not go at the speed that we wanted it to go - it just went at full power.

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTIRS2,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     rightTopDrive, tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S1_C1_2,     rightBottomDrive, tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S1_C4_1,     leftBottomDrive, tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C4_2,     leftTopDrive,  tmotorTetrix, PIDControl, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

i = 0;
task main ()
{
while(i < 7)
	{
		motor[leftBottomDrive] = 5;
		motor[rightBottomDrive] = 5;
		motor[leftTopDrive]=5;
		motor[rightTopDrive]=5;
		wait10Msec(40);
		i = i + 1;
	}
		motor[leftBottomDrive]=0;
		motor[rightBottomDrive]=0;
		motor[leftTopDrive]=0;
		motor[rightTopDrive]=0;
		i = 0;

}

Is it an issue with the program, and if so, what should be fixed?

Also, we were wondering how to program the robot with encoders (on the back wheels). Using the pragmas from the program above, can you create a sample program which tells the robot to move forward for 2 feet (wheel diameter 3.96 in)?

All help is appreciated.

Thanks,
Charging Champions