static COMMAND_FUNC( do_samp_image )
{
Data_Obj *intens_dp, *image_dp, *coord_dp;
intens_dp=pick_obj( "target list for sampled intensities" );
image_dp=pick_obj( "source image for intensities" );
coord_dp=pick_obj( "coordinate list" );
if( intens_dp==NULL || image_dp==NULL || coord_dp==NULL )
return;
sample_image(QSP_ARG intens_dp,image_dp,coord_dp);
}
inline void square_line_rotate(bool start_on_line)
{
int stage = start_on_line ? 0 : 1;
int direction = 0;
LineInfo line;
while (true)
{
sample_image(line);
int current_dir = sign(line.north_error);
if (stage == 0 && !line.north)
{
stage = 1;
}
else if (stage == 1 && line.north)
{
stage = 2;
direction = current_dir;
}
else if (stage == 2 && direction != current_dir)
{
break;
}
}
}
/**
* 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 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();
}
