stop_third_line.c

#pragma config(Sensor, S3,     light,          sensorLightActive)
#pragma config(Sensor, S1,     touch,          sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
// 48 threshold

void go(int lines);
void back(void);
void pivot_turn(float degrees);
void forward(float revolutions);
void forward_cm(float cm);

task main()
{
	for (int i = 1; i < 5; i++) {
		go(i);
		motor[motorB] = 0;
		wait1Msec(200);
		back();
	}
}

void go(int lines) {
	int threshold = 48;
	int white_touches = 0;
	while (white_touches < lines) {
		nSyncedMotors = synchBC;
		nSyncedTurnRatio = 100;
		motor[motorB] = 40;
		if (SensorValue(light) <= threshold) {
			motor[motorB] = 0;
			white_touches++;

			motor[motorB] = 20;
			while (SensorValue(light) <= threshold) {}
		}
	}
	nSyncedMotors = synchNone;
}

void back(void) {
	pivot_turn(180);
	wait1Msec(1000);

	nSyncedMotors = synchBC;
	nSyncedTurnRatio = 100;

	motor[motorB] = 40;
	while	(SensorValue(touch) == 0) {}

	forward_cm(-8);
	wait1Msec(1000);
	pivot_turn(180);
}

void pivot_turn(float degrees) {
	nSyncedMotors = synchCB;
	nSyncedTurnRatio = -100;

	nMotorEncoder[motorC] = 0;
	nMotorEncoderTarget[motorC] = 365 * (degrees/180);
	motor[motorC] = (degrees >= 0) ? -20 : 20;

	while(nMotorRunState[motorC] != runStateIdle) {}

	nSyncedMotors = synchNone;
	motor[motorC] = 0;
}

void forward(float revolutions) {
	nSyncedMotors = synchBC;
	nMotorEncoder[motorB] = 0;
	nMotorEncoderTarget[motorB] = revolutions * 360;

	nSyncedTurnRatio = 100;
	motor[motorB] = (revolutions > 0) ? 40 : -40;

	while(nMotorRunState[motorB] != runStateIdle) {}
}

void forward_cm(float cm) {
	forward((5 * (cm / 90)) * 100/98);
}