void testroute_run(void) { static int dest_pnt_list[] = {24, 0};//{24, 32, 8, 0, 8, 32, 24, 0, //14, 16, 18, 8, 4, 16, 28, 32, 18, 14, 0, 4, 16, 28, 24, 0}; static int dest_pnt_index = 0; while (1) { if (!control_can_interrupt()) continue; // 正在转弯抓角就不用再走下去了,反正控不了 // 更新边点号 if (!graph_is_edge_point(cur_point) && is_on_edge()) { cur_point = get_next_point(head_direction, cur_point); nobeep(); } _Bool interrupt = xxx(&dian, &last_, &xxx); // 先看看前面有没有人,有就停着 if (scan_infront() && !graph_is_corner_point(cur_point)) { yesbeep(); // beep!! execute_command(C_BREAK); continue; } else { //nobeep(); } if (graph_get_point_number(cur_point) == dest_pnt_list[dest_pnt_index]) { dest_pnt_index = (dest_pnt_index + 1) % 2; } curcmd = __move_to(dest_pnt_list[dest_pnt_index], D_NONE); execute_command(curcmd); } }
//状态机的运行函数 //参数应该是一个指针 //指针怎么变呢 void run_state(void *ch) { state = init_state; u16 i = 11; //我测试的时候用的,有bug,容易死循环 while (state && i > 0) { printf("[%x] --- [%d]\n", (u32)ch, i); state = (*state)(ch); ch = get_next_point(ch, TYPE_SIZE);//取到下一个指针 i--; } }
CONTROL_CALLBACK_FUNC(testroute_getcorner_callback, cmd, ev) { if (ev == E_FINISHED) { control_clear_distance(); wait_for_corner = 0; cur_point = get_next_point(head_direction, cur_point); curcmd = C_STOP; yesbeep(); } else if (ev == E_CROSS) { wait_for_corner = 1; } }
//if pTestedCells is NULL, then the tested points are not saved and it is more //efficient as it returns as soon as it sees first invalid point int EnvironmentROBARM::IsValidLineSegment(double x0, double y0, double x1, double y1, char **Grid2D, vector<CELLV>* pTestedCells) { bresenham_param_t params; int nX, nY; short unsigned int nX0, nY0, nX1, nY1; int retvalue = 1; CELLV tempcell; //make sure the line segment is inside the environment if(x0 < 0 || x0 >= EnvROBARMCfg.EnvWidth_m || x1 < 0 || x1 >= EnvROBARMCfg.EnvWidth_m || y0 < 0 || y0 >= EnvROBARMCfg.EnvHeight_m || y1 < 0 || y1 >= EnvROBARMCfg.EnvHeight_m) return 0; ContXY2Cell(x0, y0, &nX0, &nY0); ContXY2Cell(x1, y1, &nX1, &nY1); //iterate through the points on the segment get_bresenham_parameters(nX0, nY0, nX1, nY1, ¶ms); do { get_current_point(¶ms, &nX, &nY); if(Grid2D[nX][nY] == 1) { if(pTestedCells == NULL) return 0; else retvalue = 0; } //insert the tested point if(pTestedCells) { tempcell.bIsObstacle = (Grid2D[nX][nY] == 1); tempcell.x = nX; tempcell.y = nY; pTestedCells->push_back(tempcell); } } while (get_next_point(¶ms)); return retvalue; }