inline Iterator next_point(Range const& range, Iterator it) const { Iterator result = it; move_to_next_point(range, result); // TODO: we could use either piece-boundaries, or comparison with // robust points, to check if the point equals the last one while(geometry::equals(*it, *result)) { move_to_next_point(range, result); } return result; }
void calcul_mission() { struct coordinates_ C_blue; //coordinates of drone struct coordinates_ nextPoint; struct coordinates_ startPoint; mission_state_t state; int indice = 0; int index = 0; int bad_move = 0; float angle_actuel, calcul_x, calcul_y, angle_desire; node_t **path = NULL; C_blue.x = 0.0; C_blue.y = 0.0; // First of all, check if the given destination is valid if(find_point(graph, destination.x, destination.y) == -1) { printf("Invalid destination: (%.2f, %.2f)\n", destination.x, destination.y); state = FINISHED; } else state = INIT; while(state != FINISHED) { if(inC.flag_control_s == STATE_MANUAL) { state = FINISHED; } switch(state) { case INIT: printf("[Mission state: INIT]\n"); // Wait for the data to stabilize sleep(5); read_data_bluetooth(&C_blue.x,&C_blue.y); printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y); printf("Destination point: (%f, %f)\n", destination.x, destination.y); path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph); if (path == NULL) state = LOST; else { printf("Path to follow\n"); int i; for(i = 0 ; path[i] != NULL ; i++) { indice = i; printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y); } state = RUNNING; } break; case LOST: printf("[Mission state: LOST]\n"); sleep(3); read_data_bluetooth(&C_blue.x,&C_blue.y); index = find_closest_node(graph, C_blue.x, C_blue.y); startPoint.x = graph->nodes[index].x; startPoint.y = graph->nodes[index].y; Main_Nav = return_navdata(); angle_actuel = Main_Nav.magneto.heading_fusion_unwrapped; angle_desire = computeDesiredAngle(C_blue, startPoint); computeOffsetMag(&angle_desire, nav_prec, nav_suiv); yaw_power = computeDirection(angle_actuel, angle_desire, 0.3, &yaw_move); if(rotate_to_desired_angle(angle_desire) == -1) { fprintf(stderr, "[%s:%d] Error: Rotation to desired angle failed\n", __FILE__, __LINE__); break_drone(); stop_mission(); free(graph); return; } // Let the drone stabilize itself sleep(1); bad_move = move_to(startPoint, &C_blue); break_drone(); // If we find a start point, we can switch to state RUNNING if(!bad_move) { printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y); printf("Destination point: (%f, %f)\n", destination.x, destination.y); path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph); if(path == NULL) { fprintf(stderr, "[%s:%d] Error: Bluetooth Data too unstable, aborting\n", __FILE__, __LINE__); break_drone(); stop_mission(); free(graph); return; } printf("Path to follow\n"); int i; for(i = 0 ; path[i] != NULL ; i++) { indice = i; printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y); } state = RUNNING; } break; case DRIFTED: printf("[Mission state: DRIFTED]\n"); // Pretty much the same that LOST, at the difference that we think that we know where we are sleep(3); read_data_bluetooth(&C_blue.x,&C_blue.y); printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y); printf("Destination point: (%f, %f)\n", destination.x, destination.y); path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph); if (path == NULL) { // Mmh, Bluetooth data unstable, going to LOST state = LOST; } else { printf("New path to follow\n"); int i; for(i = 0 ; path[i] != NULL ; i++) { indice = i; printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y); } state = RUNNING; } break; case RUNNING: printf("[Mission state: RUNNING]\n"); sleep(3); read_data_bluetooth(&C_blue.x,&C_blue.y); printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y); printf("Destination point: (%f, %f)\n", destination.x, destination.y); // Let's start indice--; if (indice <= 0) { state = ARRIVED; } else { // FIXME: Cast path[indice] from node_t to coordinates_ since we don't use the samee structure... nextPoint.x = path[indice]->x; nextPoint.y = path[indice]->y; Main_Nav = return_navdata(); angle_actuel = Main_Nav.magneto.heading_fusion_unwrapped; angle_desire = computeDesiredAngle(C_blue, nextPoint); /* * angle désiré */ //calcul the new angle_desire computeOffsetMag(&angle_desire, nav_prec, nav_suiv); //printf("Calcul angle desire final v1 : %2.f\n", angle_desire); //calculating the direction of rotation of the Z-axis for optimum positioning. yaw_power = computeDirection(angle_actuel, angle_desire, 0.3, &yaw_move); //printf("Valeur de la puissance mise : %1.f, Valeur de l'angle souhaité = %2.f, valeur de l'angle actuel = %2.f sens de rotation = %d\n", yaw_power, angle_desire, angle_actuel, yaw_move); if (rotate_to_desired_angle(angle_desire) == -1) { break_drone(); stop_mission(); free(graph); return; } // Let the drone stabilize itself sleep(1); // Let's move to next point bad_move = move_to_next_point(nextPoint, (const node_t **) path, &C_blue); // Stop the drone movement break_drone(); if(bad_move == 1) state = LOST; else if(bad_move == 2) state = DRIFTED; else printf("One step made\n"); } break; case ARRIVED: printf("[Mission state: ARRIVED]\n"); printf("You're arrived to destination\n"); state = FINISHED; break; case FINISHED: // if stop mission called break; default: fprintf(stderr, "[%s:%d] Error: Unknown mission state\n", __FILE__, __LINE__); break_drone(); stop_mission(); free(graph); return; break; } } printf("[Mission state: FINISHED]\n"); stop_mission(); printf("fin mission\n"); free(path); }