Main FSM: Waste deposite.

diff --git a/app/robofsm/fsmmain.cc b/app/robofsm/fsmmain.cc
index 73c083798bd7be5d4fa64628b6347b1c5e5dce5f..6089ebcdc71ba4e22808ae544c9420cfe0027e4e 100644 (file)
--- a/app/robofsm/fsmmain.cc
+++ b/app/robofsm/fsmmain.cc
@@ -24,11 +24,19 @@
 /* define this macro to use main FSM for the competition in Saint Peterburg */
 #define FSM_MAIN_PETERBURG
 
-
 /* ---------------------------------------------------------------------- */
 /* use main FSM for the competition in Saint Peterburg */
 #ifdef FSM_MAIN_PETERBURG
 
+/* postition of the basket (in meters) */
+#ifdef RED_BASKET
+#define BASKET_GOALX   0.0
+#define BASKET_GOALY   0.0
+#else
+#define BASKET_GOALX   3.0
+#define BASKET_GOALY   0.0
+#endif
+
 /**
  * Convert sharp`s measured values to mm.
  *
@@ -50,7 +58,7 @@ void get_short_sharp_mm(int *sharp)
  * @ingroup fsmmain
  * @return Returns true if waste is in our reach.
  */
-int main_waste_ahead()
+int waste_ahead()
 {
        int sharp[4];
        int cnt = 0, i;
@@ -64,9 +72,22 @@ int main_waste_ahead()
        return ((cnt >= 1) ? 1 : 0);
 }
 
+/**
+ * Evaluation function to be sure the basket is behind us.
+ * 
+ * @ingroup    fsmmain
+ * @return
+ */
+int basket_behind()
+{
+       /* FIXME: return true by now, until the back sharps are connected */
+       return 1;
+}
+
 FSM_STATE_DECL(main_init);
 FSM_STATE_DECL(plan_go);
 FSM_STATE_DECL(waste_grab);
+FSM_STATE_DECL(waste_deposite)
 
 /**
  * Set starting position, playground`s safety zone and other obstacles.
@@ -130,7 +151,7 @@ FSM_STATE(plan_go)
                        break;
                case EV_TIMER:
                        frontDoorDown();
-                       if (main_waste_ahead()) {
+                       if (waste_ahead()) {
                                printf("waste is in front of me\n");
                                ROBOT_LOCK(act_pos);
                                goalx = robot.act_pos.x;
@@ -171,7 +192,7 @@ FSM_STATE(waste_grab) {
                        FSM_SIGNAL(PICKUP, EV_PICKUP_TRY);
                        break;
                case EV_PICKUP_DONE:
-                       printf("fsmmain: pickup done\n");       
+                       printf("fsmmain: pickup done. Youuhouuu, I have a bottle!!\n"); 
                        break;
                case EV_PICKUP_FAILED:
                        printf("fsmmain: pickup failed\n");     
@@ -181,6 +202,55 @@ FSM_STATE(waste_grab) {
        }
 }
 
+/**
+ * Deposite waste.
+ * 
+ * @ingroup    fsmmain
+ */
+FSM_STATE(waste_deposite)
+{
+       switch (FSM_EVENT) {
+               case EV_STATE_ENTERED:
+                       robot_goto(BASKET_GOALX, BASKET_GOALY, NAN);
+                       FSM_TIMER(100);
+                       break;
+               case EV_MOTION_DONE:
+                       printf("target achieved! \n");
+                       if (basket_behind()) {
+                       } else {
+                               printf("deposite waste\n");
+                       }
+                       break;
+               case EV_GOAL_IS_TMP_OBSTACLE:
+               case EV_GOAL_IS_OBSTACLE:
+               case EV_GOAL_NOT_REACHABLE:
+                       DBG("Goal is not reachable: %s\n",fsm_event_str(FSM_EVENT));
+                       FSM_TIMER(10000);
+                       break;
+               case EV_TIMER:
+                       frontDoorDown();
+                       if (waste_ahead()) {
+                               printf("waste is in front of me\n");
+                               ROBOT_LOCK(act_pos);
+                               goalx = robot.act_pos.x;
+                               goaly = robot.act_pos.y;
+                               robot.act_pos.phi = 0;
+                               ROBOT_UNLOCK(act_pos);
+                               robot_goto(goalx, goaly, NAN);
+                               FSM_TRANSITION(waste_grab);
+                       }
+                       FSM_TIMEOUT(100);
+                       break;
+               case EV_START:
+                       // do nothing1
+                       break;
+               case EV_EXIT:
+                       //ShmapFree();
+                       break;
+               default: break;
+       }
+}
+
 /* ---------------------------------------------------------------------- */
 /* ---------------------------------------------------------------------- */
 /* use previous main FSM for the Eurobot competition in Prague */

