fsmjoy: modified to run with roborte.

diff --git a/src/robofsm/eb2008/fsmjoy.c b/src/robofsm/eb2008/fsmjoy.c
index 340bbfdebac595a4f8e6af1581620101abe21d01..74bfe7c708ceaae067375b18800340d414a2e6c7 100644 (file)
--- a/src/robofsm/eb2008/fsmjoy.c
+++ b/src/robofsm/eb2008/fsmjoy.c
@@ -27,39 +27,53 @@
 FSM_STATE_DECL(joy_init);
 FSM_STATE_DECL(joy_scan);
 
-FSM_STATE(joy_init) {
+FSM_STATE(joy_init)
+{
        printf("joystick control initialization\n");
        FSM_TRANSITION(joy_scan);
 }
 
-FSM_STATE(joy_scan) {
-
+FSM_STATE(joy_scan)
+{
        FSM_TIMER(100);
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        break;
-               case EV_TIMEOUT:
+               case EV_TIMER:
                        /* FIXME: servos actions come here */
-//                     if(robot.joy.button1 == 1) ;
-//                     if(robot.joy.button2 == 1) ;
-//                     if(robot.joy.button3 == 1) ;
-//                     if(robot.joy.button4 == 1) ;
-//                     if(robot.joy.button5 == 1) ;
-//                     if(robot.joy.button6 == 1) ;
-//                     if(robot.joy.button7 == 1) ;
-//                     if(robot.joy.button8 == 1) ;
-//                     if(robot.joy.button9 == 1) ;
-//                     if(robot.joy.button10 == 1) ;
-//                     if(robot.joy.button11 == 1) ;
-//                     if(robot.joy.button12 == 1) ;
+                       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");
 
 #ifdef JOYSTICK_NAV                                            // if joystick is used for navigating
                        v = (double)robot.joy.axisY/32768;
                        omega = (double)robot.joy.axisX/32768;
                        lenght = sqrt( pow(v,2) + pow(omega,2));
-                       if (lenght >= 1)                // If lenght si more than 1 is used unit circle
-                       {
-                               angle = tan2(robot.joy.axisX/robot.joy.axisY);  // computing new speed and omega
+                       /*  if lenght si more than 1 is used unit circle */
+                       if (lenght >= 1) {
+                               /* computing new speed and omega */
+                               angle = tan2(robot.joy.axisX/robot.joy.axisY);
                                v = sin(angle);
                                omega = cos(angle);
                        }