#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*

                       M O T O R   I D L E   S T A T E   D E M O
                                     Ron Kessler
                                  Created 10/21/2018

Updated 10/21/2018
			Another way to move by using the nMotorRunState command.


PURPOSE: This shows a different version of project 3-2 where I show how to test if the motors have
					reached their target using the runStateIdle property. This will automatically
					stop the robot at its target and eliminates the need for wait1Msec commands.

SETUP:		Be sure to monitor rotation in the Debug | Windows | NXT Devices window
*/

//---same function to ResetMyEncoders.
void ResetMyEncoders ()
{
	//---reset the motor encoders as before.
	nMotorEncoder[leftMotor] = 0;
	nMotorEncoder[rightMotor] = 0;

}

//---set distance
void SetRobotDistance(int distance)
{
	//---get the encoder target property that is passed in to the function
	nMotorEncoderTarget[leftMotor] = distance;
	nMotorEncoderTarget[rightMotor] = distance;

}


void MoveRobot(int LMotor, int RMotor)
{
	//---now run motors forward at whatever is passed in to this function
	motor[leftMotor] = LMotor;
	motor[rightMotor] = RMotor;

}



task main()
{

	ResetMyEncoders();			//reset encoders
	SetRobotDistance(1800);			//now set the desired distance
	MoveRobot(35,35);

	while (nMotorRunState[leftMotor] != runStateIdle || nMotorRunState[rightMotor] != runStateIdle)
		{
			//keep going until target is reached. Stops motors at target automatically
		}


	//---We reached our distance so stop the motors, wait 3 seconds, and go backwards to where the bot started.
	MoveRobot(0,0);					//stop
	wait1Msec(3000);				//wait 3 sec

	//*********BACKUP SECTION***********
	ResetMyEncoders();						//reset to 0 at current location
	SetRobotDistance(1800);
	MoveRobot(-35,-35);					//backup!

		while (nMotorRunState[leftMotor] != runStateIdle || nMotorRunState[rightMotor] != runStateIdle)
		{
			//keep going until target is reached. Stops motors at target automatically
		}

		//---add more actions here if you want to.
}