#define BUTTON	SENSOR_1
#define LEFT	OUT_A
#define RIGHT	OUT_C

#define PFAC	1/2
#define IFAC    1/4
#define DFAC	-1/3

task main()
{
	SetSensor(BUTTON, SENSOR_ROTATION);
	ClearSensor(BUTTON);
	int pos = 0,                          // The 'P' in PID
		prev_pos = 0, 
		differential = 0,	              // The 'D'
		integral_sum = 0;                 // The 'I'
	int motor_power = 0;                  // Our output.  The RCX
										  // powers motors on 
										  // a scale from 0 to 7
	
	while(abs(pos)/100 < 40)              // while it hasn't tipped
										  // over hopelessly far
	{
		prev_pos = pos;
		pos = BUTTON * 100;               // increase precision
		differential = pos - prev_pos;
		integral_sum = integral_sum + pos;
		

		motor_power = PFAC*pos + DFAC*differential + IFAC*integral_sum;
										  // Apply the PID
		motor_power = motor_power / 100;  // fix precision


		if(abs(motor_power) > OUT_FULL)
			SetPower(RIGHT+LEFT, OUT_FULL);
		else
			SetPower(RIGHT+LEFT,abs(motor_power));

		if(motor_power < 0)
		{
			Rev(RIGHT+LEFT);
			On(LEFT+RIGHT);
		} else if(motor_power > 0) {
			Fwd(RIGHT+LEFT);
			On(LEFT+RIGHT);
		} else {
			Float(LEFT+RIGHT);
		}
	}
	Float(LEFT+RIGHT);
}