// given an obstacle, calculate adecuate speed based on how close we are to any of the edges
float Navigate_find_optimal_speed(Polygon* obstacle) {
printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_speed start: %lu\n",
timestamp(), CURRENT_MEMORY_USAGE);
float goal_speed = 0;
Point* current_position = Point_new(
Vector_get(ROBOT_STATUS, 0), Vector_get(ROBOT_STATUS, 1));
float distance_min = Geometry_distance_between_point_and_polygon(obstacle, current_position);
// when the distance to obstacle is reasonably big floor the gas pedal
if (distance_min > 2 * DISTANCE_MIN) {
goal_speed = 1;
// when the distance is too small and we need to stop
} else if (distance_min < DISTANCE_MIN) {
goal_speed = SPEED_MIN;
// when distance to obstacle requires a maneuver
} else {
goal_speed = ((distance_min * SPEED_MIN)
/ (DISTANCE_MIN - DISTANCE_CRITICAL)
- (SPEED_MIN * DISTANCE_CRITICAL)
/ (DISTANCE_MIN - DISTANCE_CRITICAL));
}
// cleanup and return
Point_destroy(current_position);
printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_speed end: %lu\n",
timestamp(), CURRENT_MEMORY_USAGE);
return goal_speed;
}
// given a desired end angle, current robot status and an obstacle, return the closest possible angle
// to the desired one that also avoids having the obstacle in its trayectory. This function will break
// the problem down into cases and then call the corresponding functions for each case
float Navigate_find_optimal_angle(float goal_angle, Polygon* obstacle) {
float optimal_angle = Vector_get(ROBOT_STATUS, 2);
printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_angle start: %lu\n",
timestamp(), CURRENT_MEMORY_USAGE);
// find current position
Point* current_position = Point_new(
Vector_get(ROBOT_STATUS, 0), Vector_get(ROBOT_STATUS, 1));
// find closest edge and distance to it
Edge* closest_edge = Geometry_closest_polygon_edge_to_point(obstacle, current_position);
float distance_min = Geometry_distance_between_point_and_edge(closest_edge, current_position);
// case 1: when there are no obstacles nearby so we can turn to desired angle
if (distance_min >= 2 * DISTANCE_MIN) {
printf("%s[NAVIGATION] Navigation strategy case 1: No obstacles in sight\n", timestamp());
optimal_angle = Navigate_find_optimal_angle_case_1(
goal_angle, obstacle, current_position, closest_edge);
// case 2: when there is an obstacle nearby so we need to keep our distance
} else if (distance_min > DISTANCE_MIN && distance_min < 2 * DISTANCE_MIN) {
printf("%s[NAVIGATION] Navigation strategy case 2: Obstacles nearby\n", timestamp());
optimal_angle = Navigate_find_optimal_angle_case_2(
goal_angle, obstacle, current_position, closest_edge);
// case 3: when there is an obstacle too close so we need to move away
} else if (distance_min <= DISTANCE_MIN) {
printf("%s[NAVIGATION][WARNING] Navigation strategy case 3: Obstacles too close\n", timestamp());
optimal_angle = Navigate_find_optimal_angle_case_3(
Vector_get(ROBOT_STATUS, 2), obstacle, current_position, closest_edge);
}
// make sure the chosen angle passes the min turn and the max turn check
// float delta_angle = fabs(Vector_get(SIMULATION_ROBOT_STATUS, 2) - optimal_angle);
// if (delta_angle > SERVO_MAX_TURN) {
// optimal_angle =
// Vector_get(SIMULATION_ROBOT_STATUS, 2) + (delta_angle < 0 ? -SERVO_MAX_TURN : SERVO_MAX_TURN);
// }
// cleanup
Point_destroy(current_position);
// output and return
if (optimal_angle == FLT_MAX) {
printf("%s[NAVIGATION][ERROR] No optimal angle found\n", timestamp());
// TODO: dump info here
} else {
printf("%s[NAVIGATION] Distance to closest obstacle: %f, optimal angle: %f\n",
timestamp(), distance_min, optimal_angle);
}
printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_angle end: %lu\n",
timestamp(), CURRENT_MEMORY_USAGE);
return optimal_angle;
}
/*
* display all the entrants based on status/duration
*/
void display_results(Event* event) {
Entrant* entrant;
int i = 0;
entrants_sort(event);
/* since the entrants are now sorted, we know that all entrants with a particular
* status will be grouped together */
/* display finished */
if (count_by_status(event, FINISHED) > 0) {
printf("\n\tFinished:\n");
for (; i < Vector_size(event->entrants); i++) {
Vector_get(event->entrants, i, &entrant);
if (entrant->status != FINISHED) break; /* if entrant hasn't finished, skip to
the next block of entrants */
printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
printf("\t\t\tCourse: %c Total time: %3d mins\n", entrant->course->id, entrant->duration);
}
}
/* display started and stopped */
if (count_by_status(event, STARTED) + count_by_status(event, STOPPED) > 0) {
printf("\n\tRunning:\n");
for (; i < Vector_size(event->entrants); i++) {
Vector_get(event->entrants, i, &entrant);
if (entrant->status != STARTED && entrant->status != STOPPED) break;
printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
printf("\t\t\tCourse: %c Track: %2d Run time: %3d mins\n", entrant->course->id,
entrant->curr_track->id, entrant->duration);
}
}
/* display disqualified */
if (count_by_status(event, DISQUAL_SAFETY) + count_by_status(event, DISQUAL_INCORR) > 0) {
printf("\n\tDisqualified:\n");
for (; i < Vector_size(event->entrants); i++) {
Vector_get(event->entrants, i, &entrant);
if (entrant->status != DISQUAL_SAFETY && entrant->status != DISQUAL_INCORR) break;
printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
printf("\t\t\tCourse: %c ", entrant->course->id);
if (entrant->status == DISQUAL_SAFETY) printf("Disqualified for safety\n");
else printf("Disqualified for incorrect route\n");
}
}
/* display waiting to start */
if (count_by_status(event, NOT_STARTED) > 0) {
printf("\n\tWaiting to start:\n");
for (; i < Vector_size(event->entrants); i++) {
Vector_get(event->entrants, i, &entrant);
printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
printf("\t\t\tCourse: %c\n", entrant->course->id);
}
}
}
static HandlerResult ColorsPanel_eventHandler(Panel* super, int ch) {
ColorsPanel* this = (ColorsPanel*) super;
HandlerResult result = IGNORED;
int mark = Panel_getSelectedIndex(super);
switch(ch) {
case 0x0a:
case 0x0d:
case KEY_ENTER:
case KEY_MOUSE:
case KEY_RECLICK:
case ' ':
for (int i = 0; ColorSchemeNames[i] != NULL; i++)
CheckItem_set((CheckItem*)Panel_get(super, i), false);
CheckItem_set((CheckItem*)Panel_get(super, mark), true);
this->settings->colorScheme = mark;
result = HANDLED;
}
if (result == HANDLED) {
this->settings->changed = true;
const Header* header = this->scr->header;
CRT_setColors(mark);
clear();
Panel* menu = (Panel*) Vector_get(this->scr->panels, 0);
Header_draw(header);
RichString_setAttr(&(super->header), CRT_colors[PANEL_HEADER_FOCUS]);
RichString_setAttr(&(menu->header), CRT_colors[PANEL_HEADER_UNFOCUS]);
ScreenManager_resize(this->scr, this->scr->x1, header->height, this->scr->x2, this->scr->y2);
}
return result;
}
void
test_map_pairs (void* data)
{
Map* map = Map_new(runtime);
Map_put(map, hash_for(NIL), NIL, TRUE);
Map_put(map, hash_for(TRUE), TRUE, FALSE);
Map_put(map, hash_for(FALSE), FALSE, NIL);
Vector* pairs = Map_pairs(map);
for (uint64_t i = 0; i < Vector_length(pairs); i++) {
Tuple* pair = Vector_get(pairs, i);
if (is_nil(Tuple_get(pair, 0))) {
tt_assert(is_true(Tuple_get(pair, 1)));
}
else if (is_true(Tuple_get(pair, 0))) {
tt_assert(is_false(Tuple_get(pair, 1)));
}
else if (is_false(Tuple_get(pair, 0))) {
tt_assert(is_nil(Tuple_get(pair, 1)));
}
}
end:
Map_destroy(map);
}
static HandlerResult MetersPanel_eventHandler(Panel* super, int ch) {
MetersPanel* this = (MetersPanel*) super;
int selected = Panel_getSelectedIndex(super);
HandlerResult result = IGNORED;
switch(ch) {
case 0x0a:
case 0x0d:
case KEY_ENTER:
case KEY_F(4):
case 't':
{
Meter* meter = (Meter*) Vector_get(this->meters, selected);
int mode = meter->mode + 1;
if (mode == LAST_METERMODE) mode = 1;
Meter_setMode(meter, mode);
Panel_set(super, selected, (Object*) Meter_toListItem(meter));
result = HANDLED;
break;
}
case KEY_F(7):
case '[':
case '-':
{
Vector_moveUp(this->meters, selected);
Panel_moveSelectedUp(super);
result = HANDLED;
break;
}
case KEY_F(8):
case ']':
case '+':
{
Vector_moveDown(this->meters, selected);
Panel_moveSelectedDown(super);
result = HANDLED;
break;
}
case KEY_F(9):
case KEY_DC:
{
if (selected < Vector_size(this->meters)) {
Vector_remove(this->meters, selected);
Panel_remove(super, selected);
}
result = HANDLED;
break;
}
}
if (result == HANDLED) {
Header* header = this->settings->header;
this->settings->changed = true;
Header_calculateHeight(header);
Header_draw(header);
ScreenManager_resize(this->scr, this->scr->x1, header->height, this->scr->x2, this->scr->y2);
}
return result;
}
void Navigate_test_sensor_data_parse_1() {
Polygon* obstacle = NULL;
float distance = 200.0f, angle = 0, x, y;
char test_name[100], label[20], i;
// using binary for combinatorics with k = 2
unsigned char sensor_switch;
for (sensor_switch = 0; sensor_switch < 32; sensor_switch++) {
sprintf(label, "[ %s, %s, %s, %s, %s ]",
(sensor_switch & 0b0000001) ? "1" : "0",
(sensor_switch & 0b0000010) ? "1" : "0",
(sensor_switch & 0b0000100) ? "1" : "0",
(sensor_switch & 0b0001000) ? "1" : "0",
(sensor_switch & 0b0010000) ? "1" : "0");
sprintf(test_name, "Navigate_test_sensor_data_parse_%s", label);
Draw* TEST_CANVAS = Draw_new(test_name, NULL, WORLD_MAX_X, WORLD_MAX_Y);
TEST_CANVAS->style = "stroke:red;fill:red;stroke-width:2";
for (i = 0; i < SENSOR_ORIENTATION->n; i++) {
if (sensor_switch & (1 << i)) {
angle = Vector_get(SENSOR_ORIENTATION, i);
x = distance * cosf((90 - angle) * M_PI / 180.0f);
y = distance * sinf((90 - angle) * M_PI / 180.0f);
Draw_line(TEST_CANVAS, 0, 0, x, y);
Draw_circle(TEST_CANVAS, x, y , 3);
}
}
int sensor_data[5] = {
(sensor_switch & 0b0000001) ? distance : NAVIGATION_DISTANCE_SAFE,
(sensor_switch & 0b0000010) ? distance : NAVIGATION_DISTANCE_SAFE,
(sensor_switch & 0b0000100) ? distance : NAVIGATION_DISTANCE_SAFE,
(sensor_switch & 0b0001000) ? distance : NAVIGATION_DISTANCE_SAFE,
(sensor_switch & 0b0010000) ? distance : NAVIGATION_DISTANCE_SAFE };
char sensor_data_raw[32];
sprintf(sensor_data_raw, "[%d,%d,%d,%d,%d]",
sensor_data[0],
sensor_data[1],
sensor_data[2],
sensor_data[3],
sensor_data[4]);
obstacle = Navigate_parse_sensor_data(sensor_data_raw);
if (obstacle != NULL) {
TEST_CANVAS->style = "stroke:blue;fill:none;stroke-width:2";
Polygon_draw(obstacle);
Polygon_destroy(obstacle);
}
TEST_CANVAS->style = "stroke:black;fill:black;stroke-width:2";
Draw_text(TEST_CANVAS, -125, 450, 40, label);
Draw_destroy(TEST_CANVAS);
}
}
bool compile(AstNode *ast, FILE *outfp)
{
if (ast == NULL || ast->type != AST_ROOT) {
trigger_error(0, "Invalid AstNode found. Stop.\n");
}
SNodeArray *sna = SNodeArray_create();
compile_declaration_list(ast->children[0], sna);
compile_stmt_list(ast->children[1], sna);
for (size_t i = 0; i < sna->size; i++) {
SNode *node = (SNode *) Vector_get(sna, i);
if (node->kind == SNODE_VAR) {
fprintf(outfp, "load %d\n", node->sym->location);
} else if (node->kind == SNODE_TMP) {
} else if (node->kind == SNODE_LOCATION) {
fprintf(outfp, "L%d:\n", node->location);
} else if (node->kind == SNODE_CONST) {
fprintf(outfp, "push %d\n", node->sval.ival);
} else {
switch (node->op) {
case SNODE_ASSIGN: fprintf(outfp, "store %d\n", node->result->sym->location); break;
case SNODE_IADD: fprintf(outfp, "add\n"); break;
case SNODE_ISUB: fprintf(outfp, "sub\n"); break;
case SNODE_IMUL: fprintf(outfp, "mul\n"); break;
case SNODE_IDIV: fprintf(outfp, "div\n"); break;
case SNODE_IMOD: fprintf(outfp, "mod\n"); break;
case SNODE_IPRINT: fprintf(outfp, "print\n"); break;
case SNODE_AND: fprintf(outfp, "and\n"); break;
case SNODE_OR: fprintf(outfp, "or\n"); break;
case SNODE_LT: fprintf(outfp, "cmplt\n"); break;
case SNODE_GT: fprintf(outfp, "cmpgt\n"); break;
case SNODE_EQ: fprintf(outfp, "cmpeq\n"); break;
case SNODE_NOT: fprintf(outfp, "not\n"); break;
case SNODE_NOTEQ: fprintf(outfp, "cmpeq\n"); fprintf(outfp, "not\n"); break;
case SNODE_JMP_FALSE:
fprintf(outfp, "jmpz L%d\n", node->arg->location);
break;
case SNODE_GOTO:
fprintf(outfp, "goto L%d\n", node->arg->location);
break;
default:
break;
}
}
}
fprintf(outfp, "halt\n");
SNodeArray_destroy(sna);
return true;
}
int
Stack_pop(Stack s, void *value)
{
int ret = Vector_get(&s->vector, Vector_length(&s->vector) - 1, value);
/* Only truncate the vector (shrink by 1) if getting the value succeeded. */
if (!ret)
ret = Vector_truncate(&s->vector, -1);
return ret;
}
/*
* returns how many entrants have a particular status
*/
int count_by_status(Event* event, entrant_status status) {
Entrant* entrant;
int ret_val = 0;
int i = 0;
for (i = 0; i < Vector_size(event->entrants); i++) {
Vector_get(event->entrants, i, &entrant);
if (entrant->status == status) ret_val++;
}
return ret_val;
}
void Navigate_update_robot_status(int speed, int direction) {
float adjusted_speed = speed * 0.3;
// get coordinates of current position and next immediate position given current speed
Point* p0 = Point_new(Vector_get(ROBOT_STATUS, 0), Vector_get(ROBOT_STATUS, 1));
Point* p1 = Point_new(Vector_get(ROBOT_STATUS, 0)
+ cosf((90 - Vector_get(ROBOT_STATUS, 2)) * M_PI / 180.0f) * adjusted_speed,
Vector_get(ROBOT_STATUS, 1)
+ sinf((90 - Vector_get(ROBOT_STATUS, 2)) * M_PI / 180.0f) * adjusted_speed);
// get the coordinates of goal position with given goal speed and direction
Point* p2 = Point_new(p1->x + cosf((90 - direction) * M_PI / 180.0f) * adjusted_speed,
p1->y + sinf((90 - direction) * M_PI / 180.0f) * adjusted_speed);
// update global robot status
Vector_set_val(ROBOT_STATUS, 0, p2->x);
Vector_set_val(ROBOT_STATUS, 1, p2->y);
Vector_set_val(ROBOT_STATUS, 2, direction);
Vector_set_val(ROBOT_STATUS, 3, speed);
// TODO: draw it in canvas?
// cleanup
Point_destroy(p0);
Point_destroy(p1);
Point_destroy(p2);
}
MetersPanel* MetersPanel_new(Settings* settings, const char* header, Vector* meters, ScreenManager* scr) {
MetersPanel* this = AllocThis(MetersPanel);
Panel* super = (Panel*) this;
Panel_init(super, 1, 1, 1, 1, Class(ListItem), true);
this->settings = settings;
this->meters = meters;
this->scr = scr;
Panel_setHeader(super, header);
for (int i = 0; i < Vector_size(meters); i++) {
Meter* meter = (Meter*) Vector_get(meters, i);
Panel_add(super, (Object*) Meter_toListItem(meter));
}
return this;
}
/*
* list entrants excluded for getting lost
*/
void list_excluded_incorrect(Event* event) {
Entrant* entrant;
int i = 0;
/* make sure there's at least one */
if (count_by_status(event, DISQUAL_INCORR) == 0) {
printf("\tNo entrants disqualified for incorrect route\n");
} else {
for (i = 0; i < Vector_size(event->entrants); i++) {
Vector_get(event->entrants, i, &entrant);
if (entrant->status == DISQUAL_INCORR) {
printf("\t%2d: %-50s\n", entrant->id, entrant->name);
printf("\t\tCourse: %c Last node: %2d\n", entrant->course->id,
entrant->last_cp_node->id);
}
}
}
}
void
test_map_values (void* data)
{
Map* map = Map_new(runtime);
Map_put(map, hash_for(NIL), NIL, TRUE);
Map_put(map, hash_for(TRUE), TRUE, FALSE);
Map_put(map, hash_for(FALSE), FALSE, NIL);
Vector* values = Map_values(map);
for (uint64_t i = 0; i < Vector_length(values); i++) {
Value* value = Vector_get(values, i);
tt_assert(is_nil(value) || is_true(value) || is_false(value));
}
end:
Map_destroy(map);
}
/*
* update entrants from a file
*/
int refresh(Event* event, char* filename, int t_index) {
Vector* lines = read_file(filename);
char* line;
char* token;
char type;
int node_id;
int entrant_id;
Time* time;
int i;
for (i = t_index; i < Vector_size(lines); i++) {
Vector_get(lines, i, &line);
/* type */
token = strtok(line, " ");
type = token[0];
/* node id */
token = strtok(NULL, " ");
node_id = atoi(token);
/* entrant id */
token = strtok(NULL, " ");
entrant_id = atoi(token);
/* time */
token = strtok(NULL, "\n");
time = str_to_time(token);
update_time(event, time, entrant_id);
entrant_update_location(event, type, entrant_id, node_id);
}
Vector_dispose(lines);
return i;
}
// Test your vector here
int main() {
Vector* temp = Vector_create(); //create a vector
Vector_append( temp, "a");//1
Vector_append( temp, "b");//2
Vector_append( temp, "c");//3
Vector_append( temp, "d");//4
Vector_append( temp, NULL);//5
Vector_append( temp, "f");//6
Vector_append( temp, "g");//7
Vector_append( temp, NULL);//8
Vector_append( temp, "");//9
Vector_append( temp, "h");//10
Vector_append( temp, "i");//11
Vector_append( temp, NULL);//12
Vector_resize( temp, 20 );
if( Vector_size(temp) != 20 || strcmp( Vector_get( temp, 10 ), "i" ) || Vector_get( temp, 15 ) != NULL )
perror( "something wrong.\n");
//done for append, resize, get, size
Vector_set( temp, 19, "caibi" );
Vector_insert( temp, 20, "niubi" );
Vector_insert( temp, 30, "wori" );
if( Vector_size(temp) != 31 || strcmp( Vector_get( temp, 19 ), "caibi" ) || strcmp( Vector_get( temp, 20 ), "niubi" ) || Vector_get( temp, 15 ) != NULL || strcmp( Vector_get( temp, 30 ), "wori" ) )
perror( "something wrong.\n");
Vector_delete( temp, 11 );
Vector_delete( temp, 27 );
Vector_delete( temp, 1 );
Vector_delete( temp, 18 );
if( Vector_size(temp) != 27 || strcmp( Vector_get( temp, 4 ), "f" ) || strcmp( Vector_get( temp, 26 ), "wori") || Vector_get( temp, 18 ) !=NULL || strcmp( Vector_get( temp, 17 ), "caibi") )
perror( "something wrong.\n");
Vector_destroy( temp );
return 0;
}
/**
* This function takes the raw input from the sensors and constructs a polygon
* by generating edges between each of the points known to be obstacles
*/
Polygon* Navigate_parse_sensor_data(char* raw_data) {
printf("%s[MEMORY][DEBUG] Memory at Navigate_parse_sensor_data start: %lu\n",
timestamp(), CURRENT_MEMORY_USAGE);
// parse the string into an array of integers
int sensor_data[5] = { 0, 0, 0, 0, 0 };
sscanf(raw_data, "[%d,%d,%d,%d,%d]",
&sensor_data[0], &sensor_data[1], &sensor_data[2], &sensor_data[3], &sensor_data[4]);
// loop through each of the sensors to generate the edges between each point
int i;
float angle, angle_next;
Edge *first_edge = NULL, *edge_tmp, *edge_prev = NULL;
for (i = 0; i < 5; i++) {
angle = Vector_get(ROBOT_STATUS, 2) + Vector_get(SENSOR_ORIENTATION, i);
angle_next = Vector_get(ROBOT_STATUS, 2) + Vector_get(SENSOR_ORIENTATION, i + 1);
// nothing in sight of this sensor and the next sensor
if (i < 5 - 1
&& sensor_data[i] >= NAVIGATION_DISTANCE_SAFE
&& sensor_data[i + 1] >= NAVIGATION_DISTANCE_SAFE) continue;
// this sensor picked something up but the ones around it didn't: draw a square around it
if ((i > 0 && i < 5 - 1
&& sensor_data[i] < NAVIGATION_DISTANCE_SAFE
&& sensor_data[i + 1] >= NAVIGATION_DISTANCE_SAFE
&& sensor_data[i - 1] >= NAVIGATION_DISTANCE_SAFE)
// special case 1: first sensor
|| (i == 0
&& sensor_data[0] < NAVIGATION_DISTANCE_SAFE
&& sensor_data[1] >= NAVIGATION_DISTANCE_SAFE)
// special case 2: last sensor
|| (i == 5 - 1
&& sensor_data[5 - 1] < NAVIGATION_DISTANCE_SAFE
&& sensor_data[5 - 2] >= NAVIGATION_DISTANCE_SAFE)){
// get coordinates of obstacle
float x = Vector_get(ROBOT_STATUS, 0) +
cosf((90 - angle) * M_PI / 180.0f) * sensor_data[i];
float y = Vector_get(ROBOT_STATUS, 1) +
sinf((90 - angle) * M_PI / 180.0f) * sensor_data[i];
// create a square around it
Vertice* v1 = Vertice_new(x - OBSTACLE_MIN_SIZE / 2, y - OBSTACLE_MIN_SIZE / 2);
Vertice* v2 = Vertice_new(x + OBSTACLE_MIN_SIZE / 2, y - OBSTACLE_MIN_SIZE / 2);
Vertice* v3 = Vertice_new(x + OBSTACLE_MIN_SIZE / 2, y + OBSTACLE_MIN_SIZE / 2);
Vertice* v4 = Vertice_new(x - OBSTACLE_MIN_SIZE / 2, y + OBSTACLE_MIN_SIZE / 2);
// join the edges of the square
edge_tmp = Edge_new_given_vertices(v1, v2);
edge_tmp->next = Edge_new_given_vertices(v2, v3);
edge_tmp->next->next = Edge_new_given_vertices(v3, v4);
edge_tmp->next->next->next = Edge_new_given_vertices(v4, v1);
// cleanup unused vertices
Vertice_destroy(v1);
Vertice_destroy(v2);
Vertice_destroy(v3);
Vertice_destroy(v4);
// prepare next step in loop
if (first_edge == NULL) first_edge = edge_tmp;
else edge_prev->next = edge_tmp; // link edges to one another
edge_prev = edge_tmp->next->next->next;
// this sensor and the next one picked something up
} else if (i < 5 - 1 && sensor_data[i] < NAVIGATION_DISTANCE_SAFE
&& sensor_data[i + 1] < NAVIGATION_DISTANCE_SAFE) {
edge_tmp = Edge_new_given_coordinates(
Vector_get(ROBOT_STATUS, 0) +
cosf((90 - angle) * M_PI / 180.0f) * sensor_data[i],
Vector_get(ROBOT_STATUS, 1) +
sinf((90 - angle) * M_PI / 180.0f) * sensor_data[i],
Vector_get(ROBOT_STATUS, 0) +
cosf((90 - angle_next) * M_PI / 180.0f) * sensor_data[i + 1],
Vector_get(ROBOT_STATUS, 1) +
sinf((90 - angle_next) * M_PI / 180.0f) * sensor_data[i + 1]);
// prepare next step in loop
if (first_edge == NULL) first_edge = edge_tmp;
else edge_prev->next = edge_tmp; // link edges to one another
edge_prev = edge_tmp;
}
}
printf("%s[MEMORY][DEBUG] Memory at Navigate_parse_sensor_data end: %lu\n",
timestamp(), CURRENT_MEMORY_USAGE);
Polygon* res = Polygon_new_given_first_edge(first_edge);
return res;
}
static inline void addLine(const char* line, Vector* lines, Panel* panel, const char* incFilter) {
Vector_add(lines, (Object*) ListItem_new(line, 0));
if (!incFilter || String_contains_i(line, incFilter))
Panel_add(panel, (Object*)Vector_get(lines, Vector_size(lines)-1));
}
int
Stack_peek(Stack s, void *value)
{
return Vector_get(&s->vector, Vector_length(&s->vector) - 1, value);
}