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