/****************************************************************************
*
*   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.
*
****************************************************************************/

//This file stores walking gaits

//initialize variables
//these variables store only current servo location
long int head;
long int left_foot;
long int left_ankle;
long int left_knee;
long int left_thigh;
long int left_hip;
long int left_collar;
long int left_shoulder;
long int left_elbow;
long int right_foot;
long int right_ankle;
long int right_knee;
long int right_thigh;
long int right_hip;
long int right_collar;
long int right_shoulder;
long int right_elbow;

//initialize home variables
//tweak these numbers until biped stands up strait
/*long int home_head=465;
long int home_left_foot=310; //SET 
long int home_left_ankle=475; //SET 
long int home_left_knee=255;  //SET 
long int home_left_thigh=385;  //SET 
long int home_left_hip=295; // SET
long int home_left_collar=550;
long int home_left_shoulder=550;
long int home_left_elbow=550;
long int home_right_foot=345;  // SET
long int home_right_ankle=250; // SET 
long int home_right_knee=420; // SET 
long int home_right_thigh=330; // SET 
long int home_right_hip=390; //SET
long int home_right_collar=550;
long int home_right_shoulder=550;
long int home_right_elbow=550;*/

long int home_head=930;
long int home_left_foot=620; //SET 
long int home_left_ankle=950; //SET 
long int home_left_knee=510;  //SET 
long int home_left_thigh=770;  //SET 
long int home_left_hip=590; // SET
long int home_left_collar=1100;
long int home_left_shoulder=1100;
long int home_left_elbow=1100;
long int home_right_foot=690;  // SET
long int home_right_ankle=500; // SET 
long int home_right_knee=840; // SET 
long int home_right_thigh=660; // SET 
long int home_right_hip=780; //SET
long int home_right_collar=1100;
long int home_right_shoulder=1100;
long int home_right_elbow=1100;


//define walking forward gait
long signed int forward_head[7]=			{100,150,250,150,100,0,-100};
long signed int forward_left_foot[7]=		{100,150,250,150,100,0,-100};
long signed int forward_left_ankle[7]=		{100,150,250,150,100,0,-100};
long signed int forward_left_knee[7]=		{100,150,250,150,100,0,-100};
long signed int forward_left_thigh[7]=		{100,150,250,150,100,0,-100};
long signed int forward_left_hip[7]=		{100,150,250,150,100,0,-100};
long signed int forward_left_collar[7]=		{100,150,250,150,100,0,-100};
long signed int forward_left_shoulder[7]=	{100,150,250,150,100,0,-100};
long signed int forward_left_elbow[7]=		{100,150,250,150,100,0,-100};
long signed int forward_right_foot[7]=		{100,150,250,150,100,0,-100};
long signed int forward_right_ankle[7]=		{100,150,250,150,100,0,-100};
long signed int forward_right_knee[7]=		{100,150,250,150,100,0,-100};
long signed int forward_right_thigh[7]=		{100,150,250,150,100,0,-100};
long signed int forward_right_hip[7]=		{100,150,250,150,100,0,-100};
long signed int forward_right_collar[7]=	{100,150,250,150,100,0,-100};
long signed int forward_right_shoulder[7]=	{100,150,250,150,100,0,-100};
long signed int forward_right_elbow[7]=		{100,150,250,150,100,0,-100};

//command biped to do stuff
void biped_move(void)
	{
	head(head+home_head);

	left_foot(left_foot+home_left_foot);
	left_ankle(left_ankle+home_left_ankle);
	left_knee(left_knee+home_left_knee);
	left_thigh(left_thigh+home_left_thigh);
	left_hip(left_hip+home_left_hip);
	left_collar(left_collar+home_left_collar);
	left_shoulder(left_shoulder+home_left_shoulder);
	left_elbow(left_elbow+home_left_elbow);

	right_foot(right_foot+home_right_foot);
	right_ankle(right_ankle+home_right_ankle);
	right_knee(right_knee+home_right_knee);
	right_thigh(right_thigh+home_right_thigh);
	right_hip(right_hip+home_right_hip);
	right_collar(right_collar+home_right_collar);
	right_shoulder(right_shoulder+home_right_shoulder);
	right_elbow(right_elbow+home_right_elbow);
	}


//home position
//this defines starting location of all servos
void home_position(void)
	{
/*	head=home_head;

	left_foot=home_left_foot;
	left_ankle=home_left_ankle;
	left_knee=home_left_knee;
	left_thigh=home_left_thigh;
	left_hip=home_left_hip;
	left_collar=home_left_collar;
	left_shoulder=home_left_shoulder;
	left_elbow=home_left_elbow;

	right_foot=home_right_foot;
	right_ankle=home_right_ankle;
	right_knee=home_right_knee;
	right_thigh=home_right_thigh;
	right_hip=home_right_hip;
	right_collar=home_right_collar;
	right_shoulder=home_right_shoulder;
	right_elbow=home_right_elbow;
	rprintf("H Pressed\r\n");
	biped_move(); */

	head=0;

	left_foot=0;
	left_ankle=0;
	left_knee=0;
	left_thigh=0;
	left_hip=0;
	left_collar=0;
	left_shoulder=0;
	left_elbow=0;

	right_foot=0;
	right_ankle=0;
	right_knee=0;
	right_thigh=0;
	right_hip=0;
	right_collar=0;
	right_shoulder=0;
	right_elbow=0;
	rprintf("H Pressed\r\n");
	biped_move();
	}

//walking forward gait
//higher speed makes robot walk slower
void walk_forward(int speed)
	{
	int i;
	int j;

	for (i=0;i<7;i++)
		{
		
		
		head=forward_head[i];

		left_foot=forward_left_foot[i];
		left_ankle=forward_left_ankle[i];
		left_knee=forward_left_knee[i];
		left_thigh=forward_left_thigh[i];
		left_hip=forward_left_hip[i];
		left_collar=forward_left_collar[i];
		left_shoulder=forward_left_shoulder[i];
		left_elbow=forward_left_elbow[i]; 

		right_foot=forward_right_foot[i];
		right_ankle=forward_right_ankle[i];
		right_knee=forward_right_knee[i];
		right_thigh=forward_right_thigh[i];
		right_hip=forward_right_hip[i];
		right_collar=forward_right_collar[i];
		right_shoulder=forward_right_shoulder[i];
		right_elbow=forward_right_elbow[i]; 
	rprintf("W Pressed\r\n");
		//send each command several times
		for (j=0;j<speed;j++)
			biped_move();
		}
	}