/*///////////////////////////////////////////////////////////////////
// File: move.c                                                    //
//                                                                 //
// Language: Interactive C                                         //
//                                                                 //
// Author: Christopher Huyler, Dan Bergeron                        //
//                                                                 //
// Description:                                                    //
// Movement functions for a handybot with two motors controlling   //
// the left and right wheels/treads of the robot.                  //
//                                                                 //
// Instructions:                                                   //
// Call define_motors(left,right) where left is the integer value  //
// of the port the left motor is connected to (0-3) and right is   //
// the integer value of the port the right motor is connected to.  //
// All turning functions use positive values for right turns, and  //
// negative values for left turns.  Each movement has two types:   //
// a timed movement function called t_<movement>() where the       //
// and an indefinate movement just called <movement>(). The timed  //
// movement will stop the motors after the time has passed.        //
//                                                                 //
// Functions:
//							Move Commands						   //
// define_motors(int,int) - input left motor and right motor       //
// forward()              - move forward                           //
// backward()             - move backward                          //
// stop()                 - stop all movement                      //
// turn(int)              - turn, +(right) -(left)                 //
// rotate(int)            - rotate,+(clockwise) -(counterclockwise)//
//                                                                 //
//						   Timed Commands						   //
// t_forward(float)       - move forward for t seconds             //
// t_backward(float)      - move backward for t seconds            //
// t_turn(float)          - turn for t seconds, +(right) -(left)   //
// t_rotate(float)        - rotate for t seconds, +(cw) -(ccw)     //
//																   //
///////////////////////////////////////////////////////////////////*/

persistent int LEFT_MOTOR_PORT;
persistent int RIGHT_MOTOR_PORT;


/* initialize motors */
void define_motors(int left, int right)
{
	LEFT_MOTOR_PORT = left;
	RIGHT_MOTOR_PORT = right;
}

/* move forward */
void forward()
{
	fd(LEFT_MOTOR_PORT);
	fd(RIGHT_MOTOR_PORT);
}

/* move backward */
void backward()
{
	bk(LEFT_MOTOR_PORT);
	bk(RIGHT_MOTOR_PORT);
}

/* stop */
void stop()
{
	off(LEFT_MOTOR_PORT);
	off(RIGHT_MOTOR_PORT);
}

/* turn -
	right if d is positive
	left if t is negative */
void turn(int d)
{
	if(d >= 0)
	{ /* turn right */
		fd(LEFT_MOTOR_PORT);
		off(RIGHT_MOTOR_PORT);
	}
	else
	{ /* turn left */
		fd(RIGHT_MOTOR_PORT);
		off(LEFT_MOTOR_PORT);
	}
}

/* rotate -
	clockwise (right) if positive value
	counterclockwise (left) if negative value */
void rotate(int d)
{
	if(d >=0)
	{ /* rotate clockwise (right) */
		fd(LEFT_MOTOR_PORT);
		bk(RIGHT_MOTOR_PORT);
	}
	else
	{ /* rotate counterclockwise (left) */
		fd(RIGHT_MOTOR_PORT);
		bk(LEFT_MOTOR_PORT);
	}
}

/* move forward for t seconds */
void t_forward(float t)
{
	fd(LEFT_MOTOR_PORT);
	fd(RIGHT_MOTOR_PORT);
	sleep(t);
	off(LEFT_MOTOR_PORT);
	off(RIGHT_MOTOR_PORT);
}

/* move backward for t seconds*/
void t_backward(float t)
{
	bk(LEFT_MOTOR_PORT);
	bk(RIGHT_MOTOR_PORT);
	sleep(t);
	off(LEFT_MOTOR_PORT);
	off(RIGHT_MOTOR_PORT);
}

/* turn for t seconds -
	right if t is positive
	left if t is negative */
void t_turn(float t)
{
	if(t >= 0.0)
	{ /* turn right */
		fd(LEFT_MOTOR_PORT);
		off(RIGHT_MOTOR_PORT);
	}
	else
	{ /* turn left */
		fd(RIGHT_MOTOR_PORT);
		off(LEFT_MOTOR_PORT);
		t = t * -1.0;
	}
	sleep(t);
	off(LEFT_MOTOR_PORT);
	off(RIGHT_MOTOR_PORT);
}

/* rotate for t seconds -
	clockwise (right) if positive value
	counterclockwise (left) if negative value */
void t_rotate(float t)
{
	if(t >= 0.0)
	{ /* rotate clockwise (right) */
		fd(LEFT_MOTOR_PORT);
		bk(RIGHT_MOTOR_PORT);
	}
	else
	{ /* rotate counterclockwise (left) */
		fd(RIGHT_MOTOR_PORT);
		bk(LEFT_MOTOR_PORT);
		t = t * -1.0;
	}
	sleep(t);
	off(LEFT_MOTOR_PORT);
	off(RIGHT_MOTOR_PORT);
}