robofsm: robot_orte.c: sharp sensor data handling; ACT fsm: events added

fsmact.c: some simple 'reactive demo' created
act fsm: debug_states = 1

diff --git a/src/robofsm/fsmact.c b/src/robofsm/fsmact.c
index bafb3cea9d8bd7cd61b9e28ce6fa539cbc0d504c..b46b7ca4e20ee8304faf24a9f839aebb360e15af 100644 (file)
--- a/src/robofsm/fsmact.c
+++ b/src/robofsm/fsmact.c
@@ -9,7 +9,9 @@
  * ***********************************************************/
 
 FSM_STATE_DECL(wait_for_command);
+FSM_STATE_DECL(reactive_demo);
 FSM_STATE_DECL(dummy_action);
+FSM_STATE_DECL(suck_the_puck);
 
 /* ***********************************************************
  *  STATE DEFINITIONS
@@ -20,14 +22,17 @@ FSM_STATE(wait_for_command) {
        case EV_ENTRY:
                act_belts(BELTS_OFF, BELTS_OFF);
                act_chelae(CHELA_OPEN, CHELA_OPEN);
+               FSM_TRANSITION(reactive_demo);
                // TODO
                // put all actuators to defined initial positions
                break;
        case EV_PICK_UP_PUCK:
-               FSM_TRANSITION(dummy_action);
+               //FSM_TRANSITION(dummy_action);
                break;
        case EV_RETURN:
        case EV_TIMER:
+       case EV_PUCK_REACHABLE:
+       case EV_PUCK_INSIDE:
                DBG_PRINT_EVENT("unhandled event");
                break;
        case EV_EXIT:
@@ -35,6 +40,51 @@ FSM_STATE(wait_for_command) {
        }
 }
 
+FSM_STATE(reactive_demo) {
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               break;
+       case EV_PUCK_REACHABLE:
+               FSM_TRANSITION(suck_the_puck);
+               break;
+       case EV_PUCK_INSIDE:
+               break;
+       case EV_RETURN:
+       case EV_TIMER:
+       case EV_PICK_UP_PUCK:
+               DBG_PRINT_EVENT("unhandled event");
+               break;
+       case EV_EXIT:
+               break;
+       }
+}
+
+FSM_STATE(suck_the_puck) {
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               FSM_TIMER(1000);
+               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
+               act_belts(BELTS_IN, BELTS_IN);
+               break;
+       case EV_PUCK_INSIDE:
+               act_belts(BELTS_OFF, BELTS_OFF);
+               FSM_TRANSITION(reactive_demo);
+               break;
+       case EV_RETURN:
+       case EV_TIMER:
+               FSM_TRANSITION(reactive_demo);
+               break;
+       case EV_PICK_UP_PUCK:
+       case EV_PUCK_REACHABLE:
+               DBG_PRINT_EVENT("unhandled event");
+               break;
+       case EV_EXIT:
+               act_belts(BELTS_OFF, BELTS_OFF);
+               act_chelae(CHELA_OPEN, CHELA_OPEN);
+               break;
+       }
+}
+
 FSM_STATE(dummy_action) {
        switch (FSM_EVENT) {
        case EV_ENTRY:
@@ -46,6 +96,8 @@ FSM_STATE(dummy_action) {
                break;
        case EV_RETURN:
        case EV_PICK_UP_PUCK:
+       case EV_PUCK_REACHABLE:
+       case EV_PUCK_INSIDE:
                DBG_PRINT_EVENT("unhandled event");
                break;
        case EV_EXIT:
@@ -54,4 +106,3 @@ FSM_STATE(dummy_action) {
 }
 
 
-

diff --git a/src/robofsm/fsmmain.c b/src/robofsm/fsmmain.c
index d6977ef8d9f35e2eecdfc6892abbb877edfa9754..dbd779ff89f8810410af84de06d90baa5b9597f7 100644 (file)
--- a/src/robofsm/fsmmain.c
+++ b/src/robofsm/fsmmain.c
@@ -177,7 +177,7 @@ FSM_STATE(wait_for_start)
                        robot.state = COMPETITION_STARTED;
                        ROBOT_UNLOCK(state);
                        //FSM_TRANSITION(approach_our_static_dispenser);
-                       FSM_TRANSITION(approach_opponents_static_dispenser);
+                       //FSM_TRANSITION(approach_opponents_static_dispenser);
                        break;
                case EV_RETURN:
                case EV_TIMER:

diff --git a/src/robofsm/main.cc b/src/robofsm/main.cc
index a20cb85e18f86a9df702298f7fbe6fed5e84ed7f..05003abff8efaf16d00baac5825802257a7e4500 100644 (file)
--- a/src/robofsm/main.cc
+++ b/src/robofsm/main.cc
@@ -29,6 +29,7 @@ int main()
 
        robot.fsm.main.debug_states = 1;
        robot.fsm.motion.debug_states = 1;
+       robot.fsm.act.debug_states = 1;
 
        robot.fsm.main.state = &fsm_state_main_init;
        robot.fsm.main.transition_callback = trans_callback;

diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index d65ceeb491983f56cad6a144854dfe286b76f089..2e9e8e75d959ac5862de076e00a141cceb11c723 100644 (file)
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -33,8 +33,9 @@ events = {
     },
     "act" : {
         "EV_PICK_UP_PUCK" : "Signal from the main FSM to initiate the puck picking up procedure",
+       "EV_PUCK_REACHABLE" : "Puck (or something) in reach of chelae",
+        "EV_PUCK_INSIDE" : "Puck inside the mechanism",
         #"EV_LIFT_GO_POSITION" : "",
         #"EV_LIFT_GROUND_POSITION" : "",
-        #"EV_PUCK_INSIDE" : "",
     }
 }

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 6642886b2e463154fcf16330b83a4288ba686943..9fc6892ab0ddd161bf03f004ddc3efc0e5264ec3 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -259,9 +259,15 @@ void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA:
                        //printf("Sharp distance: %f\n", robot.orte.puck_distance.distance);
-                       // FIXME F.J. (?) inform display in case of invalid data?
+                       if(robot.orte.puck_distance.distance <= 0.055) {
+                               FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+                       }
+                       if(robot.orte.puck_distance.distance < 0.13 && robot.orte.puck_distance.distance > 0.055) {
+                               FSM_SIGNAL(ACT, EV_PUCK_REACHABLE, NULL);
+                       }
                        break;
                case DEADLINE:
+                       // FIXME F.J. (?) inform display in case of invalid data?
                        // FIXME F.J. (?) limited space on the display...
                        //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
                        DBG("ORTE deadline occurred - servos receive\n");
@@ -327,13 +333,10 @@ int robot_init_orte()
                return rv;
 
        /* creating publishers */
-       robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
        robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
-       robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
        robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
        robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
-       robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
-       robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
+       //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 
        robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
        robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);