#endif
#define COMPETITION_TIME 90 /* competition time in seconds */
-#define STACK_SIZE 3 /* maximal number of bottles before the deposition */
+#define STACK_SIZE 3 /* maximal number of bottles before the deposition */
+
+/**
+ * Time constants.
+ */
+/* time before stopping the deposition belt resp. closing the back door */
+#define DEPOSITE_WAIT 1000
+/* the same as above, but we need to wait a little bit longer for the last one */
+#define LAST_DEPOSITE_WAIT 1500
+/* time before closing the inner door */
+#define WASTE_ROLL_OUT_WAIT 800
+/* time before another pickup */
+#define NEW_PICK_UP_WAIT 100
+/* waste detection period */
+#define WASTE_DETECTION_PERIOD 100
enum {
- NO_WASTE = 0,
+ UNKNOWN = 0,
BOTTLE,
CAN
};
/**
* Convert sharp`s measured values to mm.
*
- * @ingroup fsmmain
+ * @ingroup fsmmain
*/
void get_short_sharp_mm(int *sharp)
{
}
/**
- * Check out sharp values to evalute, if waste is in our reach.
+ * Check out sharp values to evaluate, if waste is in our reach.
*
- * @ingroup fsmmain
- * @return Returns true if waste is in our reach.
+ * @ingroup fsmmain
+ * @return returns true if waste is in our reach.
*/
int waste_ahead()
{
}
/**
- * Try to detect type of the bottle.
+ * Try to detect type of the waste.
*
* @ingroup fsmmain
- * @return returns type of the bottle.
+ * @return returns type of the waste.
*/
int recognize_waste()
{
- int type = NO_WASTE;
+ int type = UNKNOWN;
+ char bit = 0x01;
+ char status;
+ int i, cnt = 0;
+
+ ROBOT_LOCK(IRsensors);
+ status = robot.IRsensors.back & 0x1f;
+ ROBOT_UNLOCK(IRsensors);
+
+ for (i=0; i<8; i++)
+ if (status & (bit << i))
+ cnt++;
+
+ switch (cnt) {
+ case 0:
+ type = UNKNOWN;
+ break;
+ case 1: type = BOTTLE;
+ break;
+ default:
+ type = CAN;
+ break;
+ }
return type;
}
+/**
+ * Competition timer. Stop robot when the timer exceeds.
+ *
+ */
void *wait_for_end(void *arg)
{
sleep(COMPETITION_TIME);
- robot_exit();
printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
+ robot_exit();
return NULL;
}
+/**
+ * FSM states start here.
+ */
FSM_STATE_DECL(main_init);
FSM_STATE_DECL(plan_go);
FSM_STATE_DECL(waste_grab);
{
pthread_t thid;
+ /* start event ocurred */
if (1 || FSM_EVENT == EV_START) {
+ /* start competition timer */
pthread_create(&thid, NULL, wait_for_end, NULL);
+ /* init map and shared memory if required */
ShmapInit(1);
/* starting position of the robot */
ROBOT_LOCK(act_pos);
robot.waste_cnt = 0;
printf("waste_cnt = %d\n", robot.waste_cnt);
ROBOT_UNLOCK(waste_cnt);
- /* */
- releaseFront();
- printf("frontDoorDown\n");
- frontDoorDown();
+ /* start to do something */
+// FSM_TRANSITION(plan_go);
FSM_TRANSITION(waste_grab);
}
}
/**
- * Go randomly and try to detect some waste.
+ * Go and try to detect some waste.
*
* @ingroup fsmmain
*/
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_LOCK(act_pos);
- goalx = robot.act_pos.x + 0.2;
- goaly = robot.act_pos.y;
- robot.act_pos.phi = 0;
- ROBOT_UNLOCK(act_pos);
+ 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(100);
+ FSM_TIMER(WASTE_DETECTION_PERIOD);
break;
case EV_MOTION_DONE:
printf("target achieved! \n");
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);
+ FSM_TIMER(1000);
break;
case EV_TIMER:
- printf("frontDoorDown\n");
frontDoorDown();
if (waste_ahead()) {
printf("waste is in front of me\n");
ShmapClearOldPath();
FSM_TRANSITION(waste_grab);
}
- FSM_TIMER(100);
+ FSM_TIMER(WASTE_DETECTION_PERIOD);
break;
case EV_START:
// do nothing1
break;
case EV_PICKUP_DONE:
ROBOT_LOCK(waste_cnt);
- printf("increment waste_cnt\n");
robot.waste_cnt++;
waste_cnt = robot.waste_cnt;
ROBOT_UNLOCK(waste_cnt);
if (waste_cnt == 1 && stack_empty()) {
- printf("fsmmain: pickup done. Youuhouuu, I have a bottle!! In total: %d\n", waste_cnt);
+ printf("fsmmain: pickup done. Youuhouuu, I have "
+ "a bottle!! In total: %d\n", waste_cnt);
} else {
- printf("fsmmain: pickup done. But sensor detect empty stack!! In total: %d\n", waste_cnt);
+ printf("fsmmain: pickup done. But sensors detect "
+ "empty stack!! In total: %d\n", waste_cnt);
}
+ /* go to deposite waste if the stack is full */
if (waste_cnt >= STACK_SIZE) {
+ /* FIXME: here should be go_to_basket */
+// FSM_TRANSITION(go_to_basket);
FSM_TRANSITION(waste_deposite);
+ /* continue grabbing waste if there`s space in the stack */
} else {
- FSM_TIMER(100);
+ FSM_TIMER(NEW_PICK_UP_WAIT);
}
break;
case EV_PICKUP_FAILED:
FSM_TRANSITION(waste_grab);
break;
case EV_TIMER:
- printf("timeout: waste grab\n");
+ printf("timeout: continue grabbing waste\n");
FSM_TRANSITION(waste_grab);
break;
default:
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);
+ FSM_TIMER(1000);
break;
case EV_TIMER:
break;
*/
FSM_STATE(waste_deposite)
{
- int waste_cnt;
+ int waste_cnt, type;
ROBOT_LOCK(waste_cnt);
waste_cnt = robot.waste_cnt;
ROBOT_UNLOCK(waste_cnt);
if (stack_empty() && waste_cnt != 0) {
- printf("waste_cnt is %d, but I detect stack empty!!!\n", waste_cnt);
+ printf("waste_cnt is %d, but I detect stack empty!!!\n",
+ waste_cnt);
}
switch (FSM_EVENT) {
case EV_STATE_ENTERED:
printf("deposite waste: %d\n", waste_cnt);
innerDoorDown();
- FSM_TIMER(800);
+ FSM_TIMER(WASTE_ROLL_OUT_WAIT);
break;
case EV_TIMER:
innerDoorUp();
+ type = recognize_waste();
+ printf("waste recognition: ");
+ if (type == CAN) {
+ innerTransporterLeft();
+ printf("it`s a can\n");
+ } else if (type == BOTTLE) {
+ innerTransporterRight();
+ printf("it`s a bottle\n");
+ } else {
+ backDoorDown();
+ printf("unidentified\n");
+ }
/* need to recognize type of waste here */
- innerTransporterLeft();
FSM_TRANSITION(waste_deposite_wait);
break;
default: break;
waste_cnt = robot.waste_cnt;
ROBOT_UNLOCK(waste_cnt);
if (waste_cnt > 1)
- FSM_TIMER(1000);
+ FSM_TIMER(DEPOSITE_WAIT);
else
- FSM_TIMER(1500);
+ FSM_TIMER(LAST_DEPOSITE_WAIT);
break;
case EV_TIMER:
ROBOT_LOCK(waste_cnt);
/* FIXME: just to test grab, plan_go should
* be here instead */
innerTransporterStop();
+ backDoorUp();
FSM_TRANSITION(waste_grab);
}
break;
#include <sharp.h>
#include "serva.h"
-FSM_STATE_DECL(pickup_wait);
-FSM_STATE_DECL(pickup_try);
-FSM_STATE_DECL(pickup_free);
-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
+#define PICKUP_MAX_BAD_ATTEMPS 3
+
+/**
+ * Time constants
+ */
+/* time to wait for the waste rolling up to the stack. If time exceeded, the
+ * attempt is considered as jammed */
+#define WASTE_ROLL_IN_WAIT 1000
+/* time to wait for waste disengagement during the jam */
+#define WASTE_DISENGAGEMENT_WAIT 1500
+/* time period between unsuccessful attempts */
+#define PICK_UP_TRY_PERIOD 100
+
/**
* Convert sharp`s measured values to mm.
return rv;
}
+/**
+ * FSM states start here.
+ */
+FSM_STATE_DECL(pickup_wait);
+FSM_STATE_DECL(pickup_try);
+FSM_STATE_DECL(pickup_free);
+FSM_STATE_DECL(pickup_wait_for_success);
+FSM_STATE_DECL(pickup_jam);
+
/**
* FSM State: Pickup state machine initialization. Release front rolling belt
* and release front door down.
FSM_STATE(pickup_init)
{
releaseFront();
- printf("frontDoorDown\n");
frontDoorDown();
FSM_TRANSITION(pickup_wait);
}
case EV_PICKUP_START:
printf("start picking up waste\n");
pickup_bad_attemp_cnt = 0;
- printf("frontDoorDown\n");
+ /* FIXME: there should not be open doors again */
frontDoorDown();
frontTransporterForward();
FSM_TIMER(2000);
break;
case EV_PICKUP_TRY:
- printf("try to pickup a waste\n");
+ printf("try to pick up 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.
+ * pick up the it. If maximal attempt exceeds, picking up is considered as
+ * unsuccessful. In other way try to pick up again.
*
* @ingroup fsmpickup
*/
case EV_TIMER:
if (waste_entered()) {
printf("waste entered: continue\n");
- printf("frontDoorUp\n");
frontDoorUp();
FSM_TRANSITION(pickup_wait_for_success);
} else {
pickup_bad_attemp_cnt++;
printf("waste not entered: bad attempts: %d\n",
pickup_bad_attemp_cnt);
- if (pickup_bad_attemp_cnt >= PICKUP_MAX_BAD_ATTEMP) {
+ if (pickup_bad_attemp_cnt >= PICKUP_MAX_BAD_ATTEMPS) {
FSM_SIGNAL(MAIN, EV_PICKUP_FAILED);
FSM_TRANSITION(pickup_wait);
} else {
- FSM_TIMER(100);
+ FSM_TIMER(PICK_UP_TRY_PERIOD);
}
}
break;
FSM_STATE(pickup_wait_for_success) {
switch (FSM_EVENT) {
case EV_STATE_ENTERED:
- FSM_TIMER(1000);
+ FSM_TIMER(WASTE_ROLL_IN_WAIT);
break;
case EV_TIMER:
if(short_sharp_enclosed()
|| !front_door_jammed()) {
- printf("waste pickup done\n");
- printf("frontDoorDown\n");
+ printf("waste pick up done\n");
frontDoorDown();
- FSM_SIGNAL(MAIN, EV_PICKUP_DONE);
FSM_TRANSITION(pickup_wait);
+ FSM_SIGNAL(MAIN, EV_PICKUP_DONE);
} else {
- printf("waste pickup failed\n");
- FSM_SIGNAL(MAIN, EV_PICKUP_FAILED);
+ printf("waste pick up failed\n");
FSM_TRANSITION(pickup_jam);
+ FSM_SIGNAL(MAIN, EV_PICKUP_FAILED);
}
break;
default: break;
FSM_STATE(pickup_jam) {
switch (FSM_EVENT) {
case EV_STATE_ENTERED:
- printf("frontDoorDown\n");
frontDoorDown();
frontTransporterBackward();
- FSM_TIMER(1500);
+ FSM_TIMER(WASTE_DISENGAGEMENT_WAIT);
break;
case EV_TIMER:
FSM_TRANSITION(pickup_try);
projects / eurobot / public.git / commitdiff