-/*\r
- * mainfsm.c 07/04/20\r
- *\r
- * Finite state machine of the main part.\r
- *\r
- * Copyright: (c) 2007 DCE Eurobot Dragon Team\r
- * CTU FEE - Department of Control Engineering\r
- * License: GNU GPL v.2\r
- */\r
-\r
-#define FSM_MAIN\r
-#include "robot.h"\r
-#include <fsm.h>\r
-#include "fsmmain.h"\r
-#include <unistd.h>\r
-#include "fsmmove.h"\r
-#include "serva.h"\r
-#include <math.h>\r
-#include "trgen.h"\r
-#include "movehelper.h"\r
-\r
-#define WAIT_TRANSPORTER_FORWARD_MS 1000\r
-#define WAIT_TRANSPORTER_BACKWARD_MS 1000\r
-#define WAIT_TRANSPORTER_STOP_MS 1000\r
-\r
-#define WAIT_WASTE_PULLED_IN_MS 100\r
-\r
-#define WAIT_DOOR_OPEN_MS 1000\r
-#define WAIT_PREPARE_FLUSHING_MS 1000\r
-#define WAIT_FLUSHING_BOTTLE_MS 1000\r
-#define WAIT_FLUSHING_CAN_MS 1000\r
-\r
-enum {\r
- BOTTLE = 0,\r
- CAN,\r
- /* front transporter status */\r
- FT_STOP,\r
- FT_FORWARD,\r
- FT_BACKWARD,\r
- FD_UP,\r
- /* */\r
- WASTE_NOT_PULLED_IN,\r
- WASTE_WAIT_PULLING_IN,\r
- WASTE_PULLED_IN\r
-};\r
-\r
-void *wait_for_end(void *arg)\r
-{\r
- //sleep(90);\r
- //robot_exit();\r
- return NULL;\r
-}\r
-\r
-int fsm_main_init(struct robo_fsm *fsm, int ret_code);\r
-\r
-//#define HOMOLOGACE\r
-//#define SHOW\r
-#define DEMO\r
-\r
-FSM_STATE_DECL(plan_go);\r
-/**\r
- * FSM: main init\r
- */\r
-FSM_STATE(main_init) {\r
- pthread_t thid;\r
-\r
- if (/*1 ||*/ FSM_EVENT == EV_START) {\r
- pthread_create(&thid, NULL, wait_for_end, NULL);\r
- releaseFront(); // opens robot's front transporter\r
- frontTransporterForward();\r
- FSM_SIGNAL(PICKUP, EV_PICKUP_START);\r
-\r
-\r
- robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000.0;\r
- robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2.0)/1000.0;\r
- robot.act_pos.phi = -90.0/180*M_PI;\r
- printf("fsmmain going to plan_go\n");\r
- FSM_TRANSITION(plan_go);\r
- }\r
-}\r
-\r
-\r
-FSM_STATE_DECL(plan_go);\r
-\r
-\r
-\r
-// --------------------------------------------\r
-void avoid_enemy()\r
-{\r
- robot_trajectory_stop();\r
- ROBOT_LOCK(act_pos);\r
- double x = robot.act_pos.x, y = robot.act_pos.y;\r
- double heading = robot.act_pos.phi;\r
- ROBOT_UNLOCK(act_pos);\r
- \r
- y+=0.3*sin(heading);\r
- \r
- robot_trajectory_new(NULL);\r
- robot_trajectory_add_point(x + 0.5*cos(heading-M_PI/2.0),\r
- y + 0.5*sin(heading-M_PI/2.0));\r
- robot_trajectory_add_point(x + 0.7*cos(heading-M_PI/4.0),\r
- y + 0.7*sin(heading-M_PI/4.0));\r
- robot_trajectory_add_final_point(target_x, target_y, NAN);\r
-}\r
-\r
-void follow_bottle()\r
-{\r
- robot_trajectory_stop();\r
- \r
-}\r
- \r
-\r
-FSM_STATE(enemy_avoidance)\r
-{\r
- switch (FSM_EVENT) {\r
- case EV_DETECTED_BOTTLE:\r
- follow_bottle();\r
- break;\r
- case EV_ENEMY_AHEAD:\r
- avoid_enemy();\r
- break;\r
- case EV_GOAL_NOT_REACHABLE:\r
- case EV_MOTION_DONE:\r
- SUBFSM_RET();\r
- default:\r
- break;\r
- }\r
-}\r
-\r
-\r
-FSM_STATE_DECL(main_wait);\r
-\r
-FSM_STATE(plan_go)\r
-{\r
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;\r
- switch (FSM_EVENT) {\r
- case EV_STATE_ENTERED:\r
- tc.maxv *= 1.5;\r
-// robot_trajectory_new(&tc, 0);\r
-// robot_trajectory_add_point(robot.act_pos.x, robot.act_pos.y);\r
-// robot_trajectory_add_point(robot.act_pos.x+1.0, robot.act_pos.y);\r
-// robot_trajectory_add_point(robot.act_pos.x+1.0, robot.act_pos.y+1.0);\r
-// robot_trajectory_add_point(robot.act_pos.x, robot.act_pos.y+1.0);\r
-// robot_trajectory_add_final_point(robot.act_pos.x, robot.act_pos.y);\r
-// FSM_TRANSITION(main_wait);\r
- break;\r
- case EV_RETURN: // sem se dostanu kdyz projedu cestu, nebo najdu nepritele\r
- break;\r
- default: break;\r
- }\r
-}\r
-\r
-FSM_STATE_DECL(main_follow_bott);\r
-\r
-FSM_STATE(main_wait)\r
-{\r
- switch (FSM_EVENT) {\r
- case EV_STATE_ENTERED:\r
- break;\r
- case EV_DETECTED_BOTTLE:\r
- robot_trajectory_stop();\r
- FSM_TRANSITION(main_follow_bott);\r
- break;\r
- case EV_GOAL_NOT_REACHABLE:\r
- case EV_MOTION_DONE:\r
- FSM_TRANSITION(plan_go);\r
- break;\r
- default: break;\r
- }\r
-}\r
-\r
-FSM_STATE_DECL(main_grab);\r
-FSM_STATE(main_follow_bott)\r
-{\r
- FSM_TIMEOUT(200);\r
- int i = 0;\r
- switch (FSM_EVENT) {\r
- case EV_STATE_ENTERED:\r
- case EV_TIMEOUT:\r
- \r
- if(robot.bott_sens.sens1==CLOSE) i++;\r
- if(robot.bott_sens.sens2==CLOSE) i++;\r
- if(robot.bott_sens.sens3==CLOSE) i++;\r
- if(robot.bott_sens.sens4==CLOSE) i++;\r
- \r
- \r
- if(robot.bott_sens.sens1==FAR) robot_send_speed(0.05,0.1);\r
- else if(robot.bott_sens.sens4==FAR) robot_send_speed(0.1,0.05);\r
- else if(robot.bott_sens.sens2==FAR) robot_send_speed(0.1,0.1);\r
- else if(robot.bott_sens.sens3==FAR) robot_send_speed(0.1,0.1);\r
- else if(i >1) FSM_TRANSITION(main_grab);\r
- else FSM_TRANSITION(plan_go);\r
- break;\r
- default: break;\r
- }\r
-}\r
-\r
-FSM_STATE(main_grab)\r
-{\r
- FSM_TIMEOUT(200);\r
- \r
- switch (FSM_EVENT) {\r
- case EV_STATE_ENTERED:\r
- robot_send_speed(0.0,0.0);\r
- FSM_SIGNAL(PICKUP, EV_PICKUP_TRY);\r
- break;\r
- case EV_PICKUP_DONE:\r
- FSM_TRANSITION(plan_go);\r
- break;\r
- case EV_PICKUP_FAILED:\r
- FSM_TRANSITION(main_follow_bott);\r
- break;\r
- default: break;\r
- }\r
-}\r
+/*
+ * mainfsm.c 07/04/20
+ *
+ * Finite state machine of the main part.
+ *
+ * Copyright: (c) 2007 DCE Eurobot Dragon Team
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#define FSM_MAIN
+#include "robot.h"
+#include <fsm.h>
+#include "fsmmain.h"
+#include <unistd.h>
+#include "fsmmove.h"
+#include "serva.h"
+#include <math.h>
+#include "trgen.h"
+#include "movehelper.h"
+#include <map.h>
+#include <sharp.h>
+
+/* 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
+
+/**
+ * Convert sharp`s measured values to mm.
+ *
+ * @ingroup fsmmain
+ */
+void get_short_sharp_mm(int *sharp)
+{
+ ROBOT_LOCK(sharpsWaste);
+ sharp[0] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short1);
+ sharp[1] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short2);
+ sharp[2] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short3);
+ sharp[3] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short4);
+ ROBOT_UNLOCK(sharpsWaste);
+}
+
+/**
+ * Check out sharp values to evalute, if waste is in our reach.
+ *
+ * @ingroup fsmmain
+ * @return Returns true if waste is in our reach.
+ */
+int main_waste_ahead()
+{
+ int sharp[4];
+ int cnt = 0, i;
+
+ get_short_sharp_mm(sharp);
+ printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
+ for (i=0; i<4; i++)
+ if (sharp[i] > 0 && sharp[i] < 120)
+ cnt++;
+
+ return ((cnt >= 1) ? 1 : 0);
+}
+
+FSM_STATE_DECL(main_init);
+FSM_STATE_DECL(plan_go);
+FSM_STATE_DECL(waste_grab);
+
+/**
+ * Set starting position, playground`s safety zone and other obstacles.
+ *
+ * @ingroup fsmmain
+ */
+FSM_STATE(main_init)
+{
+ ShmapInit(1);
+ /* starting position of the robot */
+ ROBOT_LOCK(act_pos);
+ robot.act_pos.x = 0.5;
+ robot.act_pos.y = 0.5;
+ robot.act_pos.phi = 0;
+ ROBOT_UNLOCK(act_pos);
+ /* safety zone around playground`s wall */
+ ShmapSetRectangleType(0.0, 0.0, 0.2, 2.1, MAP_WALL_CSPACE);
+ ShmapSetRectangleType(0.0, 0.0, 3.0, 0.2, 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);
+ /* cup for batteries */
+ ShmapSetRectangleType(1.5, 1.4, 1.8, 1.0, MAP_WALL_CSPACE);
+ releaseFront();
+ frontDoorDown();
+ FSM_TRANSITION(plan_go);
+}
+
+/**
+ * Go randomly and try to detect some waste.
+ *
+ * @ingroup fsmmain
+ */
+FSM_STATE(plan_go)
+{
+ double goalx, goaly;
+ switch (FSM_EVENT) {
+ case EV_STATE_ENTERED:
+ do {
+ goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+ goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+ } while (!ShmapIsFreePoint(goalx, goaly));
+ robot_goto(goalx, goaly, NAN);
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_DONE:
+ printf("target achieved! \n");
+ ShmapClearOldPath();
+ FSM_TRANSITION(plan_go);
+ 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:
+ if (main_waste_ahead()) {
+ printf("waste is in front of me\n");
+ FSM_SIGNAL(PICKUP, EV_PICKUP_START);
+ }
+ FSM_TIMEOUT(200);
+ break;
+ case EV_START:
+ // do nothing1
+ break;
+ case EV_EXIT:
+ //ShmapFree();
+ break;
+
+ default: break;
+ }
+}
+
+/**
+ * Try to grab waste.
+ *
+ * @ingroup fsmmain
+ */
+FSM_STATE(waste_grab) {
+
+ switch (FSM_EVENT) {
+ case EV_STATE_ENTERED:
+ FSM_SIGNAL(PICKUP, EV_PICKUP_START);
+ robot_send_speed(0.1,0.1);
+ break;
+ case EV_PICKUP_READY:
+ robot_send_speed(0,0);
+ FSM_SIGNAL(PICKUP, EV_PICKUP_TRY);
+ break;
+ case EV_PICKUP_DONE:
+ printf("fsmmain: pickup done\n");
+ break;
+ case EV_PICKUP_FAILED:
+ printf("fsmmain: pickup failed\n");
+ break;
+ default: break;
+ }
+}
+
+/* ---------------------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+/* use previous main FSM for the Eurobot competition in Prague */
+#elif
+
+#define WAIT_TRANSPORTER_FORWARD_MS 1000
+#define WAIT_TRANSPORTER_BACKWARD_MS 1000
+#define WAIT_TRANSPORTER_STOP_MS 1000
+
+#define WAIT_WASTE_PULLED_IN_MS 100
+
+#define WAIT_DOOR_OPEN_MS 1000
+#define WAIT_PREPARE_FLUSHING_MS 1000
+#define WAIT_FLUSHING_BOTTLE_MS 1000
+#define WAIT_FLUSHING_CAN_MS 1000
+
+enum {
+ BOTTLE = 0,
+ CAN,
+ /* front transporter status */
+ FT_STOP,
+ FT_FORWARD,
+ FT_BACKWARD,
+ FD_UP,
+ /* */
+ WASTE_NOT_PULLED_IN,
+ WASTE_WAIT_PULLING_IN,
+ WASTE_PULLED_IN
+};
+
+void *wait_for_end(void *arg)
+{
+ //sleep(90);
+ //robot_exit();
+ return NULL;
+}
+
+int fsm_main_init(struct robo_fsm *fsm, int ret_code);
+
+//#define HOMOLOGACE
+//#define SHOW
+#define DEMO
+
+FSM_STATE_DECL(plan_go);
+/**
+ * FSM: main init
+ */
+FSM_STATE(main_init) {
+ pthread_t thid;
+
+ if (/*1 ||*/ FSM_EVENT == EV_START) {
+ pthread_create(&thid, NULL, wait_for_end, NULL);
+ releaseFront(); // opens robot's front transporter
+ frontTransporterForward();
+ FSM_SIGNAL(PICKUP, EV_PICKUP_START);
+
+
+ robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000.0;
+ robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2.0)/1000.0;
+ robot.act_pos.phi = -90.0/180*M_PI;
+ printf("fsmmain going to plan_go\n");
+ FSM_TRANSITION(plan_go);
+ }
+}
+
+
+FSM_STATE_DECL(plan_go);
+
+
+
+// --------------------------------------------
+void avoid_enemy()
+{
+ robot_trajectory_stop();
+ ROBOT_LOCK(act_pos);
+ double x = robot.act_pos.x, y = robot.act_pos.y;
+ double heading = robot.act_pos.phi;
+ ROBOT_UNLOCK(act_pos);
+
+ y+=0.3*sin(heading);
+
+ robot_trajectory_new(NULL);
+ robot_trajectory_add_point(x + 0.5*cos(heading-M_PI/2.0),
+ y + 0.5*sin(heading-M_PI/2.0));
+ robot_trajectory_add_point(x + 0.7*cos(heading-M_PI/4.0),
+ y + 0.7*sin(heading-M_PI/4.0));
+ robot_trajectory_add_final_point(target_x, target_y, NAN);
+}
+
+void follow_bottle()
+{
+ robot_trajectory_stop();
+
+}
+
+
+FSM_STATE(enemy_avoidance)
+{
+ switch (FSM_EVENT) {
+ case EV_DETECTED_BOTTLE:
+ follow_bottle();
+ break;
+ case EV_ENEMY_AHEAD:
+ avoid_enemy();
+ break;
+ case EV_GOAL_NOT_REACHABLE:
+ case EV_MOTION_DONE:
+ SUBFSM_RET();
+ default:
+ break;
+ }
+}
+
+
+FSM_STATE_DECL(main_wait);
+
+FSM_STATE(plan_go)
+{
+ struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+ switch (FSM_EVENT) {
+ case EV_STATE_ENTERED:
+ tc.maxv *= 1.5;
+// robot_trajectory_new(&tc, 0);
+// robot_trajectory_add_point(robot.act_pos.x, robot.act_pos.y);
+// robot_trajectory_add_point(robot.act_pos.x+1.0, robot.act_pos.y);
+// robot_trajectory_add_point(robot.act_pos.x+1.0, robot.act_pos.y+1.0);
+// robot_trajectory_add_point(robot.act_pos.x, robot.act_pos.y+1.0);
+// robot_trajectory_add_final_point(robot.act_pos.x, robot.act_pos.y);
+// FSM_TRANSITION(main_wait);
+ break;
+ case EV_RETURN: // sem se dostanu kdyz projedu cestu, nebo najdu nepritele
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE_DECL(main_follow_bott);
+
+FSM_STATE(main_wait)
+{
+ switch (FSM_EVENT) {
+ case EV_STATE_ENTERED:
+ break;
+ case EV_DETECTED_BOTTLE:
+ robot_trajectory_stop();
+ FSM_TRANSITION(main_follow_bott);
+ break;
+ case EV_GOAL_NOT_REACHABLE:
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(plan_go);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE_DECL(main_grab);
+FSM_STATE(main_follow_bott)
+{
+ FSM_TIMEOUT(200);
+ int i = 0;
+ switch (FSM_EVENT) {
+ case EV_STATE_ENTERED:
+ case EV_TIMEOUT:
+
+ if(robot.bott_sens.sens1==CLOSE) i++;
+ if(robot.bott_sens.sens2==CLOSE) i++;
+ if(robot.bott_sens.sens3==CLOSE) i++;
+ if(robot.bott_sens.sens4==CLOSE) i++;
+
+
+ if(robot.bott_sens.sens1==FAR) robot_send_speed(0.05,0.1);
+ else if(robot.bott_sens.sens4==FAR) robot_send_speed(0.1,0.05);
+ else if(robot.bott_sens.sens2==FAR) robot_send_speed(0.1,0.1);
+ else if(robot.bott_sens.sens3==FAR) robot_send_speed(0.1,0.1);
+ else if(i >1) FSM_TRANSITION(main_grab);
+ else FSM_TRANSITION(plan_go);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(main_grab)
+{
+ FSM_TIMEOUT(200);
+
+ switch (FSM_EVENT) {
+ case EV_STATE_ENTERED:
+ robot_send_speed(0.0,0.0);
+ FSM_SIGNAL(PICKUP, EV_PICKUP_TRY);
+ break;
+ case EV_PICKUP_DONE:
+ FSM_TRANSITION(plan_go);
+ break;
+ case EV_PICKUP_FAILED:
+ FSM_TRANSITION(main_follow_bott);
+ break;
+ default: break;
+ }
+}
+
+#endif /* main FSM selection */
FSM_STATE_DECL(pickup_wait_for_success);
FSM_STATE_DECL(pickup_jam);
+/* bad attempts counter. pickup tries to pickup until the limit is exceeded */
int pickup_bad_attemp_cnt = 0;
#define PICKUP_MAX_BAD_ATTEMP 3
-void pickup_get_short_sharp_mm(int *sharp)
+/**
+ * Convert sharp`s measured values to mm.
+ *
+ * @ingroup fsmpickup
+ * @param sharp Sharp values in mm
+ */
+void get_short_sharp_mm(int *sharp)
{
ROBOT_LOCK(sharpsWaste);
sharp[0] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short1);
ROBOT_UNLOCK(sharpsWaste);
}
+/**
+ * Evaluation function for waste pickuping.
+ *
+ * @ingroup fsmpickup
+ * @return return true if waste entered inside the entry.
+ */
int pickup_waste_entered()
{
int sharp[4];
int cnt = 0, i;
- pickup_get_short_sharp_mm(sharp);
+ get_short_sharp_mm(sharp);
printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
for (i=0; i<4; i++)
if (sharp[i] > 0 && sharp[i] < 55)
cnt++;
-/* FIXME: */
- if (pickup_bad_attemp_cnt > 2) {
- return 1;
- }
return ((cnt >= 2) ? 1 : 0);
}
+/**
+ * Sharp evaluation funtion.
+ *
+ * @ingroup fsmpickup
+ * @return Returns true if sharps are covered. This means the front door is closed.
+ */
int pickup_short_sharp_enclosed()
{
int sharp[4];
int cnt = 0, i;
- pickup_get_short_sharp_mm(sharp);
+ get_short_sharp_mm(sharp);
printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
for (i=0; i<4; i++)
if (sharp[i] < 20)
return ((cnt >= 3) ? 1 : 0);
}
+/**
+ * Evaluation function to detect waste jammed went pinking up.
+ *
+ * @ingroup fsmpickup
+ * @return Returns true when the door is unable to closed fully.
+ */
int pickup_front_door_jammed()
{
int rv;
}
/**
- * FSM State: Pickup state machine initialization.
+ * FSM State: Pickup state machine initialization. Release front rolling belt
+ * and release front door down.
*
* @ingroup fsmpickup
- *
*/
FSM_STATE(pickup_init)
{
}
/**
- * FSM State: Pickup main waiting loop.
+ * FSM State: Pickup main waiting loop. Wait for events from main FSM.
*
* @ingroup fsmpickup
*/
FSM_SIGNAL(MAIN, EV_PICKUP_READY);
break;
case EV_PICKUP_TRY:
+ printf("try to pickup a waste\n");
FSM_TRANSITION(pickup_try);
break;
case EV_TIMER:
}
}
+/**
+ * Wait until the waste entered the front and try to close front door to
+ * pickup the it. If maximal attempt exceeds, send EV_PICKUP_FAILED to main
+ * FSM.
+ *
+ * @ingroup fsmpickup
+ */
FSM_STATE(pickup_try)
{
switch (FSM_EVENT) {
}
}
+/**
+ * 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
+ */
FSM_STATE(pickup_wait_for_success) {
FSM_TIMEOUT(2000);
switch (FSM_EVENT) {
}
+/**
+ * Try to release the waste from the jammed state.
+ *
+ * @ingroup fsmpickup
+ */
FSM_STATE(pickup_jam) {
switch (FSM_EVENT) {
case EV_STATE_ENTERED:
projects / eurobot / public.git / commitdiff