void passive_red_line_correct() {
// only correct to red line if not backing up
if (abs(y - RENDEZVOUS_Y) < GRID_WIDTH*0.5 && layers[active_layer].speed > 0) {
digitalWrite(bottom_led, HIGH);
if (on_line(CENTER)) cycles_on_red_line = 0;
else if (on_line(RED) && !on_line(CENTER)) {
++cycles_on_red_line;
// navigate trying to get back to red line and x is far enough forward
if (seeking_red_line && RENDEZVOUS_X - x < 3*RENDEZVOUS_CLOSE) {
waypoint(LAYER_NAV);
}
}
else if (!on_line(RED)) {
// not false alarm
if (cycles_on_red_line >= CYCLES_CROSSING_LINE && current_distance() - last_correct_distance > DISTANCE_CENTER_TO_RED_ALLOWANCE) {
SERIAL_PRINT("RC");
SERIAL_PRINTLN(current_distance() - last_correct_distance);
// direction and signs are taken care of by sin and cos
float offset_y = DISTANCE_CENTER_TO_RED * sin(theta);
// avoid correcting when parallel to horizontal line
int offset_x = abs((int)x) % GRID_WIDTH;
if (abs(offset_y) > NEED_TO_HOPPER_CORRECT && (offset_x < INTERSECTION_TOO_CLOSE*0.5 || offset_x > GRID_WIDTH - INTERSECTION_TOO_CLOSE*0.5) && parallel_to_horizontal()) {
SERIAL_PRINTLN("-RC");
cycles_on_red_line = 0;
last_correct_distance = current_distance();
return;
}
y = RENDEZVOUS_Y;
// between [-180,0] left while going left
// x += DISTANCE_CENTER_TO_RED * cos(theta);
if (theta < 0) offset_y -= HALF_LINE_WIDTH;
else offset_y += HALF_LINE_WIDTH;
y += offset_y;
last_red_line_distance = current_distance();
last_correct_distance = last_red_line_distance;
if (side_of_board == SIDE_RIGHT) side_of_board = SIDE_LEFT;
else if (side_of_board == SIDE_LEFT) side_of_board = SIDE_RIGHT;
}
cycles_on_red_line = 0;
}
}
else {
digitalWrite(bottom_led, LOW);
cycles_on_red_line = 0;
}
}
void passive_position_correct() {
if (on_line(CENTER)) {
++cycles_on_line;
last_correct_distance = current_distance();
// activate line following if close enough to target and is on a line
}
else {
// false positive, not on line for enough cycles
if (cycles_on_line < CYCLES_CROSSING_LINE && cycles_on_line >= 0) { cycles_on_line = 0; return; }
else if (counted_lines >= LINES_PER_CORRECT && far_from_intersection(x, y)) {
counted_lines = 0;
// correct whichever one is closer to 0 or 200
// account for red line, can't detect along y so just correct to x
if (abs(y - RENDEZVOUS_Y) < 0.5*GRID_WIDTH) {
x = round(x / GRID_WIDTH) * GRID_WIDTH;
// leaving line forward
if (theta > -HALFPI && theta < HALFPI) x += HALF_LINE_WIDTH;
else x -= HALF_LINE_WIDTH;
}
else {
correct_to_grid();
}
SERIAL_PRINTLN('C');
}
else {
// false alarm, cool down
++counted_lines;
SERIAL_PRINTLN('L');
passive_status = PASSED_COOL_DOWN;
}
// either C or L will have last correct distance updated
// last_correct_distance = current_distance();
cycles_on_line = 0;
}
}
void FacenetClassifier::predict_knn_labels () {
Mat current_class(0, 0, CV_32F);
Mat current_response(0, 0, CV_32F);
Mat current_distance (0, 0, CV_32F);
float current_class_float, response, distance;
float prediction;
int j;
cout << "Input Images: " << input_images.size() << endl;
cout << "mat_training_tensors: " << mat_training_tensors.size () << endl;
cout << "Input Files: " << input_files.size () << endl;
for (int i = 0; i < input_images.size(); i++) {
Mat input_mat, input_mat_flattened;
output_mat.row(i).convertTo (input_mat, CV_32FC3);
input_mat_flattened = input_mat.reshape (0,1);
prediction = k_nearest->findNearest (input_mat_flattened, K_VALUE, current_class, current_response, current_distance);
current_class_float = (float) current_class.at<float>(0,0);
response = (float) current_response.at<float>(0,0);
distance = (float) current_distance.at<float>(0,0);
cout << "Output Index: " << i << endl;
cout << mat_training_tensors.row(i).size() << " " << prediction << " " << input_files[i] << ": " << current_class_float << " " << distance << endl;
}
}
// correct passing a line not too close to an intersection
void passive_correct() {
// still on cool down
if (passive_status < PASSED_NONE) {++passive_status; return;}
if (!far_from_intersection(x, y)) {
if (passive_status > PASSED_NONE) passive_status = PASSED_NONE;
return;
}
// SERIAL_PRINTLN(passive_status, BIN);
// check if center one first activated; halfway there
if (on_line(CENTER) && !(passive_status & CENTER)) {
correct_half_distance = current_distance();
SERIAL_PRINTLN("PH");
}
// activate passive correct when either left or right sensor FIRST ACTIVATES
if ((on_line(LEFT) || on_line(RIGHT)) && passive_status == PASSED_NONE) {
correct_initial_distance = current_distance();
// only look at RIGHT LEFT CENTER
passive_status |= (on_lines & ENCOUNTERED_ALL);
SERIAL_PRINT("PS");
SERIAL_PRINTLN(passive_status, BIN);
// all hit at the same time, don't know heading
if (passive_status == ENCOUNTERED_ALL) hit_first = CENTER;
else if (passive_status & LEFT) hit_first = LEFT;
else hit_first = RIGHT;
// return;
}
if ((passive_status & LEFT) && !on_line(LEFT)) passive_status |= PASSED_LEFT;
if ((passive_status & RIGHT) && !on_line(RIGHT)) passive_status |= PASSED_RIGHT;
// check if encountering any additional lines
passive_status |= (on_lines & ENCOUNTERED_ALL);
// distance from first to center too far, probably too parallel to line
if (passive_status > PASSED_NONE && !(passive_status & CENTER) && (current_distance() - correct_initial_distance > CORRECT_TOO_FAR)) {
passive_status = PASSED_COOL_DOWN;
SERIAL_PRINT("PP");
SERIAL_PRINTLN(current_distance() - correct_initial_distance);
return;
}
// already hit center, see if second half distance is too far from first half distance
else if ((passive_status & CENTER) &&
((current_distance() - correct_half_distance) > // second half distance
(correct_half_distance - correct_initial_distance + CORRECT_CROSSING_TOLERANCE))) { // first half distance plus some room for error
SERIAL_PRINT("PD");
SERIAL_PRINT(correct_half_distance - correct_initial_distance);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(current_distance() - correct_half_distance);
passive_status = PASSED_COOL_DOWN;
}
// correct at the first encounter of line for each sensor
if ((passive_status & ENCOUNTERED_ALL) == ENCOUNTERED_ALL) {
// correct only if the 2 half distances are about the same
if (abs((current_distance() - correct_half_distance) - (correct_half_distance - correct_initial_distance)) < CORRECT_CROSSING_TOLERANCE) {
// distance since when passive correct was activated
float correct_elapsed_distance = current_distance() - correct_initial_distance;
// always positive
float theta_offset = atan2(correct_elapsed_distance, SIDE_SENSOR_DISTANCE);
// reverse theta correction if direction is backwards
if (layers[get_active_layer()].speed < 0) theta_offset = -theta_offset;
float theta_candidate;
// assume whichever one passed first was the first to hit
if (passive_status & PASSED_LEFT) theta_candidate = (square_heading()*DEGS) + theta_offset;
else if (passive_status & PASSED_RIGHT) theta_candidate = (square_heading()*DEGS) - theta_offset;
else if (hit_first == LEFT) theta_candidate = (square_heading()*DEGS) + theta_offset;
else if (hit_first == RIGHT) theta_candidate = (square_heading()*DEGS) - theta_offset;
// hit at the same time?
else theta_candidate = (square_heading()*DEGS);
// check how far away correction is from current theta
if (abs(theta - theta_candidate) > THETA_CORRECT_LIMIT) {
SERIAL_PRINT("PO");
SERIAL_PRINTLN(theta_candidate*RADS);
passive_status = PASSED_COOL_DOWN;
return;
}
// else correct to candidate value
theta = theta_candidate;
SERIAL_PRINT('P');
SERIAL_PRINTLN(theta_offset*RADS);
}
// suspicious of an intersection
else {
SERIAL_PRINT("PI");
SERIAL_PRINT(correct_half_distance - correct_initial_distance);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(current_distance() - correct_half_distance);
}
// reset even if not activated on this line (false alarm)
passive_status = PASSED_COOL_DOWN;
}
}