diff --git a/app/robofsm/fsmpickup.c b/app/robofsm/fsmpickup.c
index e4bd479b201c8e5b069d0ff0d8dbc3f6639d230f..66847aba277f38fda2186487880748923db5d21f 100644 (file)
--- a/app/robofsm/fsmpickup.c
+++ b/app/robofsm/fsmpickup.c
@@ -18,8 +18,8 @@ int pickup_bad_attemp_cnt = 0;
 /**
  * Convert sharp`s measured values to mm.
  *
- * @ingroup fsmpickup
- * @param sharp Sharp values in mm
+ * @ingroup    fsmpickup
+ * @param sharp        Sharp values in mm
  */
 void get_short_sharp_mm(int *sharp)
 {
@@ -34,10 +34,10 @@ void get_short_sharp_mm(int *sharp)
 /**
  * Evaluation function for waste pickuping.
  *
- * @ingroup fsmpickup
- * @return return true if waste entered inside the entry.
+ * @ingroup    fsmpickup
+ * @return     return true if waste entered inside the entry.
  */
-int pickup_waste_entered()
+int waste_entered()
 {
        int sharp[4];
        int cnt = 0, i;
@@ -54,10 +54,11 @@ int pickup_waste_entered()
 /**
  * Sharp evaluation funtion.
  * 
- * @ingroup fsmpickup
- * @return Returns true if sharps are covered. This means the front door is closed.
+ * @ingroup    fsmpickup
+ * @return     Returns true if sharps are covered. This means the front 
+ *             door is closed.
  */
-int pickup_short_sharp_enclosed()
+int short_sharp_enclosed()
 {
        int sharp[4];
        int cnt = 0, i;
@@ -74,10 +75,10 @@ int pickup_short_sharp_enclosed()
 /**
  * Evaluation function to detect waste jammed went pinking up.
  *
- * @ingroup fsmpickup
- * @return Returns true when the door is unable to closed fully.
+ * @ingroup    fsmpickup
+ * @return     Returns true when the door is unable to closed fully.
  */
-int pickup_front_door_jammed()
+int front_door_jammed()
 {
        int rv;
 
@@ -92,7 +93,7 @@ int pickup_front_door_jammed()
  * FSM State: Pickup state machine initialization. Release front rolling belt 
  * and release front door down.
  *
- * @ingroup fsmpickup
+ * @ingroup    fsmpickup
  */
 FSM_STATE(pickup_init)
 {
@@ -107,7 +108,7 @@ FSM_STATE(pickup_init)
 /**
  * FSM State: Pickup main waiting loop. Wait for events from main FSM.
  *
- * @ingroup fsmpickup
+ * @ingroup    fsmpickup
  */
 FSM_STATE(pickup_wait) {
        switch (FSM_EVENT) {
@@ -137,14 +138,14 @@ FSM_STATE(pickup_wait) {
  * pickup the it. If maximal attempt exceeds, send EV_PICKUP_FAILED to main
  * FSM.
  *
- * @ingroup fsmpickup
+ * @ingroup    fsmpickup
  */
 FSM_STATE(pickup_try)
 {
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                case EV_TIMER:
-                       if (pickup_waste_entered()) {
+                       if (waste_entered()) {
                                printf("waste entered: continue\n");
                                frontDoorUp();
                                FSM_TRANSITION(pickup_wait_for_success);
@@ -168,7 +169,7 @@ FSM_STATE(pickup_try)
  * Front door is closed up, so the waste should goes up. If the door can`t be 
  * closed up fully means, that waste is jammed so try to release it out.
  *
- * @ingroup fsmpickup
+ * @ingroup    fsmpickup
  */
 FSM_STATE(pickup_wait_for_success) {
        FSM_TIMEOUT(2000);
@@ -176,8 +177,8 @@ FSM_STATE(pickup_wait_for_success) {
                case EV_STATE_ENTERED:
                        break;
                case EV_TIMEOUT:
-                       if(pickup_short_sharp_enclosed()) {
-//                             && !pickup_front_door_jammed()) {
+                       if(short_sharp_enclosed()
+                               || !front_door_jammed()) {
                                printf("waste pickup done\n");
                                ROBOT_LOCK(sharpsWaste);
                                robot.waste_cnt++;
@@ -199,7 +200,7 @@ FSM_STATE(pickup_wait_for_success) {
 /**
  * Try to release the waste from the jammed state.
  *
- * @ingroup fsmpickup
+ * @ingroup    fsmpickup
  */
 FSM_STATE(pickup_jam) {
        switch (FSM_EVENT) {