/******************************************************************************* * * FUNCTION: Process_Gyro_Data() * * PURPOSE: Timer 2 interrupt service routine * * CALLED FROM: user_routines_fast.c/InterruptHandlerLow() * * PARAMETERS: None * * RETURNS: Nothing * * COMMENTS: * *******************************************************************************/ void Process_Gyro_Data(void) { int temp_gyro_rate; // should the completed sample set be used to calculate the gyro bias? if(calc_gyro_bias == 1) { // convert the accumulator to an integer and update gyro_bias avg_accum += Get_ADC_Result(GYRO_CHANNEL); avg_samples++; } else { // get the latest measured gyro rate temp_gyro_rate = (int)Get_ADC_Result(GYRO_CHANNEL) - gyro_bias; // update reported gyro rate and angle only if // measured gyro rate lies outside the deadband if(temp_gyro_rate < -GYRO_DEADBAND || temp_gyro_rate > GYRO_DEADBAND) { // update the gyro rate gyro_rate = temp_gyro_rate; // integrate the gyro rate to derive the heading gyro_angle += (long)temp_gyro_rate; } else { gyro_rate = 0; } } }
void gyroUpdate(void) { static int gyro_bias = 498; static long int reading = 0; static int count = 0; static long int accum = 0; if (_enabled) { // get the latest ADC conversion reading = Get_ADC_Result(1); //printf("Reading: %ld\r\n", reading); if(track_gyro_bias) { accum += reading; count++; if (count >= 7) { gyro_bias = accum / 8; track_gyro_bias = FALSE; accum = 0; count = 0; } } else { gyro_rate = reading - 1009; gyro_angle += gyro_rate; } } }
void main(void) { /*设置红外输出引脚为推挽输出*/ P1M1 = 0x00; P1M0 = 0x1C; /*PCA模块初始化*/ PCA_Init(); /*Timer定时器初始化*/ Timer0_Init(); /*串口初始化*/ Uart1_Init(); /*ADC的初始化*/ ADC_Init(); /*打开全局中断标识位*/ EA = 1; /*系统上电,管脚初始化*/ Signal_LED_PIN = 0; Check_Signal_PIN = 0; // #ifdef DEBUG // /*启动32HZ和38KHZ方波输出*/ // T38KHZ_Start(); // T32HZ_Start(); // Timer = 0; // // System_State = 1; // #endif /*输出初始化成功标识*/ printf("%s","INIT DONE \r\n"); while(1) { /*软件复位,不断电,自动下载*/ if(System_IAP_REST_PIN == 0) IAP_CONTR = 0x60; /***************************检查系统电极接触状态******************************/ if(ADC_Timer == 198) { ADC_Buffer = Get_ADC_Result(); printf("%d ",ADC_Buffer); /*清除系统状态*/ System_State = 0; if((ADC_Buffer > 140) && (ADC_Buffer <250)) { /*电极接触*/ System_State = 1; } if(System_State != System_State_Buffer) { if(System_State == 0) { T32HZ_Stop(); T38KHZ_Start(); } else if(System_State == 1) { T38KHZ_Stop(); T32HZ_Start(); } } System_State_Buffer = System_State; printf("%d ",System_State_Buffer); } else if(ADC_Timer == 200) { ADC_Timer = 0; } /*****************************************************************************/ /************************系统状态为充电电极未接触状态*************************/ if(System_State == 0) { /*****************************指示灯部分代码程序******************************/ /*未接触时,指示灯4S亮一次*/ if(Signal_LED_PIN_Timer < 50) { Signal_LED_PIN = 1; } else if(Signal_LED_PIN_Timer >= 3999) { Signal_LED_PIN_Timer = 0; Signal_LED_PIN = 0; } else { Signal_LED_PIN = 0; } /*****************************************************************************/ /****************************红外发射部分代码程序*****************************/ if((IR_Timer >= 0) && (IR_Timer < 50)) { /*全向红外发射序列*/ /*1*/ if((IR_Timer >= 0) && (IR_Timer < 3)) { LED_FF_PIN = 1; } else if(IR_Timer == 3) { LED_FF_PIN = 0; } /*0*/ else if(IR_Timer == 4) { LED_FF_PIN = 1; } else if((IR_Timer >= 5) && (IR_Timer < 7)) { LED_FF_PIN = 0; } /*0*/ else if(IR_Timer == 7) { LED_FF_PIN = 1; } else if((IR_Timer >= 8) && (IR_Timer < 10)) { LED_FF_PIN = 0; } /*1*/ else if((IR_Timer >= 10) && (IR_Timer < 13)) { LED_FF_PIN = 1; } else if(IR_Timer == 13) { LED_FF_PIN = 0; } /*0*/ else if(IR_Timer == 14) { LED_FF_PIN = 1; } else if((IR_Timer >= 15) && (IR_Timer < 17)) { LED_FF_PIN = 0; } /*1*/ else if((IR_Timer >= 17) && (IR_Timer < 20)) { LED_FF_PIN = 1; } else if(IR_Timer == 20) { LED_FF_PIN = 0; } /*0*/ else if(IR_Timer == 21) { LED_FF_PIN = 1; } else if((IR_Timer >= 22) && (IR_Timer < 24)) { LED_FF_PIN = 0; } /*1*/ else if((IR_Timer >= 24) && (IR_Timer < 27)) { LED_FF_PIN = 1; } else { LED_FF_PIN = 0; } } else if((IR_Timer >= 50) && (IR_Timer < 100)) { /*左右侧红外发送序列*/ /*L:0 R:0*/ if(IR_Timer == 50) { LED_L_PIN = 1; LED_R_PIN = 1; } else if((IR_Timer >= 51) && (IR_Timer < 53)) { LED_L_PIN = 0; LED_R_PIN = 0; } /*L:0 R:0*/ else if(IR_Timer == 53) { LED_L_PIN = 1; LED_R_PIN = 1; } else if((IR_Timer >= 54) && (IR_Timer < 56)) { LED_L_PIN = 0; LED_R_PIN = 0; } /*L:1 R:0*/ /*L:0 R:1*/ else if(IR_Timer == 56) { LED_L_PIN = 1; LED_R_PIN = 1; } else if((IR_Timer >= 57) && (IR_Timer < 59)) { LED_L_PIN = 1; LED_R_PIN = 0; } else if(IR_Timer == 59) { LED_L_PIN = 0; LED_R_PIN = 1; } else if(IR_Timer == 60) { LED_L_PIN = 1; LED_R_PIN = 1; } else if(IR_Timer == 61) { LED_L_PIN = 0; LED_R_PIN = 1; } else if(IR_Timer == 62) { LED_L_PIN = 0; LED_R_PIN = 0; } /*L:0 R:0*/ else if(IR_Timer == 63) { LED_L_PIN = 1; LED_R_PIN = 1; } else if((IR_Timer >= 64) && (IR_Timer < 66)) { LED_L_PIN = 0; LED_R_PIN = 0; } /*L:1 R:1*/ else if((IR_Timer >= 66) && (IR_Timer < 69)) { LED_L_PIN = 1; LED_R_PIN = 1; } else if(IR_Timer == 69) { LED_L_PIN = 0; LED_R_PIN = 0; } /*L:0 R:0*/ else if(IR_Timer == 70) { LED_L_PIN = 1; LED_R_PIN = 1; } else if((IR_Timer >= 71) && (IR_Timer < 73)) { LED_L_PIN = 0; LED_R_PIN = 0; } /*L:1 R:1*/ else if((IR_Timer >= 73) && (IR_Timer < 76)) { LED_L_PIN = 1; LED_R_PIN = 1; } else { LED_L_PIN = 0; LED_R_PIN = 0; } } else if((IR_Timer >= 100)) { IR_Timer = 0; LED_FF_PIN = 0; LED_L_PIN = 0; LED_R_PIN = 0; } /*****************************************************************************/ } /**************************统状态为充电电极接触状态***************************/ else if(System_State == 1) { /*****************************指示灯部分代码程序******************************/ /*接触时,指示灯常量*/ Signal_LED_PIN = 1; Signal_LED_PIN_Timer = 0; /*****************************************************************************/ /************************发射检测信号部分代码程序*****************************/ if(Check_Signal_Timer < 190) { Check_Signal_PIN = 1; } else if((Check_Signal_Timer >= 197) && (Check_Signal_Timer < 200 )) { Check_Signal_PIN = 0; } else if(Check_Signal_Timer >= 200) { Check_Signal_Timer = 0; Check_Signal_PIN = 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(); */