/****************************************************************************
*
*   Copyright (c) 2008 www.societyofrobots.com
*   (please link back if you use this code!)
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
****************************************************************************/

//Add your code here

//enter your code in control here
//photovore is just a default sample program and can be deleted




void control(void)
{
	char cByte;
	char cByte; 
	char dByte;
	char eByte;
	int modify_servo = 1;
	int sensitivity = 10;	//change this value to move servos in smaller increments.
	int speed = 1;			//walking speed
	int repeat = 0;			
	int i = 0;


	rprintfInit(uart1SendByte);
	delay_ms(50);


	rprintf("\nSELECT FROM THE FOLLOWING OPTIONS\n");
	rprintf("\n(o) LED on, \t\t(p) LED off");
	rprintf("\n(h)ome position, \t(w)alk forward, \t(s) walk reverse");
	rprintf("\n(m)odify servos\n");

	while(1)
	{
		//rprintf("\n I'm in the first while loop\n");
		cByte=uart1GetByte(); //get byte from USB on the axon
		//rprintf("I got the cByte");

		if(cByte!=-1)//no new data
		{
			//delay_ms(50);

			//rprintf("\n new data = %d \n", cByte);

			if (cByte == 'o')
				LED_on();

			if (cByte == 'p')
				LED_off();

			if (cByte == 'h')
			{
				rprintf("\n Home position\n");
				home_position();
			}

			if (cByte == 'j')
			{
				repeat = 1;
				speed = -1;
				rprintf("jog forward at what speed?\n");

				while(speed == -48 || speed == -1)
				{
					speed = (uart1GetByte() - 47);
				}


				while(repeat)
				{
					cByte = uart1GetByte();

					if (cByte == 27)
						repeat = 0;

					
					jog_forward(speed);		
				}
			}

			if (cByte == 'w')
			{
				walk_forward_control();
				speed = -1;
				rprintf("what speed?\n");

				while(speed == -48 || speed == -1)
				{
					speed = (uart1GetByte() - 47);
				}

				rprintf("\n Walking forward \n");
				walk_forward(speed);//higher number slows gait down
			}

			if (cByte == 's')
			{
				rprintf("\n Walking reverse \n");
				walk_reverse(50);//higher number slows gait down
			}

			if (cByte == 'm')
			{
				rprintf("\nWhich servo would you like to move?\n");
				rprintf("0:left foot \t 1: right foot\n");
				rprintf("2:left ankle \t 3: right ankle\n");
				rprintf("4:left knee \t 5: right knee\n");
				rprintf("6:left thigh \t 7: right thigh\n");
				rprintf("8:left hip \t 9: right hip\n");
				rprintf("\n(p)rint all servo values to screen\n");

				rprintf("\n\nWARNING: Your robot will jolt into the home position \n");

				while(1)
				{
					delay_ms(100);

					modify_servo = 1;

					cByte = uart1GetByte(); //get byte from USB on the axon;

					if(cByte != -1)//no new data
					{
						modify_servo = 1;

//********************************begin of servo modifying********************************************

						if (cByte == '0')//left foot
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_left_foot(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_left_foot(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}

								delay_ms(100);
							}
						}

//*************************************************************************************************

						if (cByte == '1')//right foot
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_right_foot(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_right_foot(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select anothe option\n");
									modify_servo = 0;
								}

								delay_ms(100);
							}
						}

//*************************************************************************************************
						
						if (cByte == '2')//left ankle
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_left_ankle(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_left_ankle(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
									{
								delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}

								delay_ms(100);
							}
						}

//*************************************************************************************************
						
						if (cByte == '3')//right ankle
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_right_ankle(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_right_ankle(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}		
															
								delay_ms(100);
							}
						}

//*************************************************************************************************
						
						if (cByte == '4')//left knee
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_left_knee(sensitivity);
									delay_ms(10);
									biped_move();	
								}								

								if (cByte == '-')
								{
									set_left_knee(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}		
															
								delay_ms(100);
							}
						}

//*************************************************************************************************						

						if (cByte == '5') //right knee
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_right_knee(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_right_knee(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}
									
								delay_ms(100);
							}
						}

//*************************************************************************************************

						if (cByte == '6')//left thigh
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
								{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_left_thigh(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_left_thigh(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}	
																
								delay_ms(100);
							}
						}

//*************************************************************************************************

						if (cByte == '7')//right thigh
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
								{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_right_thigh(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_right_thigh(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
									{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}

								delay_ms(100);
							}
						}

//*************************************************************************************************

						if (cByte == '8')//left hip
						{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
							{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_left_hip(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_left_hip(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}	
																
								delay_ms(100);
							}
						}

//*************************************************************************************************

						if (cByte == '9')//right hip
							{
							rprintf("+/- increment/decrement \t(d)one \n");

							while(modify_servo)
								{
								cByte = uart1GetByte(); // get byte from USB on the axon

								if (cByte == '+')
								{
									set_right_hip(sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == '-')
								{
									set_right_hip(-sensitivity);
									delay_ms(10);
									biped_move();
								}

								if (cByte == 'd')
								{
									delay_ms(100);
									rprintf("\n Finished modifying current servo. Select another option\n");
									modify_servo = 0;
								}	
																
								delay_ms(100);
							}
						}

						if(cByte == 'p')
						{
							print_servo_values();
							rprintf("\nselect another option\n");
						}

//*******************************end of servo modifying************************************************

					}

					delay_ms(300);
				}
			}

			delay_ms(300);
		}
	}
}