/********************************************************************************/
/* shakey.c
 * Receives commands for our robot
 *
 * Gordon Hopper 1999
 */
/********************************************************************************/
int Kick_Armed = 0;
int packetcount = 0;
int errorcount = 0;
int main() {
	while(1)
		{
		if (start_button())
			{
				Kick_Armed = 1;
				serial_motors();
			}
		else
			reenable_pcode_serial();
		}
}

/* defines for protocol (note that IC 2.x does not support #DEFINE) */
/* These should match the codes that are being sent from the PC */

int LEFT_MOT=0x31;
int RIGHT_MOT=0x32;
int BODY_MOT=0x33;
int FORWARD = 0x38;
int BACKWARD = 0x32;
int SPINLEFT = 0x34;
int SPINRIGHT = 0x36;
int LEFTFORWARD = 0x37;
int RIGHTFORWARD = 0x39;
int LEFTBACKWARD = 0x31;
int RIGHTBACKWARD = 0x33;
int KICK=0x35;
int STATUS=0x30;
int STOP=0x35;
int RESET=0x38;
int value = 0;
int kick_count = 0;

int serial_motors() {
	int a0,a1;	/* c is next command, a0..1 are temporary arguments */
	int invalid=0;	/* number of invalid commands received */
	int cpast;
	int leftmotor;
	int rightmotor;
	char c[1];

	poke(0x1009,0x3c);
	cpast = 0;
	alloff();
	/*calibrate();*/
	bodydest=0;
	start_process(set_body());
	disable_pcode_serial(); /* that also enables my serial */

	printf ("Serial -> Motors\n");

	alloff();
	
	
	while (1) {
		while ((c[0]=serial_getchar())!=0xff)
			{
			}
	  if((c[0]=serial_getchar())==0x00)
			{
			value=serial_getchar();
			if (value>128)
				value=value-256;
			/*printf("R:%d ",value); */
			rightmotor = value;
		
			value=serial_getchar();
			if (value>128)
				value=value-256;
			/*printf("L:%d\n",value);  */
			leftmotor = value;

	/*this section turns on the brake when values of 0 are sent*/
	 		if (rightmotor==0&&leftmotor==0) 
				{
				packetcount++;
				motor(1,100);
				}	
			else
				motor(1,0);
	/*this section turns on the kicker for 10 cycles.*/
			if (rightmotor==101&&leftmotor==101)
				{
					/*motor(0,0);
					motor(2,0);
					*/
					motor(3,100);
					kick_count = 10;
					poke(0x1009,0x3c);
					poke(0x1008,0x08);
					msleep(100L);
					poke(0x1008,0x00);
				}
			else if (rightmotor==102&&leftmotor==102)
				{
				motor(0,0);
				motor(2,0);
				printf("Recieved %d Good         %d Bad\n",packetcount,errorcount-packetcount+1);
				}
			else if (rightmotor==103&&leftmotor==103)
				{
				motor(0,0);
				motor(2,0);
				printf("Starting Test\n");
				packetcount=1;
				errorcount=0;
				}
			else if (rightmotor==104&&leftmotor==104)
				{
				motor(0,0);
				motor(1,0);
				motor(2,0);
				motor(3,0);
				}
      else
				{
				if (kick_count)
					kick_count--;
				if (!kick_count)
					motor(3,0);
				errorcount++;
				motor(0,leftmotor);
				motor(2,rightmotor);
				}
			
			}
	}
	/*while(1)
	{
	c[0] = serial_getchar();
	printf("1:%x ",(int)c[0]);
  c[0] = serial_getchar();
	printf("2:%x ",(int)c[0]);
  c[0] = serial_getchar();
	printf("3:%x ",(int)c[0]);
  c[0] = serial_getchar();
	printf("4:%x\n",(int)c[0]);
	}
*/
}
/*
int calibrate()
{
	/* This function was originally designed to automatically calibrate the center motor potentiometer.  However, we found that it was easier to just send commands from our PC interface until we noticed that it was centered.  See the PC code for more information */
/*	int low,high,speed,m;

	speed=25; m=2;
	printf("Calibrate=Start Abort=Stop\n");
	while (!start_button()) {
		if (stop_button()) return (0);
		isleep(50);
		}
	/*motor(m,-speed);*/
/*	while(!stop_button()) { printf("%d: Press Stop\n",analog(0)); isleep(100); }
	/*
	motor(m,0);
	while(!start_button()) { printf("%d: Record then Press Start\n",analog(0)); isleep(100); }
	low=analog(0);
	motor(m,speed);
	while(!stop_button()) { printf("%d: Press Stop\n",analog(0)); isleep(100); }
	motor(m,0);
	while(!start_button()) { printf("%d: Record then Press Start\n",analog(0)); isleep(100); }
	high=analog(0);
	bodydest=(low+high)/2;*/
/*	return(1);
}
*/
int bodydest;
int set_body()
{
	/* this is the function which runs to keep the body motor positioned correctly */
	int cur,diff;
	while(1) {
		if (bodydest!=0) {
			cur = analog(0);
			diff = bodydest-cur;
			defer();
			defer();
			if (diff>0) {
				if (diff>6) motor(2,100);
				else if (diff>0) motor(2,70);
				else motor(2,0);
			} else { /* diff <0 */
				if (diff<-6) motor(2,-100);
				else if (diff<0) motor(2,-70);
				else motor(2,0);
			}
		}
		defer();
		defer();
		defer();
		defer();
		defer();
	}
}