robofsm: first uncompleted version of the homologation program.

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index 1f0c07263f21ae7c49ba61cf16c7be658ce0741c..148fdbe38620d503cf8f7a4325dd67477d8ef614 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -1,14 +1,17 @@
-
 /*
  * fsmmain.cc       08/04/29
  * 
  * Robot's main control program (Eurobot 2008).
  *
- * Copyright: (c) 2007 CTU Dragons
+ * 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>
@@ -22,8 +25,9 @@
 #include <sharp.h>
 #include <robomath.h>
 
-
+/* define this macro */
 #undef COMPETITION
+
 #ifdef COMPETITION
 #define WAIT_FOR_START
 #define COMPETITION_TIME_DEFAULT       90
@@ -39,14 +43,12 @@
 #define WAIT_FOR_DEPOSITION    WAIT_FOR_DEPOSITION_DEFAULT
 /* competition time in seconds */
 
-
 enum {
        LEFT = 0,
        RIGHT,
        CENTER
 };
 
-
 /**
  * Convert back sharps' measured values to mm.
  *
@@ -146,6 +148,20 @@ int enemy_ahead()
        return (0);
 }
 
+/**
+ * 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;
+}
+
 /**
  * Competition timer. Stop robot when the timer exceeds.
  *
@@ -202,6 +218,11 @@ void robot_go_backward_to_point(struct ref_pos_type des_pos)
 }
 
 FSM_STATE_DECL(main_init);
+FSM_STATE_DECL(go_to_our_white_dispenser);
+FSM_STATE_DECL(get_balls);
+FSM_STATE_DECL(wait_for_start);
+FSM_STATE_DECL(go_to_container);
+FSM_STATE_DECL(deposite_balls);
 
 /**
  * Set starting position, playground`s safety zone and other obstacles.
@@ -210,55 +231,141 @@ FSM_STATE_DECL(main_init);
  */
 FSM_STATE(main_init) 
 {
-       pthread_t thid;
-
        /* start event ocurred */
        switch (FSM_EVENT) {
-#ifndef WAIT_FOR_START
                case EV_ENTRY:
-#endif
-               case EV_START:
-                       /* start competition timer */
-                       pthread_create(&thid, NULL, wait_for_end, NULL);
-                       pthread_create(&thid, NULL, wait_to_deposition, NULL);
                        /* init map and shared memory if required */
                        ShmapInit(1);
                        /* starting position of the robot */
-                       //robot_set_act_pos_trans(ROBOT_AXIS_TO_BACK_M, 
-                       //                      PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2,
-                       //                      DEG2RAD(0));
                        robot_set_act_pos_trans(ROBOT_WIDTH_M/2,
                                                PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-                                               DEG2RAD(-90));
-                       /* safety zone around playground`s wall  */
-//                     ShmapSetRectangleType(0.0, 0.0, 0.1, 2.1, MAP_WALL_CSPACE);
-//                     ShmapSetRectangleType(0.0, 0.0, 3.0, 0.1, MAP_WALL_CSPACE);
-//                     ShmapSetRectangleType(0.0, 2.0, 3.0, 2.1, MAP_WALL_CSPACE);
-//                     ShmapSetRectangleType(2.9, 0.0, 3.0, 2.1, MAP_WALL_CSPACE);
-                       /* 3 cups for batteries */
-//                     ShmapSetRectangleFlag(1.3, PLAYGROUND_HEIGHT_M, 1.7, 
-//                                     PLAYGROUND_HEIGHT_M - 0.8, MAP_FLAG_WALL, 0);
-//                     ShmapSetRectangleFlag(1.3, PLAYGROUND_HEIGHT_M/2 - 0.2, 
-//                                             1.7, PLAYGROUND_HEIGHT_M/2 + 0.2, 
-//                                             MAP_FLAG_WALL, 0);
-                       ShmapSetRectangleFlag(1.3, 0.0, 1.7, 0.8, MAP_FLAG_WALL, 0);
+                                               DEG2RAD(135));
                        /* waste counter */
-                       FSM_TIMER(100);
+                       FSM_TRANSITION(wait_for_start);
                        break;
-               case EV_TIMER:
-                       brushes_out();
-                       /* start to do something */
-#ifdef COMPETITION
-//                     FSM_TRANSITION(plan_go_start);
+               default: break;
+       }
+}
+
+FSM_STATE(wait_for_start)
+{
+       pthread_t thid;
+
+       switch (FSM_EVENT) {
+#ifdef WAIT_FOR_START
+               case EV_START:
+                       /* start competition timer */
+                       pthread_create(&thid, NULL, wait_for_end, NULL);
+                       pthread_create(&thid, NULL, wait_to_deposition, NULL);
 #else
-//                     FSM_TRANSITION(plan_go_start);
-//                     FSM_TRANSITION(plan_go);
+               case EV_ENTRY:
 #endif
+                       /* start to do something */
+                       FSM_TRANSITION(go_to_our_white_dispenser);
                        break;
                default: break;
        }
 }
 
