void reconstruct_reverse_track() { if (get_step_count() > 0) { TRACE_DEBUG("-- reconstruct_reverse_track -- \r\n"); char txt_buff[16]; sprintf(txt_buff, "Total steps: %2d", get_step_count()); HY1602F6_Log("Reversing track", txt_buff); init_oa_configuration(); CD4053_EnableIRSensors(); waitms(1000); int step_count = get_step_count() - 1; for (; step_count >= 0; step_count--) { char lcd_buff1[16]; sprintf(lcd_buff1, "angle: %3d", (int) steps_data[step_count]); char lcd_buff2[16]; sprintf(lcd_buff2, "step: %3d", step_count); HY1602F6_Log(lcd_buff1, lcd_buff2); turn_of_angle(steps_data[step_count]); go_prosto(); } Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); } else { TRACE_WARNING("-- No steps recorded. --\r\n"); HY1602F6_Log("Invalid track", "No data found"); } }
char go_prosto() { char result = 0; char step_count = 0; if (val == 2) { go_prosto_count++; } for (; step_count < 38; step_count++) { ADC_StartDoubleChannelConversion(ADC_CHANNEL_7, ADC_CHANNEL_5, onDistanceDataRdy); while (!ADC_IsDataReady()) ; OAA_OUTPUT oa_result = avoid_obstacles(level_mask); if (oa_result.gear_left == oa_result.gear_right) { PWM_Set(0, oa_result.speed_left); Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); waitms(100); } result = avoid_obstacle(); } return result; }
void turn_at_magnetic_angle(double angle) { double turn_angle = 0; mag_info current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid); double current_angle = compute_angle(current_mg_i.y, current_mg_i.x); turn_angle = compute_turn_angle(angle, current_angle); PWM_Set(0, 15); if (turn_angle > 0) { Kierunek(RIGHT_ENGINES, REVERSE_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); } else { Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, REVERSE_GEAR); } do { current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid); current_angle = compute_angle(current_mg_i.y, current_mg_i.x); turn_angle = compute_turn_angle(angle, current_angle); waitms(5); } while (ABS(turn_angle) > 1); Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); }
void turn_of_angle(double angle) { AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, 0x1 << 29); L3G4200D_Reset(&twid); PWM_Set(0, 15); waitms(1000); if (angle < 0) { TRACE_INFO("-- w lewo -- \r\n"); Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, REVERSE_GEAR); } else { TRACE_INFO("-- w prawo -- \r\n"); Kierunek(RIGHT_ENGINES, REVERSE_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); } while (ABS(angle) - ABS(L3G4200D_GetData().sAngle_z) > 0) { L3G4200D_ReadData(&twid); } L3G4200D_Reset(&twid); Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); AT91F_PIO_SetOutput(AT91C_BASE_PIOA, 0x1 << 29); }
//////////////////////////////////////////////////////////////////////////////// // Proste sterowanie silnikami // kierunek 0 - stop, 1 - w lewo, 2 - prosto, 3 - w prawo //////////////////////////////////////////////////////////////////////////////// inline void go(char kierunek, char pwm1, char pwm2) { switch (kierunek) { case 0: Kierunek(1, 0); Kierunek(2, 0); PWM_Set(0, pwm1); PWM_Set(1, pwm1); PWM_Set(2, pwm2); PWM_Set(3, pwm2); break; case 1: Kierunek(1, 2); Kierunek(2, 1); PWM_Set(0, pwm1); PWM_Set(1, pwm1); PWM_Set(2, pwm2); PWM_Set(3, pwm2); break; case 2: Kierunek(1, 1); Kierunek(2, 1); PWM_Set(0, pwm1); PWM_Set(1, pwm1); PWM_Set(2, pwm2); PWM_Set(3, pwm2); break; case 3: Kierunek(1, 1); Kierunek(2, 2); PWM_Set(0, pwm1); PWM_Set(1, pwm1); PWM_Set(2, pwm2); PWM_Set(3, pwm2); break; } }