Simple ROBOTC program for a robot with two TETRIX DC motors, one NXT motors, and an NXT ultrasonic sensor

monsieur_robot.c

#pragma config(  Hubs, S1, HTMotor,   none, none, none                                  )
#pragma config(Sensor, S1,           hubby, sensorI2CMuxController                      )
#pragma config(Sensor, S2,           sonar,            sensorSONAR                      )
#pragma config( Motor, motorA,      motorA,              tmotorNXT, openLoop            )
#pragma config( Motor, motorC,            ,              tmotorNXT, openLoop            )
#pragma config( Motor, mtr_S1_C1_1, motorD,           tmotorTetrix, openLoop, driveRight)
#pragma config( Motor, mtr_S1_C1_2, motorE,           tmotorTetrix, openLoop, driveLeft )

/************************************************************\
 * This robot will move around autonomously, and decide     *
 * where to go based on ultrasonic readings taken from      *
 * around it.                                               *
 * The sensor/motor configuration is as follows:            *
 *                                                          *
 *                       D-(UU)-D                           *
 *                         \||/                             *
 *                          II                              *
 * Legend:                                                  *
 * (UU) = NXT Ultrasonic Sensor         D = TETRIX DC Motor *
 *        (mounted on an NXT motor)                         *
 *  --  = Structure                     I = Omni-Wheel      *
 *  ||  = Structure                    /\ = Structure       *
\************************************************************/

/**
 * sweep left and right.
 * if there is no object on the left and no object on the right, return  0.
 * if there is an object on the left and an object on the right, return  2.
 * if there is an object on the left and no object on the right, return -1.
 * if there is no object on the left and an object on the right, return  1.
 */
int sweep() {
	bool left = false;
	bool right = false;
	//left
	nMotorEncoder[motorA] = 0;
	bMotorReflected[motorA] = true;
	nMotorEncoderTarget[motorA] = 60;
	motor[motorA] = 10;
	while(nMotorRunState[motorA] != runStateIdle) {//wait
	}
	motor[motorA] = 0;
	if(SensorValue[S2] != 255) {
		left = true;
	}
	//right
	nMotorEncoder[motorA] = 0;
	bMotorReflected[motorA] = false;
	nMotorEncoderTarget[motorA] = 120;	
	motor[motorA] = 10;
	while(nMotorRunState[motorA] != runStateIdle) {//wait
	}
	motor[motorA] = 0;
	if(SensorValue[S2] != 255) {
		right = true;
	}
	//back to centre
	nMotorEncoder[motorA] = 0;
	bMotorReflected[motorA] = true;
	nMotorEncoderTarget[motorA] = 60;
	motor[motorA] = 10;
	while(nMotorRunState[motorA] != runStateIdle) {//wait
	}
	motor[motorA] = 0;
	//decision
	if(left && right) {
		return 2;
	} else if(!left && !right) {
		return 0;
	} else if(left && !right) {
		return -1;
	} else if(!left && right) {
		return 1;
	}
	return 50; //if we get here.... 0_0
}
/**
 * main task that runs when the program is started
 */
task main() {
	int detect_range = 30;
	motor[motorD] = 0;
	motor[motorE] = 0;
	while(true) {
		while(SensorValue[S2] >= detect_range) {
			motor[motorD] =  10;
			motor[motorE] = -10;
		}
		motor[motorD] = 0;
		motor[motorE] = 0;
		int lr = sweep();
		switch(lr) {
			case 2: //turn around
				motor[motorD] = 20;
				motor[motorE] = 20;
				wait1Msec(3000);
			break;
			case 0: //turn left (nothing on either side)
				motor[motorD] = 20;
				motor[motorE] = 20;
				wait1Msec(1000);
			break;
			case -1: //turn right
				motor[motorD] = -20;
				motor[motorE] = -20;
				wait1Msec(1000);
			break;
			case 1: //turn left (something on right)
				motor[motorD] = 20;
				motor[motorE] = 20;
				wait1Msec(1000);
		}
	}
}