persistent int PGAIN;
persistent int DGAIN;

/* move n counts away proportionally:
 * motor power is proportional to distance from desired counts and
 * proportional to velocity.
 * positive number: move forward; negative number: move backward */
void pd_move(int n)
{
	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));
		}

		motor(LEFT_MOTOR_PORT, lpower);						/* adjust left motor */
		motor(RIGHT_MOTOR_PORT, 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 > 50)
			done = 1;	/* 50 checks and robot of robot not changing position */
	}
	off(LEFT_MOTOR_PORT);
	off(RIGHT_MOTOR_PORT);
}

/* rotate n counts away proportionally:
 * motor power is proportional to distance from desired counts.
 * positive number: rotate clockwise; negative number: rotate ctrclockwise */
void pd_rotate(int n)
{
	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));
		}

		motor(LEFT_MOTOR_PORT, lpower);						/* adjust left motor */
		motor(RIGHT_MOTOR_PORT, 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 > 50)
			done = 1;	/* 50 checks and robot of robot not changing position */
	}
}