void ahead(void) { reset_counter(LEFT); reset_counter(RIGHT); while(read_encoder_counter(RIGHT) < MOVE_DURATION && read_encoder_counter(LEFT) < MOVE_DURATION) { ahead_R(); ahead_L(); control(); } if (read_encoder_counter(RIGHT) < MOVE_DURATION ) { stop_motor_L(); while (read_encoder_counter(RIGHT) < MOVE_DURATION ) { ahead_R(); } stop_motor_R(); } else if (read_encoder_counter(LEFT) < MOVE_DURATION ) { stop_motor_R(); while (read_encoder_counter(LEFT) < MOVE_DURATION ) { ahead_L(); } stop_motor_L(); } }
void turn_left(void) { reset_counter(LEFT); reset_counter(RIGHT); while(read_encoder_counter(RIGHT) < (int)(QUARTER_TURN+0.5) && read_encoder_counter(LEFT) < (int)(QUARTER_TURN-0.5)) { ahead_R(); back_L(); control(); } if (read_encoder_counter(RIGHT) < (int)(QUARTER_TURN+0.5) ) { stop_motor_L(); while (read_encoder_counter(RIGHT) < (int)(QUARTER_TURN+0.5) ) { ahead_R(); } stop_motor_R(); } else if (read_encoder_counter(LEFT) < (int)(QUARTER_TURN-0.5) ) { stop_motor_R(); while (read_encoder_counter(LEFT) < (int)(QUARTER_TURN-0.5) ) { back_L(); } stop_motor_L(); } }
void PID(void) { now_r = read_encoder_counter(RIGHT); now_l = read_encoder_counter(LEFT); float xdiff = (now_r - now_l)*0.8 + last_xdiff * 0.2; i_xdiff += xdiff; float d_xdiff = xdiff - last_xdiff; //Ver relação entre rpms pra definir relação entre ticks e fazer essa conta aqui de cima direito int now_time = timer0_ovf_count; float rpm_r = 0; float rpm_l = 0; if (now_time != last_time) { rpm_r = (now_r-last_r)*23437.5/(now_time-last_time); if (resr < 0) rpm_r *= -1; rpm_l = (now_l-last_l)*23437.5/(now_time-last_time); if (resl < 0) rpm_l *= -1; } float filtered_r = 0.5*last_rpm_r + 0.5*rpm_r; float filtered_l = 0.5*last_rpm_l + 0.5*rpm_l; float pl = target_l - filtered_l; float pr = target_r - filtered_r; il += pl; ir += pr; float dr = pr - last_pl; float dl = pl - last_pr; resl = (KpL * pl + KiL * il + KdL * dl) + (KpT * xdiff + KiT * i_xdiff); resr = (KpR * pr + KiR * ir + KdR * dr) - (KpT * xdiff + KiT * i_xdiff); // printnum(KpL * pl); // print("\t"); // printnum(KiL * il); // print("\t"); // printnum(KdL * dl); // print("\t"); // printnum(resl); // print("\t"); // printnum(filtered_l); // print("\r\n"); // printnum(now_r); // print("\t"); // printnum(now_l); // print("\r\n"); pow_right = (int)resr; pow_left = (int)resl; if (pow_right > 255) pow_right = 255; else if (pow_right < -255) pow_right = -255; else if (filtered_r < 5 && target_r == 0) pow_right = 0; if (pow_left > 255) pow_left = 255; else if (pow_left < -255) pow_left = -255; else if (filtered_l < 5 && target_l == 0) pow_left = 0; update_powers(); last_r = now_r; last_l = now_l; last_pr = pr; last_pl = pl; last_time = now_time; last_rpm_l = filtered_l; last_rpm_r = filtered_r; last_xdiff = xdiff; }