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;
}
