Localization: laser-nav has been tested. Works well, needs to solve some problem.

Need to be tested with the MCL.

Robomon: robot with different colors, used to display the actual and estimated position in different colors.

diff --git a/app/laser-nav/lasernav/laser-nav.c b/app/laser-nav/lasernav/laser-nav.c
index e10faf92fde44507a12f81ec45d08d471d16a2b9..17eb7c6da1850484f0a9bc416b3f6e0412b36576 100644 (file)
--- a/app/laser-nav/lasernav/laser-nav.c
+++ b/app/laser-nav/lasernav/laser-nav.c
@@ -707,6 +707,11 @@ void pln_coordinate(struct pln_pos_state estpos, struct pln_aangle_state aas,
        as->alfa = __angles[0];
        as->beta = __angles[1];
        as->gama = __angles[2];
+
+       /* FIXME: used just to test, remove it later */
+       as->alfa = _as.alfa;
+       as->beta = _as.beta;
+       as->gama = _as.gama;
 }
 
 /**
@@ -764,6 +769,17 @@ void pln_sel_angles(struct pln_pos_state estpos, double *angles,
        double eval1, eval2, sgn;
        int i, j;
 
+       /* FIXME: used just to test the calculation, remove this after the test */
+       if (angcount != 3) {
+               printf("the number of angles is not equal 3!! (angles selection is not used by now)\n");
+       } else {
+               maas->theta[0] = angles[0];
+               maas->theta[1] = angles[1];
+               maas->theta[2] = angles[2];
+       }
+       return;
+
+
        /* convert the estimated position to angles. Angles are absolute to 
         * the robot`s head */
        pln_pos2ang(estpos, &aas);

diff --git a/app/laser-nav/lasernav/laser-nav.h b/app/laser-nav/lasernav/laser-nav.h
index 28a96a9faf1d346591c335fc5312a881bf78ba9d..e09b88482f5138e845a0807c254b5796ec2d27dc 100644 (file)
--- a/app/laser-nav/lasernav/laser-nav.h
+++ b/app/laser-nav/lasernav/laser-nav.h
@@ -58,15 +58,15 @@
  */
 
 /* verbose level 0-3 */
-/*#ifndef DEBUG
+#ifndef DEBUG
 #define DEBUG 1
-#endif*/
+#endif
 
 /**
  * Program parameters
  */
 /*define this macro to use red reflectors instead of default blue reflectors*/
-/*#define USE_RED_REFLECTORS*/
+// #define USE_RED_REFLECTORS
 
 /** 
  * Playground size and known points

diff --git a/app/robofsm/fsmloc.c b/app/robofsm/fsmloc.c
index 10bfcad5c746f1e9b8a4983e96ad637af2785a3d..f24fd386c98cfd795f6b11d13288d53745a17c05 100644 (file)
--- a/app/robofsm/fsmloc.c
+++ b/app/robofsm/fsmloc.c
@@ -17,6 +17,7 @@
 #include "robot.h"
 #include <fsm.h>
 
+struct mcl_particle cal_pos;
 void loc_pln_update();
 int loc_mcl_update(struct mcl_model *mcl, double dx, double dy, double dangle);
 void dbg_print_parts();
@@ -97,15 +98,13 @@ FSM_STATE(loc_run)
 {
 //     DBG_PRINT_EVENT("loc_run event:");
 
-       FSM_TIMEOUT(1000);
+//     FSM_TIMEOUT(1000);
        switch (FSM_EVENT) {
                case EV_LOC_INIT:
                        /*DBG_PRINT_EVENT("mcl_init event:");*/
                        FSM_TRANSITION(loc_mcl_init);
                        break;
-               case EV_TIMEOUT:
-//                     FIXME: dont forget to uncomment the break expression bellow
-//                     when the laser works.
+//             case EV_TIMEOUT:
 //                     DBG_PRINT_EVENT("timeout event:");
 //                     break;
                case EV_LOC_LOCATE:
@@ -140,15 +139,15 @@ FSM_STATE(loc_update)
        loc_pln_update();
 
        /* update mcl with measured position */
-       rv = loc_mcl_update(&robot.mcl, (double)dx, (double)dy, (double)dangle);
-
-       /* send update signal if mcl update has finished successful */
-       if (!rv)
-               FSM_SIGNAL(MOTION, EV_LOC_UPDATED);
-       else {
-               printf("MCL_RESET: LOST!\n");
-               FSM_SIGNAL(MOTION, EV_LOC_LOST);
-       }
+//     rv = loc_mcl_update(&robot.mcl, (double)dx, (double)dy, (double)dangle);
+
+//     /* send update signal if mcl update has finished successful */
+//     if (!rv)
+//             FSM_SIGNAL(MOTION, EV_LOC_UPDATED);
+//     else {
+//             printf("MCL_RESET: LOST!\n");
+//             FSM_SIGNAL(MOTION, EV_LOC_LOST);
+//     }
 
        SUBFSM_RET();
 }
@@ -161,6 +160,7 @@ void loc_pln_update()
        struct pln_pos_state _act_pos;
        struct pln_pos_state _cal_pos;
        unsigned int times[LAS_CNT], cnt;
