/****************************************************************************
*
*   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 signed int head_pan;
long signed int head_tilt;
long signed int left_foot;
long signed int left_ankle;
long signed int left_knee;
long signed int left_thigh;
long signed int left_hip;
long signed int left_collar;
long signed int left_shoulder;
long signed int left_elbow;
long signed int right_foot;
long signed int right_ankle;
long signed int right_knee;
long signed int right_thigh;
long signed int right_hip;
long signed int right_collar;
long signed int right_shoulder;
long signed int right_elbow;

//initialize home variables
//tweak these numbers until biped stands up strait
long signed int home_head_pan=550;
long signed int home_head_tilt=550;
long signed int home_left_foot=550;
long signed int home_left_ankle=550;
long signed int home_left_knee=550;
long signed int home_left_thigh=550;
long signed int home_left_hip=550;
long signed int home_left_collar=550;
long signed int home_left_shoulder=550;
long signed int home_left_elbow=550;
long signed int home_right_foot=550;
long signed int home_right_ankle=550;
long signed int home_right_knee=550;
long signed int home_right_thigh=550;
long signed int home_right_hip=550;
long signed int home_right_collar=550;
long signed int home_right_shoulder=550;
long signed int home_right_elbow=550;

//define walking forward gait
int forward_count=7;//number of positions in the forward gait array
long signed int forward_head_pan[7]=		{100,150,200,150,100,0,-100};
//long signed int forward_head_tilt[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_foot[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_ankle[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_knee[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_thigh[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_hip[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_collar[7]=		{100,150,200,150,100,0,-100};
long signed int forward_left_shoulder[7]=	{100,150,200,150,100,0,-100};
long signed int forward_left_elbow[7]=		{100,150,200,150,100,0,-100};
long signed int forward_right_foot[7]=		{100,150,200,150,100,0,-100};
long signed int forward_right_ankle[7]=		{100,150,200,150,100,0,-100};
long signed int forward_right_knee[7]=		{100,150,200,150,100,0,-100};
long signed int forward_right_thigh[7]=		{100,150,200,150,100,0,-100};
long signed int forward_right_hip[7]=		{100,150,200,150,100,0,-100};
long signed int forward_right_collar[7]=	{100,150,200,150,100,0,-100};
long signed int forward_right_shoulder[7]=	{100,150,200,150,100,0,-100};
long signed int forward_right_elbow[7]=		{100,150,200,150,100,0,-100};


//various common initialized variables
int i;
int j;
float per_a;
float per_b;
int next;

//this function calculates gait meshing
void calculate_per(int speed)
	{
	//mesh current gait position with future gait position
	per_a=(speed-j)/speed;
	per_b=1-per_a;

	//make sure future gait position isn't outside array
	if(i!=forward_count-1)
		next=i+1;
	else //if it is, then restart back at zero
		next=0;
	}

//command biped to do stuff
void biped_move(void)
	{
	head_pan(head_pan+home_head_pan);
	//head_tilt(head_tilt+home_head_tilt);

	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
//all positions are zeroed onto home position
void home_position(void)
	{
	head_pan=0;
	//head_tilt=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;

	biped_move();
	}

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

	for (i=0;i<forward_count;i++)
		{
		//to test a particular position, uncomment i
		//and set it to a particular value
		//i=0;
			
		//send each command several times
		for (j=0;j<speed;j++)
			{
			calculate_per(speed);

			head_pan=forward_head_pan[i]*per_a + forward_head_pan[next]*per_b;		
			//head_tilt=forward_head_tilt[i]*per_a + forward_head_tilt[next]*per_b;

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

			right_foot=forward_right_foot[i]*per_a + forward_right_foot[next]*per_b;
			right_ankle=forward_right_ankle[i]*per_a + forward_right_ankle[next]*per_b;
			right_knee=forward_right_knee[i]*per_a + forward_right_knee[next]*per_b;
			right_thigh=forward_right_thigh[i]*per_a + forward_right_thigh[next]*per_b;
			right_hip=forward_right_hip[i]*per_a + forward_right_hip[next]*per_b;
			right_collar=forward_right_collar[i]*per_a + forward_right_collar[next]*per_b;
			right_shoulder=forward_right_shoulder[i]*per_a + forward_right_shoulder[next]*per_b;
			right_elbow=forward_right_elbow[i]*per_a + forward_right_elbow[next]*per_b;

			biped_move();
			}
		}
	}

//walking reverse gait
//does the forward gait, but in reverse, making robot walk backwards
void walk_reverse(int speed)
	{

	for (i=(forward_count-1);i==0;i--)
		{
		//to test a particular position, uncomment i
		//and set it to a particular value
		//i=0;

		//send each command several times
		for (j=0;j<speed;j++)
			{
			calculate_per(speed);

			head_pan=forward_head_pan[i]*per_a + forward_head_pan[next]*per_b;
			//head_tilt=forward_head_tilt[i]*per_a + forward_head_tilt[next]*per_b;

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

			right_foot=forward_right_foot[i]*per_a + forward_right_foot[next]*per_b;
			right_ankle=forward_right_ankle[i]*per_a + forward_right_ankle[next]*per_b;
			right_knee=forward_right_knee[i]*per_a + forward_right_knee[next]*per_b;
			right_thigh=forward_right_thigh[i]*per_a + forward_right_thigh[next]*per_b;
			right_hip=forward_right_hip[i]*per_a + forward_right_hip[next]*per_b;
			right_collar=forward_right_collar[i]*per_a + forward_right_collar[next]*per_b;
			right_shoulder=forward_right_shoulder[i]*per_a + forward_right_shoulder[next]*per_b;
			right_elbow=forward_right_elbow[i]*per_a + forward_right_elbow[next]*per_b;

			biped_move();
			}
		}
	}