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(); */