+       int i;
 
        /* measured times from laser beacon */
        ROBOT_LOCK(laser_recv);
@@ -188,35 +188,41 @@ void loc_pln_update()
        times[10] = robot.laser_recv.measures9;
        ROBOT_UNLOCK(laser_recv);
 
-//     printf("cnt=%d period=%d measures: ", 
-//             robot.laser_recv.cnt, robot.laser_recv.period);
-//     printf("times:");
-//     for (i=0; i<cnt; i++)
-//             printf("%d ", times[i]);
-//     printf("\n");
+       printf("\n");
+       printf("cnt=%d period=%d measures: ", 
+               robot.laser_recv.cnt, robot.laser_recv.period);
+       printf("times:");
+       for (i=1; i<cnt; i++)
+               printf("%d ", times[i]);
+       printf("\n");
 
        /* actual position */
-       /*ROBOT_LOCK(act_pos);
-       _act_pos.x = robot.act_pos.x;
-       _act_pos.y = robot.act_pos.y;
+       ROBOT_LOCK(act_pos);
+       _act_pos.x = robot.act_pos.x * 1000;
+       _act_pos.y = robot.act_pos.y * 1000;
        _act_pos.head = robot.act_pos.phi;
-       ROBOT_UNLOCK(act_pos);*/
+       ROBOT_UNLOCK(act_pos);
        /* FIXME: this stays here just until the odometry will be done */
-       _act_pos.x = 500;
-       _act_pos.y = 1600;
-       _act_pos.head = 0;
+//     _act_pos.x = 2000;
+//     _act_pos.y = 1950;
+//     _act_pos.head = DEG2RAD(0);
 //     printf("act: x=%f y=%f h=%f\n", _act_pos.x, _act_pos.y, _act_pos.head);
 
        /* calculated measured position */
        pln_cal_position(times, cnt, _act_pos, &_cal_pos);
-//     printf("calculated posititon: [x=%f,y=%f]\n", _cal_pos.x, _cal_pos.y);
+       printf("calculated posititon: [x=%f,y=%f]\n", _cal_pos.x, _cal_pos.y);
 
        /* set currently calculated position */
+       /* FIXME: remove this when the mcl is used */
        ROBOT_LOCK(est_pos);
-       robot.est_pos.x = _cal_pos.x;
-       robot.est_pos.y = _cal_pos.y;
+       robot.est_pos.x = _cal_pos.x / 1000;
+       robot.est_pos.y = _cal_pos.y / 1000;
        robot.est_pos.phi = _cal_pos.head;
        ROBOT_UNLOCK(est_pos);
+
+       cal_pos.x = _cal_pos.x;
+       cal_pos.y = _cal_pos.y;
+       cal_pos.angle = _cal_pos.head;
 }
 
 /**
@@ -230,14 +236,6 @@ void loc_pln_update()
 int loc_mcl_update(struct mcl_model *mcl, double dx, double dy, double dangle)
 {
        struct mcl_particle est_pos;
-       struct mcl_particle cal_pos;
-
-       /* get calculated position by laser */
-       ROBOT_LOCK(est_pos);
-       cal_pos.x = robot.est_pos.x;
-       cal_pos.y = robot.est_pos.y;
-       cal_pos.angle = robot.est_pos.phi;
-       ROBOT_UNLOCK(est_pos);
 
        mcl->cycle++;
 //     printf("loc_mcl_update: cycle = %d\n", mcl->cycle);

