/**
* \brief Update status of a player.
* \param index The index of the player.
* \param p Status of the player.
*/
void ptb::frame_play_story::update_player_status
(unsigned int index, player_status& p)
{
std::ostringstream oss1;
oss1 << gettext("Score:") << game_variables::get_persistent_score(index);
p.score_text->set_text( oss1.str() );
std::ostringstream oss2;
oss2 << game_variables::get_persistent_lives_count(index);
p.lives_text->set_text( oss2.str() );
std::ostringstream oss3;
oss3 << game_variables::get_persistent_stones_count(index);
p.stone_text->set_text( oss3.str() );
double energy(game_variables::get_max_energy(index));
p.energy->set_length(energy);
p.energy->set_level(energy);
p.energy->set_max_level(energy);
update_powers(index, p);
} // frame_play_story::update_player_status()
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;
}
