robofsm: joystick fsm, ORTEs updated.

diff --git a/src/robofsm/eb2008/fsmjoy.c b/src/robofsm/eb2008/fsmjoy.c
index 74bfe7c708ceaae067375b18800340d414a2e6c7..666a6682ed32f583f65a657e7f5ba740aa115fbe 100644 (file)
--- a/src/robofsm/eb2008/fsmjoy.c
+++ b/src/robofsm/eb2008/fsmjoy.c
@@ -24,12 +24,19 @@
 
 //#define JOYSTICK_NAV
 
+#define JOY_BUTTONS    12
+static unsigned char buttons[JOY_BUTTONS];
+
 FSM_STATE_DECL(joy_init);
 FSM_STATE_DECL(joy_scan);
 
 FSM_STATE(joy_init)
 {
+       int i;
+
        printf("joystick control initialization\n");
+       for(i=0; i<JOY_BUTTONS; i++)
+               buttons[i] = 0;
        FSM_TRANSITION(joy_scan);
 }
 
@@ -40,31 +47,55 @@ FSM_STATE(joy_scan)
                case EV_STATE_ENTERED:
                        break;
                case EV_TIMER:
-                       /* FIXME: servos actions come here */
-                       if (robot.gorte.joy_data.button1 == 1)
-                               printf("button1\n");
-                       if (robot.gorte.joy_data.button2 == 1)
-                               printf("button2\n");
-                       if (robot.gorte.joy_data.button3 == 1)
-                               printf("button3\n");
-                       if (robot.gorte.joy_data.button4 == 1)
-                               printf("button4\n");
-                       if (robot.gorte.joy_data.button5 == 1)
-                               printf("button5\n");
-                       if (robot.gorte.joy_data.button6 == 1)
-                               printf("button6\n");
-                       if (robot.gorte.joy_data.button7 == 1) 
-                               printf("button7\n");
-                       if (robot.gorte.joy_data.button8 == 1)
-                               printf("button8\n");
-                       if (robot.gorte.joy_data.button9 == 1)
-                               printf("button9\n");
-                       if (robot.gorte.joy_data.button10 == 1)
-                               printf("button10\n");
-                       if (robot.gorte.joy_data.button11 == 1)
-                               printf("button11\n");
-                       if (robot.gorte.joy_data.button12 == 1)
-                               printf("button12\n");
+                       if (robot.gorte.joy_data.button1 == 1) {
+                               buttons[0] = ~buttons[0];
+                               printf("buttons[0]=%d\n", buttons[0]);
+                               if (buttons[0])
+                                       open_back_door();
+                               else
+                                       close_back_door();
+                       }
+                       if (robot.gorte.joy_data.button2 == 1) {
+                               buttons[1] = ~buttons[1];
+                       }
+                       if (robot.gorte.joy_data.button3 == 1) {
+                               buttons[2] = ~buttons[2];
+                               if (buttons[2])
+                                       open_bottom_door();
+                               else
+                                       close_bottom_door();
+                       }
+                       if (robot.gorte.joy_data.button4 == 1) {
+                               buttons[3] = ~buttons[3];
+                       }
+                       if (robot.gorte.joy_data.button5 == 1) {
+                               buttons[4] = ~buttons[4];
+                               if (buttons[4])
+                                       open_top_door();
+                               else
+                                       close_top_door();
+                       }
+                       if (robot.gorte.joy_data.button6 == 1) {
+                               buttons[5] = ~buttons[5];
+                       }
+                       if (robot.gorte.joy_data.button7 == 1)  {
+                               buttons[6] = ~buttons[6];
+                       }
+                       if (robot.gorte.joy_data.button8 == 1) {
+                               buttons[7] = ~buttons[7];
+                       }
+                       if (robot.gorte.joy_data.button9 == 1) {
+                               buttons[8] = ~buttons[8];
+                       }
+                       if (robot.gorte.joy_data.button10 == 1) {
+                               buttons[9] = ~buttons[9];
+                       }
+                       if (robot.gorte.joy_data.button11 == 1) {
+                               buttons[10] = ~buttons[10];
+                       }
+                       if (robot.gorte.joy_data.button12 == 1) {
+                               buttons[11] = ~buttons[11];
+                       }
 
 #ifdef JOYSTICK_NAV                                            // if joystick is used for navigating
                        v = (double)robot.joy.axisY/32768;

diff --git a/src/robofsm/eb2008/fsmloc.c b/src/robofsm/eb2008/fsmloc.c
index b24651c5537be3601b3ef4c6a8a2bb313d2d6e93..98da75b08f478648c0a4dd71aabbf5437da81393 100644 (file)
--- a/src/robofsm/eb2008/fsmloc.c
+++ b/src/robofsm/eb2008/fsmloc.c
@@ -18,6 +18,7 @@
 #include <fsm.h>
 
 struct mcl_model mcl;
+struct mcl_angles meas_angles;;
 
 FSM_STATE_DECL(loc_init);
 FSM_STATE_DECL(loc_run);
@@ -54,7 +55,7 @@ FSM_STATE(loc_init)
        mcl.move = mcl_move;
        /* style of particles evaluation */
        mcl.update = mcl_update2;
-       mcl.data = NULL;/* FIXME */
+       mcl.data = &meas_angles;
        mcl.beacon_cnt = 4;
        mcl.beacon_color = BEACON_BLUE;
 
@@ -101,7 +102,6 @@ FSM_STATE(loc_run)
  */
 FSM_STATE(loc_update)
 {
-       struct mcl_angles meas_angles;;
        static unsigned int times[LAS_CNT];
        static int cnt;
        static struct mcl_particle est_pos;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index d531798b75deba0acbfc465e49cd065e6f2b451d..e301c9af08621837d9de6d877a30c9dd87c0b99d 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -30,6 +30,11 @@ void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
 {
 }
 
+void send_dummy_cb(const ORTESendInfo *info, void *vinstance, 
+                       void *sendCallBackParam)
+{
+}
+
 /* ---------------------------------------------------------------------- 
  * SUBSCRIBER CALLBACKS - GENERIC
  * ---------------------------------------------------------------------- */
@@ -251,6 +256,9 @@ void rcv_laser_meas_cb(const ORTERecvInfo *info, void *vinstance,
 
 void robot_init_orte()
 {
+       robot.orte.strength = 20;
+       robot.gorte.strength = 20;
+
        eb2008_roboorte_init(&robot.orte);
        generic_roboorte_init(&robot.gorte);
 
@@ -264,6 +272,9 @@ void robot_init_orte()
        generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
        generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);
 
+       eb2008_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
+       eb2008_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
+
        /* create generic subscribers */
        generic_subscriber_motion_irc_create(&robot.gorte, rcv_motion_irc_cb, &robot.gorte);
        generic_subscriber_motion_speed_create(&robot.gorte, rcv_motion_speed_cb, &robot.gorte);