/*///////////////////////////////////////////////////////////////////
// File: pmove.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 using priority    //
// input for each command for use in multitasking.				   //
//                                                                 //
// 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.        //
//																   //
// The last, or only argument for each function is its 			   //
// process id (pid) which determines its priority.				   //
//                                                                 //
// Functions:													   //
//						  Priority Commands						   //
// mt_forward(int)        - move forward 			               //
// mt_backward(int)       - move backward 			               //
// mt_turn(int,int)       - turn, +(right) -(left)   			   //
// mt_rotate(int,int)     - rotate, +(cw) -(ccw)     			   //
//																   //
//					   Timed Priority Commands					   //
// mtt_forward(float)     - move forward for t seconds             //
// mtt_backward(float)    - move backward for t seconds            //
// mtt_turn(float)        - turn for t seconds, +(right) -(left)   //
// mtt_rotate(float)      - rotate for t seconds, +(cw) -(ccw)     //
//																   //
//					Proportional Priority Commands				   //
// set_PGAIN()			  - set PGAIN using knob				   //
// set_DGAIN()			  - set DGAIN using knob				   //
// mtp_move(int,int)	  - move n counts						   //
// mtp_rotate(int,int)	  - rotate n counts						   //
/////////////////////////////////////////////////////////////////////
// Global Variables												   //
//																   //
*/ persistent int PGAIN;										   /*
*/ persistent int DGAIN;										   /*
//																   //
///////////////////////////////////////////////////////////////////*/


void set_PGAIN()
{
	while(!start_button())
	{
		PGAIN = knob() % 6;
		printf("\nAdjust and hit Start: PGAIN = %d",PGAIN);
		sleep(.2);
	}
	sleep(1.0);
}

void set_DGAIN()
{
	while(!start_button())
	{
		DGAIN = knob() % 6;
		printf("\nAdjust and hit Start: DGAIN = %d",DGAIN);
		sleep(.2);
	}
	sleep(1.0);
}

/* prioritized move forward */
void mt_forward (int pid) {
  left_motor[pid]= 100;
  right_motor[pid]= 100;
}

/* prioritized move backward */
void mt_backward (int pid) {
  left_motor[pid]= -100;
  right_motor[pid]= -100;
}

/* prioritized stop */
void mt_stop (int pid) {
  left_motor[pid]= 0;
  right_motor[pid]= 0;
}

/* prioritized turn -
	right if d is positive
	left  if d is negative */
void mt_turn (int d,int pid)
{
  if (d >= 0)
  { /* turn right */
  	left_motor[pid]= 100;
  	right_motor[pid]= 0;
  }
  else
  { /* turn left */
    left_motor[pid] = 0;
    right_motor[pid] = 100;
  }
}

/* prioritized rotate -
	clockwise (right) if positive value
	counterclockwise (left) if negative value */
void mt_rotate (int d,int pid)
{
	if(d >=0)
	{ /* rotate clockwise (right) */
		left_motor[pid] = 100;
		right_motor[pid] = -100;
	}
	else
	{ /* rotate counterclockwise (left) */
		right_motor[pid] = 100;
		left_motor[pid] = -100;
	}
}

/* move forward for t seconds */
void mtt_forward(float t,int pid)
{
	left_motor[pid]= 100;
	right_motor[pid]= 100;
	sleep(t);
	left_motor[pid] = 0;
	right_motor[pid]= 0;
}

/* move backward for t seconds*/
void mtt_backward(float t,int pid)
{
	left_motor[pid]= -100;
	right_motor[pid]= -100;
	sleep(t);
	left_motor[pid] = 0;
	right_motor[pid]= 0;
}

/* turn for t seconds -
	right if t is positive
	left if t is negative */
void mtt_turn(float t,int pid)
{
	if(t >= 0.0)
	{ /* turn right */
		left_motor[pid]= 100;
		right_motor[pid]= 0;
	}
	else
	{ /* turn left */
		right_motor[pid]= 100;
		left_motor[pid] = 0;
		t = t * -1.0;
	}
	sleep(t);
	left_motor[pid] = 0;
	right_motor[pid]= 0;
}

/* rotate for t seconds -
	clockwise (right) if positive value
	counterclockwise (left) if negative value */
void mtt_rotate(float t,int pid)
{
	if(t >= 0.0)
	{ /* rotate clockwise (right) */
		left_motor[pid]= 100;
		right_motor[pid]= -100;
	}
	else
	{ /* rotate counterclockwise (left) */
		right_motor[pid]= 100;
		left_motor[pid]= -100;
		t = t * -1.0;
	}
	sleep(t);
	left_motor[pid] = 0;
	right_motor[pid]= 0;
}