+FSM_STATE(go_to_our_white_dispenser)
+{
+       static int attempts = 0;
+
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       brushes_out();
+                       close_back_door();
+
+                       robot_trajectory_new_backward(NULL);
+                       robot_trajectory_add_point_trans(0.7, PLAYGROUND_HEIGHT_M - 0.7);
+                       robot_trajectory_add_final_point_trans(0, PLAYGROUND_HEIGHT_M - 0.7, NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       if (closed_to_dispenser || attempts > 3)
+                               FSM_TRANSITION(get_balls);
+
+                       attempts++;
+                       robot_trajectory_new(NULL);
+                       robot_trajectory_add_point_trans(0.7, PLAYGROUND_HEIGHT_M - 0.7);
+                       robot_trajectory_add_final_point_trans(0.7, PLAYGROUND_HEIGHT_M, NO_TURN());
+                       break;
+               default: break;
+       }
+}
+
+FSM_STATE(get_balls)
+{
+       static int get_ball_attemps = 0;
+       static int ball_inside = 0;
+
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       on_bagr();
+                       brushes_in();
+                       FSM_TIMER(500);
+                       break;
+               case EV_TIMER:
+                       if (ball_inside) {
+                               ball_inside = 0;
+                               close_bottom_door();
+                               if (robot.carousel_cnt >= CAROUSEL_SIZE) {
+                                       FSM_TRANSITION(go_to_container);
+                               } else {
+                                       robot.carousel_pos = 
+                                               (robot.carousel_pos+2) % CAROUSEL_SIZE;
+                                       set_carousel(robot.carousel_pos);
+                               }
+                       } else if (get_ball_attemps++ < 3) {
+                               FSM_TIMER(500);
+                       } else {
+                               /* FIXME: kam s tim? */
+                       }
+                       break;
+               case EV_BALL_INSIDE:
+//                     robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] = (enum )robot.cmu.color;
+                       ball_inside = 1;
+                       robot.carousel_cnt++;
+                       FSM_TIMER(500);
+                       break;
+               default: break;
+       }
+}
+
+FSM_STATE(go_to_container)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(NULL);
+                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.5, 
+                                               PLAYGROUND_HEIGHT_M, TURN_CW(90));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(deposite_balls);
+                       break;
+               default: break;
+       }
+}
+
+FSM_STATE(deposite_balls)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       open_back_door();
+                       ROBOT_LOCK(drives);
+                       robot.carousel_pos = robot.drives.carousel_pos;
+                       ROBOT_UNLOCK(drives);
+                       FSM_TIMER(300);
+                       break;
+               case EV_TIMER:
+                       close_back_door();
+                       if (robot.carousel_cnt <= 0)
+                               FSM_TRANSITION(go_to_our_white_dispenser);
+                       robot.carousel_pos = (robot.carousel_pos+2) % CAROUSEL_SIZE;
+                       set_carousel(robot.carousel_pos);
+                       break;
+               default: break;
+       }
+}
 
 /* main loop */
 int main()
@@ -266,12 +373,15 @@ 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;
        robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
-       robot.fsm[FSM_ID_DISP].state = &fsm_state_disp_init;
-       robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
+//     robot.fsm[FSM_ID_DISP].state = &fsm_state_disp_init;
+//     robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
 
         /* start threads and wait */
         robot_start();