diff --git a/app/robofsm/joystick.cc b/app/robofsm/joystick.cc
index 2456d46199de0a0fc01c736aca243e9ef7db694a..3c7f91c5a4a1cc148cfd5f02eaa294888a4d6747 100644 (file)
--- a/app/robofsm/joystick.cc
+++ b/app/robofsm/joystick.cc
@@ -36,9 +36,12 @@ FSM_STATE(main_init)
        switch (FSM_EVENT) {
                case EV_INIT:
                case EV_START:
-                       robot_set_act_pos_trans(ROBOT_WIDTH_M/2,
-                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-                               DEG2RAD(-90));
+//                     robot_set_act_pos_trans(ROBOT_WIDTH_M/2,
+//                             PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
+//                             DEG2RAD(-90));
+                       robot_set_act_pos_trans(0.0,
+                               2.0,
+                               DEG2RAD(0));
                        FSM_TIMER(100);
                        break;
                case EV_TIMER:
@@ -54,7 +57,7 @@ int main()
        /* robot initialization */
        robot_init();
 
-       FSM(MAIN)->debug_states = 1;
+//     FSM(MAIN)->debug_states = 1;
 
        robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
        robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;

diff --git a/app/robofsm/roboorte.c b/app/robofsm/roboorte.c
index d8424a4f89d39afa9570876d73c91dd74c596cc3..68ebeaaabd03e3be0a2e0743fd8c2e1af61358e2 100644 (file)
--- a/app/robofsm/roboorte.c
+++ b/app/robofsm/roboorte.c
@@ -40,7 +40,7 @@ static void recvCallBackLaser(const ORTERecvInfo *info,void *vinstance, void *re
                        //robot.laser_recv.period = ((struct laserData_type *)vinstance)->measures[0];
                        //memcpy(&robot.laser_recv.measures, instance->measures, sizeof(short)*10);
                        robot.laser_recv = *instance;
-                       printf("laser data received\n");
+                       //printf("laser data received\n");
                        ROBOT_UNLOCK(laser_recv);
                        FSM_SIGNAL(LOC, EV_LASER_RECEIVED);
                        break;

diff --git a/app/robomon/src/robomon.cpp b/app/robomon/src/robomon.cpp
index c4dcc0b2ff676c2246fcacf543b36c02bc14cf97..e084eb76a307813cd5519f44c1351633c350acd0 100644 (file)
--- a/app/robomon/src/robomon.cpp
+++ b/app/robomon/src/robomon.cpp
@@ -242,9 +242,9 @@ robomon::robomon(QWidget* parent, Qt::WFlags fl)
        connect(this->cbObstackle, SIGNAL(stateChanged(int)), this->scene, SLOT(showObstacle(int)));
        connect(this->scene, SIGNAL(obstacleChanged(QPointF)), this, SLOT(changeObstacle(QPointF)));
 
-       rob = new robot(NULL, scene);
+       rob = new robot(NULL, scene, 0);
        rob->setZValue(10);
-       est_rob = new robot(NULL, scene);
+       est_rob = new robot(NULL, scene, 1);
        est_rob->setZValue(10);
        
        scene->addItem(rob);

diff --git a/app/robomon/src/robot.cpp b/app/robomon/src/robot.cpp
index 841808e2e0c543de83dd2519ff6594a6a1cfebe9..d81e345b83b158c33692891fdc04193de45e81d8 100644 (file)
--- a/app/robomon/src/robot.cpp
+++ b/app/robomon/src/robot.cpp
@@ -4,7 +4,7 @@
 #include <QPen>
 #include "playgroundscene.h"
 
-robot::robot(QGraphicsItem *parent, QGraphicsScene *scene) : QGraphicsItem(parent)
+robot::robot(QGraphicsItem *parent, QGraphicsScene *scene, int color) : QGraphicsItem(parent)
 {
        actPhi = M_PI/2;
        
@@ -12,7 +12,23 @@ robot::robot(QGraphicsItem *parent, QGraphicsScene *scene) : QGraphicsItem(paren
 
        this->robotBody = new QGraphicsRectItem(QRectF(OFFSET_X,OFFSET_Y,ROBOT_SIZE_X,ROBOT_SIZE_Y),parent,scene);
        this->robotBody->setPen(QPen(QBrush(Qt::black),1,Qt::SolidLine,Qt::FlatCap,Qt::BevelJoin));
-       this->robotBody->setBrush(QBrush(Qt::black));
+       switch (color) {
+               case 0:
+                       this->robotBody->setBrush(QBrush(Qt::black));
+                       break;
+               case 1:
+                       this->robotBody->setBrush(QBrush(Qt::green));
+                       break;
+               case 2:
+                       this->robotBody->setBrush(QBrush(Qt::yellow));
+                       break;
+               case 3:
+                       this->robotBody->setBrush(QBrush(Qt::gray));
+                       break;
+               default:
+                       this->robotBody->setBrush(QBrush(Qt::black));
+                       break;
+       }
        this->robotBody->setParentItem(this);
        this->robotBody->setZValue(10);         
                        ;

diff --git a/app/robomon/src/robot.h b/app/robomon/src/robot.h
index f5e72aa7321168480bae3b81fcb8905c4cec627d..312a7d66531bcb43d869d1935ca5e55ca75b6bd6 100644 (file)
--- a/app/robomon/src/robot.h
+++ b/app/robomon/src/robot.h
@@ -36,7 +36,7 @@ class robot : public QGraphicsItem
 //Q_OBJECT
 public:
        QGraphicsScene *scene;
-       robot(QGraphicsItem *parent = 0, QGraphicsScene *scene = 0);
+       robot(QGraphicsItem *parent = 0, QGraphicsScene *scene = 0, int color = 0);
        
        ~robot();
        //robotSensor *sens;

diff --git a/build/h8mirosot/config.omk b/build/h8mirosot/config.omk
index 13227c1181823f7be0d4d63e8c836d340aad9b8e..44ab0c5befa3ea49f564f69fd1dd1ff3c10e8866 100644 (file)
--- a/build/h8mirosot/config.omk
+++ b/build/h8mirosot/config.omk
@@ -18,7 +18,8 @@ OPTIMIZE ?= -O2
 
 -include $(MAKERULES_DIR)/config.tohit
 HIT_BAUD ?= 57600
-HIT_DEV ?= /dev/ttyS0
+#HIT_DEV ?= /dev/ttyS0
+HIT_DEV ?= /dev/ttyUSB0
 TOHIT = $(MAKERULES_DIR)/$(COMPILED_DIR_NAME)/bin-utils/tohit --baud $(HIT_BAUD) --sdev $(HIT_DEV)
 LOAD_CMD-boot = \
        $(TOHIT) --erase --start 0x000000 --length 0x1600; \