//
// This program controls the claw Z direction and automatic gripping of toys
// 
// by Will Gorman
//
int state = 0;
task main() {
	SetSensor(SENSOR_1, SENSOR_TOUCH);
	SetSensor(SENSOR_2, SENSOR_ROTATION);
	SetSensor(SENSOR_3, SENSOR_TOUCH);
	while (true) {
		// initial reset of state
		if (state == 0 && SENSOR_1 == 1) {
			OnRev(OUT_A);
			Wait(300);
			Off(OUT_A);
			OnRev(OUT_B);
			Wait(100);
			Off(OUT_B);
			ClearSensor(SENSOR_2);
			state = 1;
		} else if (SENSOR_3 == 1) {
			OnRev(OUT_A);
			Wait(100);
			Off(OUT_A);
			Wait(20);
			ClearSensor(SENSOR_2);
		} else if (state == 1 && SENSOR_1 == 1) {
			// lower claw
			ClearSensor(SENSOR_2);
			SetUserDisplay(SENSOR_2, 0);
			SetPower(OUT_A, OUT_FULL);
			OnFwd(OUT_A);
			until(SENSOR_2 > 88);
			SetPower(OUT_B, OUT_FULL);
			OnFwd(OUT_B);
			Off(OUT_A);
			Wait(100);
			SetPower(OUT_B, 1);
			// raise claw
			OnRev(OUT_A);
			until (SENSOR_2 <= 0);
			Off(OUT_A);
			SetUserDisplay(SENSOR_2, 0);
			state = 2;
		} else if (state == 2 && SENSOR_1 == 1) {
			// open claw
			SetPower(OUT_B, OUT_FULL);
			OnRev(OUT_B);
			Wait(100);
			Off(OUT_B);
			state = 1;
		} else if (state > 0 && SENSOR_2 > 2) {
			OnRev(OUT_A);
			Wait(10);
			Off(OUT_A);
		}
	}
}