/* move forward process */
void mtp_forward(int pid)
{
	encoder10_counts = 0;
	encoder12_counts = 0;
	left_motor[pid] = 100;
	right_motor[pid] = 100;
	while(!stop_button())
	{
		if (encoder10_counts < encoder12_counts)
		{
			left_motor[pid] = 100;
			right_motor[pid] = 20;
		}
		else if (encoder12_counts < encoder10_counts)
		{
			left_motor[pid] = 20;
			right_motor[pid] = 100;
		}
	}
}

/* move n counts away proportionally:
 * motor power is proportional to distance from desired counts.
 * positive number: move forward; negative number: move backward */
void mtp_move(int n, int pid)
{
	int done = 0;
	int check = 0;		/* checks several times to make sure robot is finshed */
	int lpower, rpower; /* proportional power of let and right motors */

	encoder10_counts = 0;/* reset left shaft encoder counts  */
	encoder12_counts = 0;/* reste right shaft encoder counts */

	while(!done)
	{
		printf("\nCounts = %d, %d",encoder10_counts, encoder12_counts);
		lpower = PGAIN * (n - encoder10_counts)
						- DGAIN * encoder10_velocity;	/* set left power */
		rpower = PGAIN * (n - encoder12_counts)
						- DGAIN * encoder12_velocity;	/* set right power */
		if(lpower > 100) lpower = 100;
		if(lpower <-100) lpower =-100;
		if(rpower > 100) rpower = 100;
		if(rpower <-100) rpower =-100;

		if((n - encoder10_counts) >= 0 && (n - encoder12_counts) >= 0)
		{
			if ((n - encoder10_counts) < (n - encoder12_counts))
				lpower = (int) ((float) lpower - ((float) lpower / 1.05));
			else if ((n - encoder10_counts) > (n - encoder12_counts))
				rpower = (int) ((float) rpower - ((float) rpower / 1.05));
		}
		else if((n - encoder10_counts) < 0 && (n - encoder12_counts) < 0)
		{
			if ((n - encoder10_counts) > (n - encoder12_counts))
				lpower = (int) ((float) lpower + ((float) lpower / 1.05));
			else if ((n - encoder10_counts) < (n - encoder12_counts))
				rpower = (int) ((float) rpower + ((float) rpower / 1.05));
		}

		left_motor[pid] = lpower;					/* adjust left motor */
		right_motor[pid] = rpower;					/* adjust right motor */

		if(encoder10_counts == n && encoder10_counts == n)
			check++;	/* increment check if at desired location */
		else
			check = 0;	/* not finished */
		if(check > 20)
			done = 1;	/* 50 checks and robot of robot not changing position */
	}
	left_motor[pid] = 0;					/* adjust left motor */
	right_motor[pid] = 0;					/* adjust right motor */

}


/* rotate n counts away proportionally:
 * motor power is proportional to distance from desired counts.
 * positive number: rotate clockwise; negative number: rotate ctrclockwise */
void mtp_rotate(int n, int pid)
{
	int done = 0;
	int check = 0;		/* checks several times to make sure robot is finshed */
	int lpower, rpower; /* proportional power of let and right motors */

	encoder10_counts = 0;/* reset left shaft encoder counts  */
	encoder12_counts = 0;/* reste right shaft encoder counts */

	while(!done)
	{
		printf("\nCounts = %d, %d",encoder10_counts, encoder12_counts);
		lpower = PGAIN * (n - encoder10_counts)
						- DGAIN * encoder10_velocity;	/* set left power */
		rpower = PGAIN * ((-n) - encoder12_counts)
						- DGAIN * encoder12_velocity;	/* set right power */

		if((n - encoder10_counts) >= 0 && ((-n) - encoder12_counts) >= 0)
		{
			if ((n - encoder10_counts) < ((-n) - encoder12_counts))
				lpower = (int) ((float) lpower - ((float) lpower / 1.05));
			else if ((n - encoder10_counts) > ((-n) - encoder12_counts))
				rpower = (int) ((float) rpower + ((float) rpower / 1.05));
		}
		else if((n - encoder10_counts) < 0 && ((-n) - encoder12_counts) < 0)
		{
			if ((n - encoder10_counts) > ((-n) - encoder12_counts))
				lpower = (int) ((float) lpower + ((float) lpower / 1.05));
			else if ((n - encoder10_counts) < ((-n) - encoder12_counts))
				rpower = (int) ((float) rpower - ((float) rpower / 1.05));
		}

		left_motor[pid] = lpower;					/* adjust left motor */
		right_motor[pid] = rpower;					/* adjust right motor */

		if(encoder10_counts == n && encoder10_counts == n)
			check++;	/* increment check if at desired location */
		else
			check = 0;	/* not finished */
		if(check > 20)
			done = 1;	/* 50 checks and robot of robot not changing position */
	}
	left_motor[pid] = 0;					/* adjust left motor */
	right_motor[pid] = 0;					/* adjust right motor */
}