robofsm/Makefile.omk: actlib added; hokuyo init removed temporarily; robomon: blue...

diff --git a/src/hokuyo/hokuyo.c b/src/hokuyo/hokuyo.c
index a2e2f5ab53b84a17a49cb090cfe42a7956239a1d..7d8576e4766a84b346c7225048e710bb0c38c053 100644 (file)
--- a/src/hokuyo/hokuyo.c
+++ b/src/hokuyo/hokuyo.c
@@ -261,7 +261,7 @@ int main(int argc, char **argv)
   int i,pt;
 
 
-  hokuyo_init(HOKUYO_DEVICE);
+  hokuyo_init(HOKUYO_DEVICE);  // FIXME: temporarily commented out by Filip 04.04.2009: 
   // Open output file
 #if 0  
   output=fopen("hoklog","w");

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 84e503715fc6fdd3916ae5c6aeddcea366d41e63..b7b90b0a0693e191f6d7c4a0abe0ba3d7f2976f9 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -25,7 +25,7 @@ actlib_SOURCES = actuators.c
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot mcl robomath roboorte robottype robottype                \
                pthread rt m orte pathplan sharp map fsm uoled oledlib  \
-               rbtree motion robodim sercom
+               rbtree motion robodim sercom actlib
 
 # Automatic generation of event definition files
 include-pass_HOOKS = roboevent.c roboevent.h

diff --git a/src/robofsm/actuators.h b/src/robofsm/actuators.h
index bb809ac2e435376ebac8e4feb19a7f59d772bf9e..ec266306efeccaf9146c40cf6732ffab7081a14b 100644 (file)
--- a/src/robofsm/actuators.h
+++ b/src/robofsm/actuators.h
@@ -31,13 +31,6 @@ of the robot.
 #define CHELA_OPEN   0x30
 #define CHELA_LOOSE  0x80
 
-#define CHELA_LEFT_CLOSED 0x00
-#define CHELA_RIGHT_CLOSED 0xFF
-#define CHELA_LEFT_OPEN 0xD0
-#define CHELA_RIGHT_OPEN 0x30
-#define CHELA_LEFT_TIGHT 0x60
-#define CHELA_RIGHT_TIGHT 0xA0
-
 /* FIXME: (temporary, LPCs not ready) Timeout */
 #define CHELA_TIMEOUT 0x07
 

diff --git a/src/robofsm/fsmact.c b/src/robofsm/fsmact.c
index 783957963223dc9cfe5281356ef5bde10534d958..388f0ea94fe61484f4835733f68791e180132d8b 100644 (file)
--- a/src/robofsm/fsmact.c
+++ b/src/robofsm/fsmact.c
@@ -2,6 +2,7 @@
 
 #include <robot.h>
 #include <fsm.h>
+#include "actuators.h"
 
 /* ***********************************************************
  *  STATE DECLARATIONS
@@ -16,6 +17,8 @@ FSM_STATE_DECL(wait_for_command);
 FSM_STATE(wait_for_command) {
        switch (FSM_EVENT) {
        case EV_ENTRY:
+               act_belts(BELTS_OFF, BELTS_OFF);
+               act_chelae(CHELA_OPEN, CHELA_OPEN);
                // TODO
                // put actuators to defined initial positions
                break;

diff --git a/src/robofsm/fsmmain.c b/src/robofsm/fsmmain.c
index 5252d4cbaa02ccf3272dc0917a5caed278957f13..4b2d58d45b39ece2067535d2303c44a1008271a0 100644 (file)
--- a/src/robofsm/fsmmain.c
+++ b/src/robofsm/fsmmain.c
@@ -69,6 +69,7 @@ void *wait_for_end(void *arg)
 /**
  * Timer to go to tray.
  *
+ * FIXME: rename this function?
  */
 void *wait_to_deposition(void *arg)
 {
@@ -117,6 +118,7 @@ void robot_go_backward_to_point(struct ref_pos_type des_pos)
 FSM_STATE_DECL(init);
 FSM_STATE_DECL(wait_for_start);
 /* movement states */
+FSM_STATE_DECL(go_somewhere);
 
 /************************************************************************
  * INITIAL AND STARTING STATES
@@ -136,6 +138,9 @@ FSM_STATE(init)
 
 FSM_STATE(wait_for_start)
 {
+       #ifdef COMPETITON
+               printf("COMPETITION mode set");
+       #endif
        switch (FSM_EVENT) {
 #ifdef WAIT_FOR_START
                case EV_START: {
@@ -148,13 +153,14 @@ FSM_STATE(wait_for_start)
 #else
                case EV_ENTRY:
 #endif
-                       robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
-                                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-                                               DEG2RAD(-45));
+                       robot_set_est_pos_trans(ROBOT_WIDTH_M,
+                                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M - 0.1,
+                                               DEG2RAD(-90));
                        /* start to do something */
                        ROBOT_LOCK(state);
                        robot.state = COMPETITION_STARTED;
                        ROBOT_UNLOCK(state);
+                       FSM_TRANSITION(go_somewhere);
                        break;
                case EV_LASER_POWER:
                        break;
@@ -171,3 +177,31 @@ FSM_STATE(wait_for_start)
  * MOVEMENT STATES
  ************************************************************************/
 
+FSM_STATE(go_somewhere)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:  {
+                       //struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+                       //tc.maxv /= 4.0;
+                       //robot_trajectory_new(&tc);
+
+                       //robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.8, 1.0);                                      
+                       /* robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.6, 
+                                                              0.4, TURN(DEG2RAD(90))); */
+
+                       /* robot_trajectory_add_final_point_trans(ROBOT_WIDTH_M, 
+                                                              1.5, NO_TURN()); */
+                       robot_stop();
+                       FSM_TIMER(500);
+                       //robot_send_speed(0.5,0.5);
+                       }
+                       break;
+               case EV_TIMER:
+                       robot_stop();
+                       break;
+               case EV_EXIT:
+                       break;
+               default: break; // FIXME: we do not want this
+       }
+}
+

