--- /dev/null
+/*
+ * homologation.cc 08/04/29
+ *
+ * Robot's main control program (Eurobot 2008).
+ *
+ * Copyright: (c) 2008 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#ifndef DEBUG
+#define DEBUG
+#endif
+
+#define FSM_MAIN
+#include <robodata.h>
+#include <robot_eb2008.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <servos_eb2008.h>
+#include <math.h>
+#include "trgen.h"
+#include <movehelper_eb2008.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+
+/* define this macro */
+#define COMPETITION
+
+#ifdef COMPETITION
+#define WAIT_FOR_START
+#define COMPETITION_TIME_DEFAULT 90
+#define TIME_TO_DEPOSITE_DEFAULT 60
+#else
+#undef WAIT_FOR_START
+#define COMPETITION_TIME_DEFAULT 900
+#define TIME_TO_DEPOSITE_DEFAULT 60
+#endif
+
+/* competition time in seconds */
+#define COMPETITION_TIME COMPETITION_TIME_DEFAULT
+#define TIME_TO_DEPOSITE TIME_TO_DEPOSITE_DEFAULT
+/* competition time in seconds */
+
+enum {
+ LEFT = 0,
+ RIGHT,
+ CENTER
+};
+
+/************************************************************************
+ * MISC FUNCTIONS
+ ************************************************************************/
+
+/**
+ * Convert back sharps' measured values to mm.
+ *
+ * @ingroup fsmmain
+ */
+void get_back_sharp_mm(int *sharp)
+{
+ ROBOT_LOCK(sharps);
+ sharp[LEFT] = (int)(robot.sharps.back_left)*1000;
+ sharp[RIGHT] = (int)(robot.sharps.back_right)*1000;
+ ROBOT_UNLOCK(sharps);
+}
+
+/**
+ * Convert rear sharps' measured values to mm.
+ *
+ * @ingroup fsmmain
+ */
+void get_rear_sharp_mm(int *sharp)
+{
+ ROBOT_LOCK(sharps);
+ sharp[LEFT] = (int)(robot.sharps.left)*1000;
+ sharp[RIGHT] = (int)(robot.sharps.right)*1000;
+ ROBOT_UNLOCK(sharps);
+}
+
+/**
+ * Convert front sharps' measured values to mm.
+ *
+ * @ingroup fsmmain
+ */
+void get_front_sharp_mm(int *sharp)
+{
+ ROBOT_LOCK(sharps);
+ sharp[LEFT] = (int)(robot.sharps.front_left)*1000;
+ sharp[RIGHT] = (int)(robot.sharps.front_right)*1000;
+ ROBOT_UNLOCK(sharps);
+}
+
+/**
+ * Get values from front sharps.
+ *
+ * @ingroup fsmmain
+ */
+void get_front_sharp_m(double *sharp)
+{
+ ROBOT_LOCK(sharps);
+ sharp[LEFT] = robot.sharps.front_left;
+ sharp[RIGHT] = robot.sharps.front_right;
+ ROBOT_UNLOCK(sharps);
+}
+
+/**
+ * Get values from rear sharps.
+ *
+ * @ingroup fsmmain
+ */
+void get_rear_sharp_m(double *sharp)
+{
+ ROBOT_LOCK(sharps);
+ sharp[LEFT] = robot.sharps.left;
+ sharp[RIGHT] = robot.sharps.right;
+ ROBOT_UNLOCK(sharps);
+}
+
+/**
+ * Get values from back sharps.
+ *
+ * @ingroup fsmmain
+ */
+void get_back_sharp_m(double *sharp)
+{
+ ROBOT_LOCK(sharps);
+ sharp[LEFT] = robot.sharps.back_left;
+ sharp[RIGHT] = robot.sharps.back_right;
+ ROBOT_UNLOCK(sharps);
+}
+
+/**
+ * Use bumpers check if we are closed to the dispenser
+ */
+int closed_to_dispenser()
+{
+ int rv = 0;
+
+ ROBOT_LOCK(bumper);
+ rv = robot.bumper.left | robot.bumper.right;
+ ROBOT_UNLOCK(bumper);
+
+ return rv;
+}
+
+int closed_to_container()
+{
+ int rv = 0;
+
+ ROBOT_LOCK(sharps);
+ rv = ((robot.sharps.back_left < 0.05)
+ && (robot.sharps.back_right < 0.05));
+ ROBOT_UNLOCK(sharps);
+
+ /* FIXME: */
+ return 1;
+}
+
+/**
+ * Competition timer. Stop robot when the timer exceeds.
+ *
+ */
+void *wait_for_end(void *arg)
+{
+ sleep(COMPETITION_TIME);
+ printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
+ robot_exit();
+ return NULL;
+}
+
+/**
+ * Timer to go to tray.
+ *
+ */
+void *wait_to_deposition(void *arg)
+{
+ sleep(TIME_TO_DEPOSITE);
+ FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
+ return NULL;
+}
+
+/**
+ * Get position of the point when we know the distance and angle to turn.
+ *
+ * @param act actual position
+ * @param pos countered position
+ */
+void get_relative_pos(struct est_pos_type *est, struct ref_pos_type *ref,
+ double l, double phi)
+{
+ ref->x = est->x + l*cos(est->phi + phi);
+ ref->y = est->y + l*sin(est->phi + phi);
+ ref->phi = est->phi + phi;
+}
+
+void robot_goto_point(struct ref_pos_type des_pos)
+{
+ struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+
+ tc.maxv /= 3;
+ robot_trajectory_new(&tc);
+ robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
+}
+
+void robot_go_closer()
+{
+ struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+
+ tc.maxv /= 4;
+ robot_trajectory_new(&tc);
+ robot_move_by(0.05, NO_TURN(), NULL);
+}
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+/* initial and starting states */
+FSM_STATE_DECL(init);
+FSM_STATE_DECL(wait_for_start);
+/* movement states */
+FSM_STATE_DECL(go_to_our_white_dispenser);
+FSM_STATE_DECL(go_to_our_white_dispenser2);
+FSM_STATE_DECL(go_to_container);
+/* ball and carousel states */
+FSM_STATE_DECL(get_balls);
+FSM_STATE_DECL(next_carousel_position);
+FSM_STATE_DECL(deposite_balls);
+
+/************************************************************************
+ * INITIAL AND STARTING STATES
+ ************************************************************************/
+
+FSM_STATE(init)
+{
+ /* start event ocurred */
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
+ PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
+ DEG2RAD(135));
+// robot_set_est_pos_trans(0.15, PLAYGROUND_HEIGHT_M - 0.7, DEG2RAD(180));
+ off_brush_left();
+ off_brush_right();
+ brushes_out();
+ open_bottom_door();
+ close_back_door();
+ FSM_TRANSITION(wait_for_start);
+// FSM_TRANSITION(go_to_container);
+// FSM_TRANSITION(get_balls);
+ /* FIXME: delete after the test */
+// robot.carousel_cnt = 5;
+// robot.carousel_pos = 0;
+// FSM_TRANSITION(deposite_balls);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(wait_for_start)
+{
+ switch (FSM_EVENT) {
+#ifdef WAIT_FOR_START
+ case EV_START: {
+ pthread_t thid;
+
+ /* start competition timer */
+ pthread_create(&thid, NULL, wait_for_end, NULL);
+ pthread_create(&thid, NULL, wait_to_deposition, NULL);
+ }
+#else
+ case EV_ENTRY:
+#endif
+ /* start to do something */
+ FSM_TRANSITION(go_to_our_white_dispenser);
+ break;
+ default: break;
+ }
+}
+
+/************************************************************************
+ * MOVEMENT STATES
+ ************************************************************************/
+
+FSM_STATE(go_to_our_white_dispenser)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(NULL);
+ robot_trajectory_add_final_point_trans(0.7,
+ PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(go_to_our_white_dispenser2);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(go_to_our_white_dispenser2)
+{
+ struct ref_pos_type des_pos = {0.15, PLAYGROUND_HEIGHT_M - 0.65, 0};
+ static int approaching_attempts = 0;
+
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_goto_point(des_pos);
+ break;
+ case EV_MOTION_DONE:
+// if (closed_to_dispenser() || approaching_attempts++ < 2) {
+// robot_go_closer();
+// } else {
+ FSM_TRANSITION(get_balls);
+// approaching_attempts = 0;
+// }
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(go_to_our_red_dispenser)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(NULL);
+ robot_trajectory_add_final_point_trans(0.7,
+ PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(go_to_our_white_dispenser2);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(go_to_our_red_dispenser2)
+{
+ struct ref_pos_type des_pos = {0.65, PLAYGROUND_HEIGHT_M - 0.20, 0};
+ static int approaching_attempts = 0;
+
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_goto_point(des_pos);
+ break;
+ case EV_MOTION_DONE:
+ if (closed_to_dispenser() || approaching_attempts++ < 2) {
+ robot_go_closer();
+ } else {
+ FSM_TRANSITION(get_balls);
+ approaching_attempts = 0;
+ }
+ break;
+ default: break;
+ }
+}
+
+#define GO_TO_CONTAINER_TIMER 10000
+
+FSM_STATE(go_to_container)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ off_brush_left();
+ off_brush_right();
+ off_bagr();
+ robot_trajectory_new_backward(NULL);
+ robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.8, 1.0);
+ robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.6,
+ 0, NO_TURN());
+ FSM_TIMER(GO_TO_CONTAINER_TIMER);
+ break;
+ case EV_TIMER:
+ case EV_MOTION_DONE:
+ if (closed_to_container()) {
+ robot.carousel_pos = 0;
+ FSM_TRANSITION(deposite_balls);
+ } else {
+ DBG("FIXME: go_closer_to_container\n");
+ }
+ break;
+ default: break;
+ }
+}
+
+/************************************************************************
+ * BALLS AND CAROUSEL MANIPULATION STATES
+ ************************************************************************/
+
+#define MAX_GET_BALL_ATTEMPS 8
+#define GET_BALL_TIMER 1500
+#define WAIT_BALL_INSIDE 5000
+#define GET_BALL_BAGR_SPEED 120
+
+FSM_STATE(get_balls)
+{
+ static int get_ball_attemps = 0;
+ static int ball_inside = 0;
+
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ case EV_RETURN:
+ open_bottom_door();
+ set_bagr(GET_BALL_BAGR_SPEED);
+ brushes_drives_in();
+ FSM_TIMER(500);
+ break;
+ case EV_TIMER:
+ if (ball_inside) {
+ ball_inside = 0;
+ close_bottom_door();
+ off_bagr();
+ if (robot.carousel_cnt >= BALL_TO_COLLECT) {
+ /* FIXME: just to test deposition */
+ robot.carousel_pos = 0;
+ FSM_TRANSITION(deposite_balls);
+// printf("go_to_container\n");
+// FSM_TRANSITION(go_to_container);
+ } else {
+ robot.carousel_pos =
+ (robot.carousel_pos+2) % CAROUSEL_SIZE;
+ SUBFSM_TRANSITION(
+ next_carousel_position, NULL);
+ }
+ } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) {
+ FSM_TIMER(GET_BALL_TIMER);
+ } else {
+ /* FIXME: */
+ DBG("go_to_container\n");
+// FSM_TRANSITION(go_to_container);
+ }
+ break;
+ case EV_BALL_INSIDE:
+ /* ball is already inside */
+ if (ball_inside)
+ break;
+
+ ball_inside = 1;
+ robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] =
+ (enum ball_color)robot.cmu.color;
+ robot.carousel_cnt++;
+ DBG("ball cnt=%d\n", robot.carousel_cnt);
+ FSM_TIMER(WAIT_BALL_INSIDE);
+ break;
+ default: break;
+ }
+}
+
+#define MAX_CAROUSEL_ATTEMPTS 3
+#define CAROUSEL_ATTEMPT_TIMER 1000
+
+FSM_STATE(next_carousel_position)
+{
+ static int carousel_attempts;
+ int rv;
+
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ carousel_attempts = 0;
+ DBG("carousel_pos = %d\n", robot.carousel_pos);
+ set_carousel(robot.carousel_pos);
+ FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
+ break;
+ case EV_TIMER:
+ carousel_attempts++;
+ /* check out, if the carousel reached position */
+ ROBOT_LOCK(drives);
+ rv = (robot.drives.carousel_pos == robot.carousel_pos);
+ ROBOT_UNLOCK(drives);
+ if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) {
+ FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
+ } else if(rv) {
+ DBG("carousel reached the position.\n");
+ SUBFSM_RET(NULL);
+ } else {
+ DBG("FIXME: carousel lost.\n");
+ SUBFSM_RET(NULL);
+ }
+ break;
+ default: break;
+ }
+}
+
+#define WAIT_FOR_DEPOSITION_TIMER 1500
+
+FSM_STATE(deposite_balls)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ if (robot.carousel_pos < 5) {
+ close_back_door();
+ SUBFSM_TRANSITION(next_carousel_position, NULL);
+ } else {
+ robot.carousel_cnt = 0;
+ /* FIXME: */
+ DBG("go_to_our_white_dispenser\n");
+// FSM_TRANSITION(go_to_our_white_dispenser);
+ }
+ break;
+ case EV_RETURN:
+ DBG("open_back_door\n");
+ open_back_door();
+ FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
+ break;
+ case EV_TIMER:
+ robot.carousel_cnt--;
+ robot.carousel_pos++;
+ close_back_door();
+ DBG("carousel_cnt = %d\n", robot.carousel_cnt);
+ FSM_TRANSITION(deposite_balls);
+ break;
+ default: break;
+ }
+}
+
+/* main loop */
+int main()
+{
+ /* robot initialization */
+ robot_init();
+
+ robot.carousel_cnt = 0;
+ robot.carousel_pos = 0;
+
+ FSM(MAIN)->debug_states = 1;
+
+ robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
+
+ /* start threads and wait */
+ robot_start();
+
+ robot_wait();
+
+ /* clean up */
+ robot_destroy();
+
+ return 0;
+}
projects / eurobot / public.git / commitdiff