# Library with general support functions for the robot
lib_LIBRARIES += robot_eb2008
robot_eb2008_SOURCES = robot_orte.c servos_eb2008.c robot_eb2008.c \
- fsmmove.cc fsmjoy.c fsmloc.c \
+ fsmmove.cc fsmjoy.c \
movehelper_eb2008.cc fsmdisplay.cc
robot_eb2008_GEN_SOURCES = roboevent_eb2008.c
include_GEN_HEADERS += roboevent_eb2008.h
#include <robot_eb2008.h>
#include <fsm.h>
-struct mcl_model mcl;
-struct mcl_angles meas_angles;;
+struct mcl_laser l;
+struct mcl_model *mcl;
+//struct mcl_laser_measurement meas_angles;
FSM_STATE_DECL(loc_init);
FSM_STATE_DECL(loc_run);
FSM_STATE(loc_init)
{
/* MCL initialization */
- mcl.width = PLAYGROUND_WIDTH_M; /* in meters */
- mcl.height = PLAYGROUND_HEIGHT_M; /* in meters */
+ l.width = PLAYGROUND_WIDTH_M; /* in meters */
+ l.height = PLAYGROUND_HEIGHT_M; /* in meters */
/* the noises */
- mcl.gen_dnoise = 0.99;
- mcl.gen_anoise = 360;
- mcl.mov_dnoise = 0.01;
- mcl.mov_anoise = 0.02;
- mcl.w_min = 0.25;
- mcl.w_max = 2.0;
- mcl.eval_sigma = 160;
- mcl.aeval_sigma = 0.007;
- mcl.maxavdist = 0.150;
+ l.pred_dnoise = 0.01;
+ l.pred_anoise = 0.02;
+ l.aeval_sigma = 0.007;
/* bad cycles before reseting */
- mcl.maxnoisecycle = 10;
+ /* l.maxnoisecycle = 10; */
- /* amount of particles */
- mcl.count = 2000;
- mcl.parts = (struct mcl_particle *)
- malloc(sizeof(struct mcl_particle)*mcl.count);
-
- /* phases of the MCL */
- mcl.init = mcl_init;
- mcl.move = mcl_move;
- /* style of particles evaluation */
- mcl.update = mcl_update2;
- mcl.data = &meas_angles;
- mcl.beacon_cnt = 4;
- mcl.beacon_color = BEACON_BLUE;
-
- mcl.normalize = mcl_normalize;
- mcl.sort = mcl_partsort;
- mcl.resample = mcl_resample;
-
- /* cycles of enumeration */
- mcl.cycle = 0;
- mcl.noisecycle = 0;
-
- /* change flag */
- mcl.flag = MCL_RUN;
-
- /* generate particles with noises */
- mcl.init(&mcl);
+ mcl = mcl_laser_init(&l, 2000);
FSM_TRANSITION(loc_run);
}
DBG_PRINT_EVENT("loc_update:");
- mcl.update(&mcl, mcl.data);
+ mcl->update(mcl, mcl.data);
mcl.normalize(&mcl);
mcl.sort(&mcl);
mcl.resample(&mcl);
#include <servos_eb2008.h>
#include <movehelper_eb2008.h>
#include <robot_orte.h>
+#include <robomath.h>
#define THREAD_PRIO_MAIN 10
#define THREAD_PRIO_MOTION 15
void robot_loc_init()
{
/* MCL initialization */
- robot.mcl.xoff = 0.5;
- robot.mcl.xoff = 0.2;
- robot.mcl.width = 1.0; /* in meters */
- robot.mcl.height = 0.5; /* in meters */
+ robot.laser.width = PLAYGROUND_WIDTH_M; /* in meters */
+ robot.laser.height = PLAYGROUND_WIDTH_M; /* in meters */
/* the noises */
- robot.mcl.gen_dnoise = 0.99;
- robot.mcl.gen_anoise = 360;
- robot.mcl.mov_dnoise = 0.01;
- robot.mcl.mov_anoise = 0.02;
- robot.mcl.w_min = 0.25;
- robot.mcl.w_max = 2.0;
- robot.mcl.eval_sigma = 160;
- robot.mcl.aeval_sigma = 0.007;
- robot.mcl.maxavdist = 0.150;
- /* bad cycles before reseting */
- robot.mcl.maxnoisecycle = 10;
-
- /* amount of particles */
- robot.mcl.count = 1000;
- robot.mcl.parts = (struct mcl_particle *)
- malloc(sizeof(struct mcl_particle)*robot.mcl.count);
-
- /* phases of the MCL */
- robot.mcl.init = mcl_init;
- robot.mcl.move = mcl_move;
- /* style of particles evaluation */
- robot.mcl.update = mcl_update2;
- robot.mcl.data = &robot.meas_angles;
- robot.mcl.beacon_cnt = 4;
- robot.mcl.beacon_color = robot.beacon_color;
-
- robot.mcl.normalize = mcl_normalize;
- robot.mcl.sort = mcl_partsort;
- robot.mcl.resample = mcl_resample;
-
- /* cycles of enumeration */
- robot.mcl.cycle = 0;
- robot.mcl.noisecycle = 0;
-
- /* change flag */
- robot.mcl.flag = MCL_RUN;
-
- /* generate particles with noises */
- robot.mcl.init(&robot.mcl);
+ robot.laser.pred_dnoise = 0.01;
+ robot.laser.pred_anoise = DEG2RAD(2);
+ robot.laser.aeval_sigma = DEG2RAD(4);
+
+ robot.mcl = mcl_laser_init(&robot.laser, 1000);
}
/**
#include <stdint.h>
#include <stdio.h>
-#include <mcl.h>
+#include <mcl_laser.h>
#include <trgenconstr.h>
#include <robottype.h>
#include <robottype_eb2008.h>
struct generic_orte_data gorte;
/* mcl */
- struct mcl_model mcl;
- struct mcl_angles meas_angles;;
+ struct mcl_model *mcl; /* Pointer to the generic MCL model */
+ struct mcl_laser laser; /* MCL implementation for laser */
+ struct mcl_laser_measurement meas_angles;;
unsigned char use_loc;
}; /* robot_eb2008 */
/* spocitat prevodovy pomer */
double n = (double)(28.0 / 1.0);
/* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
- double c = (M_PI*2*ROBOT_WHEEL_RADIUS_MM/1000) / (n * 4*500.0);
+ double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
double aktk0 = 0;
double aktk1 = 0;
prevInstance = *instance;
deltaU = (aktk0 + aktk1) / 2;
- deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);
+ deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
- ROBOT_LOCK(act_pos);
- aktUhel = robot.act_pos.phi;
- ROBOT_UNLOCK(act_pos);
-
- /* aktualni uhel */
- aktUhel += deltAlfa;
-
- double x, y, phi, dx, dy;
-
- dx = deltaU * cos(aktUhel);
- dy = deltaU * sin(aktUhel);
-
- /*if (dx != 0 || dy != 0 || deltAlfa != 0)
- printf("dx=%f dy=%f dphi=%f\n", dx, dy, deltAlfa);*/
-
- ROBOT_LOCK(act_pos);
- x = robot.act_pos.x + dx;
- y = robot.act_pos.y + dy;
- phi = robot.act_pos.phi = aktUhel;
- ROBOT_UNLOCK(act_pos);
- robot_set_act_pos_notrans(x, y, phi);
-
- /* don't use localization */
- if (!robot.use_loc)
- break;
-
- if (fabs(dx) > 0.001 || fabs(dy) > 0.001 || fabs(deltAlfa) > 0.001) {
- ROBOT_LOCK(mcl);
- robot.mcl.move(&robot.mcl, dx, dy, deltAlfa);
- ROBOT_UNLOCK(mcl);
- }
+ struct mcl_robot_odo odo = {deltaU, 0, deltAlfa};
+ ROBOT_LOCK(mcl);
+ robot.mcl->predict(&robot.mcl, dx, dy, deltAlfa);
+ ROBOT_UNLOCK(mcl);
break;
case DEADLINE:
projects / eurobot / public.git / commitdiff