diff --git a/src/robofsm/movehelper.h b/src/robofsm/movehelper.h
index b6a9abd6b6cb46a2dd12c76d0ce6e6166a8beb65..3dd2eba3c0c5c504e360296d07f87637c3f38f7c 100644 (file)
--- a/src/robofsm/movehelper.h
+++ b/src/robofsm/movehelper.h
@@ -95,6 +95,7 @@ void robot_goto_notrans(double x, double y, struct final_heading *heading, struc
 
 void robot_move_by(double distance, struct final_heading *heading, struct TrajectoryConstraints *tc);
 
+/* FIXME: not used anywhere */
 static inline void robot_translate_coordinates(double *x, double *y, double *phi)
 {
        if (x) {

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 93f35b8de05a88076dbd994cde949e165a59c324..72e45a17706cbd9482324424f98f972396baa640 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -146,6 +146,7 @@ int robot_init()
 
        /* init ORTE domain, create publishers, subscribers, .. */
        rv = robot_init_orte();
+       act_init(&robot.orte);
 
        return rv;
 }

diff --git a/src/robofsm/test/Makefile.omk b/src/robofsm/test/Makefile.omk
index fd0cbcb63b9f88965ef8c256b25343a5d314bb4c..8234762e7b1764a7ef758aba1c824db246ea10fa 100644 (file)
--- a/src/robofsm/test/Makefile.omk
+++ b/src/robofsm/test/Makefile.omk
@@ -30,5 +30,5 @@ sharpcalib_SOURCES = sharpcalib.cc
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot mcl robodim laser-nav robomath uoled oledlib     \
                sercom roboorte robottype pthread rt m orte pathplan    \
-               sharp map fsm rbtree motion
+               sharp map fsm rbtree motion actlib
 

diff --git a/src/robomon/PlaygroundScene.cpp b/src/robomon/PlaygroundScene.cpp
index f62c3b94a2dcc8eb89f8571f2ada5813fbffe9b2..853df375522230e6ff9bc92c170eee068ea69063 100644 (file)
--- a/src/robomon/PlaygroundScene.cpp
+++ b/src/robomon/PlaygroundScene.cpp
@@ -85,9 +85,9 @@ void PlaygroundScene::createPlaygroundEurobot2007()
        tempRect->setZValue(1);
        
        /* blue starting area */
-       tempRect = addRect(QRectF(11, 1, 100, 100), QPen(QBrush(Qt::blue),
+       tempRect = addRect(QRectF(11, 1, 100, 100), QPen(QBrush(Qt::green),
                                1, Qt::SolidLine, Qt::FlatCap, Qt::BevelJoin),
-                               QBrush(Qt::blue));
+                               QBrush(Qt::green));
        tempRect->setZValue(3);
        
        /* red starting area */

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index 59c6ad2cd3e384d6fb52cbbed54ea7422bb3111c..8aa3a96aa67e276ef2b14526c0ca4df2e0856962 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -497,8 +497,7 @@ void RobomonAtlantis::setDO(int state)
 
 void RobomonAtlantis::pick()
 {
-       act_picker(CHELA_LEFT_TIGHT, CHELA_RIGHT_TIGHT, BELTS_OUT, BELTS_OUT, 
-                       CHELA_TIMEOUT);
+       // TODO: send signal to the fsmact
 }
 
 void RobomonAtlantis::setBelts(int value)