*y = sy+v*sin(sa);
}
- void update_map_hokuyo(struct hokuyo_scan_type *s)
-void get_checkerboard(std::vector<Shape_detect::Point> &team)
-{
- Shape_detect::Point tmp, newPoint, start;
- unsigned int i;
-
- if (robot.team_color) {
- start.x = 0.625;
- start.y = 0.525;
- } else {
- start.x = 0.975;
- start.y = 0.525;
-
- }
-
- tmp = start;
-
- for (i = 1; i < 4; i++) {
- for (int j = 0; j < 3; j++) {
- newPoint.y = tmp.y + 0.7 * j;
- newPoint.x = tmp.x;
- team.push_back(newPoint);
- }
- tmp.x = tmp.x + 0.7;
- }
-
- if (robot.team_color) {
- tmp.x = start.x + 0.35;
- tmp.y = start.y + 0.35;
- } else {
- tmp.x = start.x - 0.35;
- tmp.y = start.y + 0.35;
-
- }
-
- for (i = 1; i < 4; i++) {
- for (int j = 0; j < 2; j++) {
- newPoint.y = tmp.y + 0.7 * j;
- newPoint.x = tmp.x;
- team.push_back(newPoint);
- }
- tmp.x = tmp.x + 0.7;
- }
-
- if (robot.team_color) {
- tmp.x = 1.675;
- tmp.y = 0.175;
- } else {
- tmp.x = 1.325;
- tmp.y = 0.175;
-
- }
-
- team.push_back(tmp);
-}
-
-inline float point_distance(Shape_detect::Point a, Shape_detect::Point b)
-{
- return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
-}
-
+ void update_map_hokuyo(struct lidar_scan_type *s)
{
double x, y;
struct robot_pos_type e;
extern "C" {
#endif
-void * thread_obstacle_forgeting(void * arg);
-/*void update_map(struct sharps_type *s);*/
+void * thread_target_detecion(void * arg);
- void update_map_hokuyo(struct hokuyo_scan_type *s);
+ void update_map_hokuyo(struct lidar_scan_type *s);
#ifdef __cplusplus
}
double min = 1000;
int min_i;
-- struct hokuyo_scan_type scan = robot.hokuyo;
++ struct lidar_scan_type scan = robot.hokuyo;
uint16_t *data = scan.data;
for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {