void Calculate_Gyro_Bias(void)
{
static unsigned int i = 0;
static unsigned int j = 0;
int temp_gyro_rate;
long temp_gyro_angle;
int temp_gyro_bias;
static char done=0;
if(done==0)
{
i++;
j++; // this will rollover every ~1000 seconds
if(j == 10)
{
printf("\rCalculating Gyro Bias...");
}
if(j == 60)
{
// start a gyro bias calculation
Start_Gyro_Bias_Calc();
}
if(j == 300)
{
// terminate the gyro bias calculation
Stop_Gyro_Bias_Calc();
// reset the gyro heading angle
Reset_Gyro_Angle();
printf("Done\r");
}
if(i >= 30 && j >= 300)
{
temp_gyro_bias = Get_Gyro_Bias();
temp_gyro_rate = Get_Gyro_Rate();
temp_gyro_angle = Get_Gyro_Angle();
printf(" Gyro Bias=%d\r\n", temp_gyro_bias);
printf(" Gyro Rate=%d\r\n", temp_gyro_rate);
printf("Gyro Angle=%d\r\n\r\n", (int)temp_gyro_angle);
i = 0;
done = 1;
}
}
}
/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE: Performs the default mappings of inputs to outputs for the
* Robot Controller.
* CALLED FROM: this file, Process_Data_From_Master_uP routine
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Default_Routine(void)
{
static int temp_angle = 0;
//%d = decimal
//%i = integer
//%li = long integer
encoder_1_count = (int)Get_Encoder_1_Count();
encoder_2_count = (int)Get_Encoder_2_Count();
pan_gyro_angle = Get_Gyro_Angle();
//debug
printf("ARM: %i | WRIST: %i | cam tilt %i | cam_pan : %i | M: %li %i\r\n", encoder_1_count, encoder_2_count, PAN_SERVO, TILT_SERVO, Get_Gyro_Angle(), Get_ADC_Result(2));
//printf("%i %i %i %i", auto_switch_1, auto_switch_2, auto_switch_3, auto_switch_4)
//printf("\r\nauto_switch_1: %i | auto_switch_2: %i | auto_switch_3: %i | auto_switch_4: %i", auto_switch_1, auto_switch_2, auto_switch_3, auto_switch_4);
//DRIVETRAIN CONTROL (arcade drive)
if (!p4_sw_aux2 && !p4_sw_top) {
drive_R1 = drive_R2 = Limit_Mix(2000 + (p3_x) + p3_y - 127);
drive_L1 = drive_L2 = flip_axis(Limit_Mix(2000 + (p3_x) - p3_y + 127), 127);
tar_prev = 0;
desired_robot_angle = 0;
}else if (p4_sw_top) {
if (tar_prev == 0) {
tar_prev = 1;
desired_robot_angle = pan_gyro_angle;
//printf("\nEntered drive mode\n");
}
temp_angle = pid_control(&gyro_c, pan_gyro_angle - desired_robot_angle);
drive_R1 = drive_R2 = Limit_Mix(2000 + (temp_angle) + p3_y - 127);
drive_L1 = drive_L2 = flip_axis(Limit_Mix(2000 + (temp_angle) - p3_y + 127), 127);
}else if (p4_sw_aux2) {
drive_R1 = drive_R2 = Limit_Mix(2000 + (p3_x) + flip_axis(p3_y, 127) - 127);
drive_L1 = drive_L2 = flip_axis(Limit_Mix(2000 + (p3_x) - flip_axis(p3_y, 127) + 127), 127);
}
/*else{
drive_R1 = drive_L1 = pid_control(&Mr_Roboto, pan_gyro_angle - desired_robot_angle);
}*/
//printf("gy: %i | %i | %i\r\n", drive_R1, drive_L1, tar_prev);
//if (tar_prev && !p4_sw_aux2) {
// tar_prev = 0;
//}
//printf("pid, dones (arm, wrist, dist, angle) (%i)", score_pos);
////**V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V
//SET THE ARM POS
if (p2_sw_aux2 == 1) { //UPPER
set_arm_pos(ARM_TOP, WRIST_TOP);
score_pos = score_top;
}else if(p2_sw_aux1 == 1) { //MIDDLE
set_arm_pos(ARM_MID, WRIST_MID);
score_pos = score_mid;
}else if(p2_sw_top == 1) { //LOWER
set_arm_pos(ARM_LOW, WRIST_LOW);
score_pos = score_low;
}else if (p2_sw_trig == 1) { //HOME
set_arm_pos(ARM_HOME, WRIST_HOME);
}
//Zach's Absolute Arm Positions
//set_arm_pos(p3_y, WRIST_HOME);
//where_i_want_to_be -= (((int)p1_y - 127)/30); //PRECISION ARM
//desired_wrist_pos += (((int)p2_y - 127)/20); //PRECISION WRIST
if (where_i_want_to_be > ARM_MAX) { //ARM LIMIT
where_i_want_to_be = ARM_MAX;
}else if (where_i_want_to_be < ARM_MIN) {
where_i_want_to_be = ARM_MIN;
}
//END SET ARM POS
////**^*^*^*^*^*^*^*^*^*^^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*
if (p1_sw_aux1) { //Set pickup position
zach_var = encoder_2_count;
}
if (p1_sw_trig > 0) { //SAFETY TRIGGER
arm_l_motor = arm_r_motor = p1_y;
wrist_motor = flip_axis(p2_y, 127);
where_i_want_to_be = encoder_1_count;
desired_wrist_pos = encoder_2_count;
}else if (able_to_correct) { //CONSTANT CONTROL
arm_l_motor = arm_r_motor = pid_control(&arm, where_i_want_to_be - encoder_1_count);
wrist_motor = pid_control(&wrist, desired_wrist_pos - encoder_2_count);
}else if (p1_sw_top == 1) {
arm_l_motor = arm_r_motor = p1_y;
if (p4_sw_trig) {
desired_wrist_pos = (3 * encoder_1_count)/4 + 276 - ((int)p2_y - 127); //insert formula here.
}else{
desired_wrist_pos = zach_var; //ZACH_AND_ELLEN_RULE //245
}
wrist_motor = pid_control(&wrist, desired_wrist_pos - encoder_2_count);
}else{
arm_l_motor = arm_r_motor = wrist_motor = 127;
}
ramp_up = p4_sw_aux1;
ramp_down = !p4_sw_aux1;
//JAW CONTROL (player 1 top);
grabber = p3_sw_trig | p1_sw_aux2;
//EXTENDER ARM
if (p3_sw_top == 1 && ext_but_prev == 0) {
extender = (extender == 1) ? 0 : 1;
ext_but_prev = 1;
}else{
if (!p3_sw_top) {
ext_but_prev = 0;
}
}
extension = extender;
/*
if (track_a_light) {
if (Targets.num_of_lights == 1) {
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.c_light_angle) * 10 + pan_gyro_angle;
}else{
switch (p1_sw_top | (p1_sw_aux1 << 2) | (p1_sw_aux2 << 3)) {
case 1:
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.l_light_angle) * 10 + pan_gyro_angle;
break;
case 4:
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.c_light_angle) * 10 + pan_gyro_angle;
break;
case 8:
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.r_light_angle) * 10 + pan_gyro_angle;
break;
}
}
}*/
//COMPRESSOR CONTROL
//compressor = !pressure_switch;
} /* END Default_Routine(); */
