Autonomous Gyro Omni Wheel Functions

/*This code will only be able to strafe straight sideways....no fancy movements or anything...for now
  It will also be able to turn (normal turning).*/

/*This code (and the above comment) are valid as of May 20, 2007.  All major revisions will be added here*/

#include "user_routines.h"


float gyro_heading=(Get_Gyro_Angle()/10);


//This only works at the beginning of Auton period to go straight


void Straight_Driving_Beginning(int str_dir){

/*
When using the seconds function, for the "function" to enter, enter "1" to call upon this function
*/

	FR=speed(100);
	FL=flip(speeD(100));

	if((gyro_heading>str_dir) && (gyro_heading<180)){
		
		FR=blend(FR,speed(8));
		FL=blend(FL,speed(-8);
		
	}
	if((gyro_heading>str_dir) && (gyro_heading>180) && (gyro_heading<=360)){
	
	/*the above happens because of the 360-0 degree offset.  because this code only works at the beginning, the str_dir should
	  be zero*/
		
		FR=blend(FR,speed(-8));
		FL=blend(FL,speed(8));
	
	}


} /*end straight_driving_beginning*/


void Straight_Driving(int input){

/*
When using the seconds function, for the "function" to enter, enter "2" to call upon this function
*/
	
	FR=speed(100);
	FL=speed(100);

	if(gyro_heading>input){
		FR=blend(FR,speed(8));
		FL=blend(FL,speed(-8);
	}
	if(gyro_heading<input){
		FR=blend(FR,speed(-8));
		FL=blend(FL,speed(8));
	}

}/*end straight_driving*/


void Turn_To(int inp){



/* This function takes a heading input (in degrees) and turns the robot to face that direction
   For now the robot may spin uncontrollably; just make sure you (the person reading this) are far
   away from the robot when the killswitch is turned off.*/

	FR=speed(100);
	FL=flip(speed(100));

	if(gyro_heading>inp){
		turnLeft();
	}
	if(gyro_heading<inp){
		turnRight();
	}

}/*end Turn_To*/


void seconds(int time,char function,heading){

/*This function waits for x amount of seconds, going by the RC loops 38 times/second*/

/* 1 - straight_driving_beginning
   2 - straight_driving   
*/

int new_time=time*38;
//int counter;   //not sure if this is needed....

switch function{

	case 1: for(counter,counter<=new_time,counter++){
			Straight_Driving_Beginning(0);
		}
		break;
	case 2: for(counter,counter<=new_time,counter++){
			Straight_Driving(heading);
		}
		break;
	}

}/*end seconds*/