/**
* Makes the robot attempt to follow a line, halting should it lose it.
*/
void follow_line()
{
LineInfo line;
int movement = 0;
int integral = 0;
int previous_error = 0;
timeval prev_time;
// Initialize prev_time with the current time
gettimeofday(&prev_time, nullptr);
set_speed(SPEED_DEF);
for (int i = 0; true; i++)
{
int error = sample_image(line);
// Check if we have reached the end of the curvy maze
if (!line.north && line.east && line.south && line.west)
{
printf("ATTEMPT stop\n");
break;
}
// Try reverse if line is lost
if (line.white_count < STOP_COUNT)
{
if (line.east)
{
printf("ATTEMPT save right\n");
set_speed(0);
turn(75);
while (!line.north)
{
sample_image(line);
}
}
else if (line.west)
{
printf("ATTEMPT save left\n");
set_speed(0);
turn(-75);
while (!line.north)
{
sample_image(line);
}
}
else
{
printf("ATTEMPT reverse %i : %i\n", line.white_count, STOP_COUNT);
set_speed(-SPEED_DEF);
reset_turn();
//turn(sign(previous_error) * SPEED_DEF * 1);
move();
}
// We need to get time here as to not mess with the next
// iteration's derivative
gettimeofday(&prev_time, nullptr);
Sleep(0, REVERSE_DELAY);
i--;
continue;
}
else
{
set_speed(SPEED_DEF);
}
// Calculate how much to turn by
int pixels_x = IMAGE_SIZE_X / SAMPLE_STEPS;
int pixels_y = IMAGE_SIZE_Y / SAMPLE_STEPS;
// Doing Proportional
int proportional_error = error / (pow(pixels_x, SCALE_POW) * pixels_y);
// Doing Integral
int proportional_integral = i == 0 ? 0 : integral / i;
// Doing Derivative
// We need the time in seconds, but not rounded to a second as we'll
// never spend that long and a time rounded to 0 is worthless. To
// fix this we get the time in microseconds and add that to the seconds
double d_time = get_seconds(prev_time);
int derivative = (proportional_error - previous_error) / d_time;
// Now to compile all the calculations
// Start with the proportional component;
movement = proportional_error * K_P;
// Add the integral component
movement += proportional_integral * K_I;
// Add the derivative component
movement += derivative * K_D;
// Update integral component
integral += proportional_error;
// Store previous error for the derivative
previous_error = proportional_error;
// Turn
turn(movement);
}
halt();
}
void tduel::pre_show(CVideo& /*video*/, twindow& window)
{
// Override the user value, to make sure it's set properly.
window.set_click_dismiss(false);
// ***** ***** ***** ***** Set up the widgets ***** ***** ***** *****
window.canvas(1).set_variable("left_image", variant(left_.image(true)));
window.canvas(1).set_variable("right_image", variant(right_.image(true)));
window.canvas(1).set_variable("center_image", variant(""));
window.canvas(1).set_variable("hp_left", variant("misc/hp-blue.png"));
window.canvas(1).set_variable("hp_right", variant("misc/hp-red.png"));
window.canvas(1).set_variable("percentage", variant((hp_ * 100) / TOTAL_HP));
std::stringstream str;
tbutton* b = find_widget<tbutton>(&window, "endturn", false, false);
for (int i = 0; i < 4; i ++) {
b->canvas()[i].set_variable("image", variant("misc/ok.png"));
}
find_widget<tcontrol>(&window, "lname", false, false)->set_label(left_.name());
find_widget<tcontrol>(&window, "rname", false, false)->set_label(right_.name());
tcontrol* control = find_widget<tcontrol>(&window, "lforce", false, false);
str.str("");
str << setting_[0].force_ << "(" << hero::adaptability_str2(left_.skill_[hero_skill_hero]).c_str() << ")";
control->set_label(str.str());
control = find_widget<tcontrol>(&window, "rforce", false, false);
str.str("");
str << setting_[1].force_ << "(" << hero::adaptability_str2(right_.skill_[hero_skill_hero]).c_str() << ")";;
control->set_label(str.str());
endturn_ = find_widget<tbutton>(&window, "endturn", false, false);
connect_signal_mouse_left_click(
*endturn_
, boost::bind(
&tduel::end_turn
, this
, boost::ref(window)));
endturn_->set_sound_button_click("hatchet.wav");
lskill0_ = find_widget<ttoggle_button>(&window, "lskill0", false, false);
connect_signal_mouse_left_click(
*lskill0_
, boost::bind(
&tduel::on_deadfight
, this
, boost::ref(window)));
lskill1_ = find_widget<ttoggle_button>(&window, "lskill1", false, false);
connect_signal_mouse_left_click(
*lskill1_
, boost::bind(
&tduel::on_fightback
, this
, boost::ref(window)));
find_widget<ttoggle_button>(&window, "lturn0", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn0_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<tlabel>(&window, "turn0", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn0", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn0_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn1", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn1_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<tlabel>(&window, "turn1", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn1", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn1_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn2", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn2_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<tlabel>(&window, "turn2", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn2", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn2_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn3", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn3_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<tlabel>(&window, "turn3", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn3", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn3_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn4", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "lturn4_skill", false, false)->set_visible(twidget::INVISIBLE);
find_widget<tlabel>(&window, "turn4", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn4", false, false)->set_visible(twidget::INVISIBLE);
find_widget<ttoggle_button>(&window, "rturn4_skill", false, false)->set_visible(twidget::INVISIBLE);
reset_turn(window);
}
void follow_square_line()
{
LineInfo line;
int movement = 0;
int previous_error = 0;
timeval prev_time;
// Initialize prev_time with the current time
gettimeofday(&prev_time, nullptr);
set_speed(SPEED_DEF);
while (true)
{
int error = sample_image(line);
// Check left
if (line.south && line.west)
{
printf("ATTEMPT Going square left");
printf(line.north ? " : line N" : " : line ");
printf(line.east ? "E" : " ");
printf(line.south ? "S" : " ");
printf(line.west ? "W\n" : "\n");
Sleep(0, REVERSE_DELAY);
set_speed(0);
turn(-TURN_90_SPEED);
//Sleep(0, TURN_90_DELAY);
square_line_rotate(line.north);
set_speed(SPEED_DEF);
turn(0);
Sleep(0, TURN_90_DELAY);
}
// Check right
else if (!line.north && line.east && line.south && !line.west)
{
printf("ATTEMPT Going square right");
printf(line.north ? " : line N" : " : line ");
printf(line.east ? "E" : " ");
printf(line.south ? "S" : " ");
printf(line.west ? "W\n" : "\n");
Sleep(0, REVERSE_DELAY);
set_speed(0);
turn(TURN_90_SPEED);
// Sleep(0, TURN_90_DELAY);
square_line_rotate(line.north);
set_speed(SPEED_DEF);
turn(0);
Sleep(0, TURN_90_DELAY);
}
// Check center line stop
else if (!line.north && !line.east && line.south && !line.west)
{
printf("ATTEMPT Dead end! Do a barrel roll!");
printf(line.north ? " : line N" : " : line ");
printf(line.east ? "E" : " ");
printf(line.south ? "S" : " ");
printf(line.west ? "W\n" : "\n");
set_speed(0);
turn(2*TURN_90_SPEED);
// Sleep(0, TURN_90_DELAY * 2);
square_line_rotate(false);
set_speed(SPEED_DEF);
turn(0);
}
// Try reverse if line is lost
else if (line.white_count < STOP_COUNT)
{
printf("ATTEMPT reversing\n");
set_speed(-SPEED_DEF);
reset_turn();
//turn(sign(previous_error) * SPEED_DEF * 1);
move();
// We need to get time here as to not mess with the next
// iteration's derivative
gettimeofday(&prev_time, nullptr);
Sleep(0, REVERSE_DELAY / 2);
continue;
}
else {
// This is here to make the robot go forward after reversing
set_speed(SPEED_DEF);
}
// Calculate how much to turn by
int pixels_x = IMAGE_SIZE_X / SAMPLE_STEPS;
int pixels_y = IMAGE_SIZE_Y / SAMPLE_STEPS;
// Doing Proportional
int proportional_error = error / (pow(pixels_x, SCALE_POW) * pixels_y);
// No integral here; with sharp corners it is more likely to throw us
// off than actually help
// Doing Derivative
// We need the time in seconds, but not rounded to a second as we'll
// never spend that long and a time rounded to 0 is worthless. To
// fix this we get the time in microseconds and add that to the seconds
double d_time = get_seconds(prev_time);
int derivative = (proportional_error - previous_error) / d_time;
// Now to compile all the calculations
// Start with the proportional component;
movement = proportional_error * K_P;
// Add the derivative component
movement += derivative * K_D;
// Store previous error for the derivative
previous_error = proportional_error;
// Turn
turn(movement);
}
halt();
}
