#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               !!*//

/*

                   F U N C T I O N S   W I T H   P A R A M E T E R S
                                     Ron Kessler
                                  Created 8/9/2018

Updated 9/13/2018
			Added another way to back up using the nEncoderTarget and nMotorRunState commands.




PURPOSE: Now that you understand basic Functions, let's look at another version of these
  			 functions. Let's build one function that we can use to both move the motors in
				 any manner we wish. This will eliminate the need to write so much code.

				 I also introduce loops to show how to repeat code statements.


We will modify our Up and Back with functions project to show how this works. We will add
another function called MoveRobot. But this time, we will pass in those motor commands/values
like 50 or -50. We will send two pieces of information to the function. The desired motor
speed for the left motor and the speed for the right one.


//****************************C U S T O M   F U N C T I O N S***********************************************
STEP 1: Create another function for setting motor speed. We will tell the computer these
				numbers are going to be integers.
*/

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

}

//---my other function to set distance. This time it takes in 1 integer
void SetRobotDistance(int distance)
{
	//---get the encoder target property that is passed in to the function
	nMotorEncoderTarget[leftMotor] = distance;
	nMotorEncoderTarget[rightMotor] = distance;

}

//---my new function used to move the motors. In parentheses, I defined the data type to
//		send to the motors. In RobotC, the word "int" stands for integer. I made up the
//		names "LMotor" and RMotor. Always use something that makes sense. When we use this
//		function, we will call it and pass in the desired speed (-100 to +100) like we have been
//		doing. We can do all the turns and other movements all from one function.

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

}

//***************************************M A I N   T A S K********************************************************
//Step 2: Press F7 to update our code

task main()
{
	//Step 3: Now use our new functions. Just type in their name.It will appear in the intellisense
	//        drop down menu. Press TAB to pop in in your code! It only shows up if you F7 first.
	ResetMyEncoders();			//reset encoders
	SetRobotDistance(1800);			//now set the desired distance

	//---use a loop to repeat line 82 until our distance is reached
	//---when we reach our distance the loop finishes and the program continues....
	while(nMotorEncoder[leftMotor] < 1800)
	{
		MoveRobot(35,35);
	}


	//---We reached our distance so stop the motors, wait 3 seconds, and go backwards to where the bot started.

	//ALWAYS GOOD PRACTICE TO STOP MOTORS BEFORE CHANGING DIRECTION. SAVES MOTOR WEAR/FAILURE!!!
	MoveRobot(0,0);					//stop
	wait1Msec(3000);				//wait 3 sec

	//*********BACKUP SECTION***********

	ResetMyEncoders();						//reset to 0 at current location
	SetRobotDistance(-1800);			//GO BACK TO WHERE IT STARTED

	while(nMotorEncoder[leftMotor] > -1800)
	{
		MoveRobot(-35,-35);
	}


}