diff --git a/src/robofsm/eb2008/roboevent_eb2008.py b/src/robofsm/eb2008/roboevent_eb2008.py
index 21b2d247ea3536dce15886e21699e8e39085198b..272af0c60a9eee1f0b571ad994a00772b2b247bd 100644 (file)
--- a/src/robofsm/eb2008/roboevent_eb2008.py
+++ b/src/robofsm/eb2008/roboevent_eb2008.py
@@ -4,6 +4,8 @@ events = {
         "EV_SHORT_TIME_TO_END" : "Time to go for the deposition regarless of what do we have.",
        "EV_DEPOSITE_DONE" : "",
        "EV_STACK_FULL" : "",
+       "EV_BALL_INSIDE" : "",
+       "EV_CAROUSEL_POS_ACHIEVED" : "",
 
        "EV_MOTION_DONE" : "Previously submitted motion is finished",
        "EV_LOST_DURING_MOTION" : "Actual position of the robot is too far from reference",

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index 0a745fee54a729b3b5212347624e7ce66fd093d2..3a146e5da16e42ea4df4e9d33646059d3a99bdb7 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -74,6 +74,7 @@ enum robot_fsm_id {
 #define __robot_lock_joy_data          lock_joy_data
 #define __robot_lock_meas_angles       lock_meas_angles
 #define __robot_lock_new_trajectory    lock
+#define __robot_lock_drives            lock
 #define __robot_lock_sharps            lock
 #define __robot_lock_cmu               lock
 #define __robot_lock_bumper            lock
@@ -98,6 +99,7 @@ struct robot_eb2008 {
        #define CAROUSEL_SIZE 5
        enum ball_color carousel[CAROUSEL_SIZE];
        unsigned char carousel_cnt; /* The number of balls in carousel */
+       unsigned char carousel_pos; /* required position of the carousel */
 
        /** Temporary storage for new trajectory. After the trajectory creation is
         * finished, this trajectory is submitted to fsmmove. */
@@ -121,6 +123,7 @@ struct robot_eb2008 {
 
        struct motion_irc_type odometry;
        struct joy_data_type joy_data;
+       struct drives_type drives;
 
        /* orte */
        struct eb2008_orte_data orte;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index d6f9ad65f6165e60d1854a0879ff0ba7ad4fa3b9..7b278b83109f2fd19cf5cfb7dd72f8e2c7e7be72 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -178,8 +178,17 @@ void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
 void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
+       struct drives_type *instance = (struct drives_type *)vinstance;
+
        switch (info->status) {
                case NEW_DATA:
+                       ROBOT_LOCK(drives);
+                       robot.drives = *instance;
+                       ROBOT_UNLOCK(drives);
+                       struct drives_type *drives = 
+                                       malloc(sizeof(struct drives_type));
+                       *drives = *instance;
+                       FSM_SIGNAL(MAIN, EV_CAROUSEL_POS_ACHIEVED, drives);
                        break;
                case DEADLINE:
                        printf("ORTE deadline occurred - drives receive\n");
@@ -217,6 +226,44 @@ void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
+                       void *recvCallBackParam)
+{
+       struct cmu_type *instance = (struct cmu_type *)vinstance;
+
+       switch (info->status) {
+               case NEW_DATA: {
+                       ROBOT_LOCK(cmu);
+                       robot.cmu = *instance;
+                       ROBOT_UNLOCK(cmu);
+                       if (robot.cmu.color != NO_BALL)
+                               FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
+                       break;
+               }
+               case DEADLINE:
+                       printf("%s: ORTE deadline occurred\n", __FUNCTION__);
+                       break;
+       }
+}
+
+void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
+                       void *recvCallBackParam)
+{
+       struct bumper_type *instance = (struct bumper_type *)vinstance;
+
+       switch (info->status) {
+               case NEW_DATA: {
+                       ROBOT_LOCK(bumper);
+                       robot.bumper = *instance;
+                       ROBOT_UNLOCK(bumper);
+                       break;
+               }
+               case DEADLINE:
+                       printf("%s: ORTE deadline occurred\n", __FUNCTION__);
+                       break;
+       }
+}
+
 void robot_init_orte()
 {
        robot.orte.strength = 20;
@@ -250,5 +297,7 @@ void robot_init_orte()
        eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
        eb2008_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
        eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
+       eb2008_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
+       eb2008_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
 }