while(1) {
timer.tv_sec = 0;
- timer.tv_nsec = 2000000;
+ timer.tv_nsec = 100000000;
while(read(joy_fd, &js, sizeof(struct js_event))>0) {
switch (js.type & ~JS_EVENT_INIT) {
#SUBDIRS = test
-default_CONFIG = CONFIG_OPEN_LOOP=n
+#default_CONFIG = CONFIG_OPEN_LOOP=n
LOCAL_CONFIG_H = robot_config.h
#bin_PROGRAMS += robomain
robot_exit();
}
+void robot_loc_init()
+{
+ /* MCL initialization */
+ robot.mcl.width = PLAYGROUND_WIDTH_M; /* in meters */
+ robot.mcl.height = PLAYGROUND_HEIGHT_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 = 2000;
+ 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);
+}
+
/**
* Initializes the robot.
* Setup fields in robot structure, initializes FSMs and ORTE.
{
int i;
+ /* robot and competition configuration */
robot.mode = ROBO_TESTING;
+ robot.team_color = BLUE;
+ robot.beacon_color = BEACON_BLUE;
pthread_mutex_init(&robot.lock, NULL);
pthread_mutex_init(&robot.lock_act_pos, NULL);
signal(SIGINT, int_handler);
block_signals();
+ /* initial values */
robot.gorte.motion_speed.left = 0;
robot.gorte.motion_speed.right = 0;
robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
robot.orte.drives.bagr = BAGR_DRIVE_OFF;
+ /* localizaton initialization */
+ robot_loc_init();
+
+ /* init ORTE domain, create publishers, subscribers, .. */
robot_init_orte();
+ /* just for easier manipulation with this variables */
robot.act_pos = &robot.gorte.act_pos;
robot.est_pos = &robot.gorte.est_pos;
return 0;
}
-
#define __robot_lock_act_pos lock_act_pos
#define __robot_lock_est_pos lock
#define __robot_lock_des_pos lock
+#define __robot_lock_mcl lock_mcl
#define __robot_lock_goal lock
#define __robot_lock_IRsensors lock
#define __robot_lock_joy_data lock
unsigned char mode;
pthread_mutex_t lock;
pthread_mutex_t lock_act_pos;
+ pthread_mutex_t lock_mcl;
/* competition parameters */
unsigned char team_color;
+ unsigned char beacon_color;
/** Temporary storage for new trajectory. After the trajectory creation is
* finished, this trajectory is submitted to fsmmove. */
struct generic_orte_data gorte;
struct motion_irc_type odometry;
+
+ /* mcl */
+ struct mcl_model mcl;
+ struct mcl_angles meas_angles;;
+
}; /* robot_eb2008 */
extern struct robot_eb2008 robot;
#include <robot_eb2008.h>
#include <movehelper_eb2008.h>
#include <math.h>
+#include <laser-nav.h>
/* ----------------------------------------------------------------------
* PUBLISHER CALLBACKS - GENERIC
* ---------------------------------------------------------------------- */
-void send_act_pos_cb(const ORTESendInfo *info, void *vinstance,
- void *sendCallBackParam)
-{
-}
-
-void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
- void *sendCallBackParam)
-{
-}
-
void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
/* aktualni uhel */
aktUhel += deltAlfa;
- double x, y, phi;
+ double x, y, phi, dx, dy;
+
+ dx = deltaU * cos(aktUhel);
+ dy = deltaU * sin(aktUhel);
+
ROBOT_LOCK(act_pos);
- x = robot.act_pos->x + deltaU * cos(aktUhel);
- y = robot.act_pos->y + deltaU * sin(aktUhel);
+ 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);
+
+ ROBOT_LOCK(mcl);
+ robot.mcl.move(&robot.mcl, dx, dy, deltAlfa);
+ ROBOT_UNLOCK(mcl);
+
break;
case DEADLINE:
printf("ORTE deadline occurred - motion_irc receive\n");
void rcv_laser_meas_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct laser_meas_type *laser_meas =
- (struct laser_meas_type *)vinstance;
+ static unsigned int times[LAS_CNT];
+ static struct mcl_particle est_pos;
+ static int i;
switch (info->status) {
case NEW_DATA:
ROBOT_LOCK(laser_meas);
- robot.orte.laser_meas = *laser_meas;
+ robot.meas_angles.count = robot.orte.laser_meas.cnt + 1;
+ times[0] = robot.orte.laser_meas.period;
+ times[1] = robot.orte.laser_meas.measures0;
+ times[2] = robot.orte.laser_meas.measures1;
+ times[3] = robot.orte.laser_meas.measures2;
+ times[4] = robot.orte.laser_meas.measures3;
+ times[5] = robot.orte.laser_meas.measures4;
+ times[6] = robot.orte.laser_meas.measures5;
+ times[7] = robot.orte.laser_meas.measures6;
+ times[8] = robot.orte.laser_meas.measures7;
+ times[9] = robot.orte.laser_meas.measures8;
+ times[10] = robot.orte.laser_meas.measures9;
ROBOT_UNLOCK(laser_meas);
+
+ for (i=0; i<robot.meas_angles.count; i++)
+ robot.meas_angles.val[i] =
+ (double)TIME2ANGLE(times[0],times[i+1]);
+
+ ROBOT_LOCK(mcl);
+ robot.mcl.update(&robot.mcl, robot.mcl.data);
+ robot.mcl.normalize(&robot.mcl);
+ robot.mcl.sort(&robot.mcl);
+ robot.mcl.resample(&robot.mcl);
+ ROBOT_LOCK(mcl);
+
+ est_pos = mcl_get_pos(&robot.mcl);
+
+ ROBOT_LOCK(est_pos);
+ robot.gorte.est_pos.x = est_pos.x;
+ robot.gorte.est_pos.y = est_pos.y;
+ robot.gorte.est_pos.phi = est_pos.angle;
+ ROBOT_UNLOCK(est_pos);
+
break;
case DEADLINE:
printf("ORTE deadline occurred - laser_meas receive\n");
generic_publisher_motion_irc_create(&robot.gorte, NULL, &robot.gorte);
generic_publisher_motion_speed_create(&robot.gorte, NULL, &robot.gorte);
generic_publisher_motion_status_create(&robot.gorte, NULL, &robot.gorte);
- generic_publisher_act_pos_create(&robot.gorte, send_act_pos_cb, &robot.gorte);
- generic_publisher_est_pos_create(&robot.gorte, send_est_pos_cb, &robot.gorte);
+ generic_publisher_act_pos_create(&robot.gorte, send_dummy_cb, &robot.gorte);
+ generic_publisher_est_pos_create(&robot.gorte, send_dummy_cb, &robot.gorte);
generic_publisher_joy_data_create(&robot.gorte, NULL, &robot.gorte);
generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);
countMeasuredAnglesFrequency();
measuredAnglesWidget->animate();
break;
+ case QEVENT(QEV_MOTION_IRC):
+ emit motionIrcReceivedSignal();
+ break;
default:
if (event->type() == QEvent::KeyPress)
keyPressEvent((QKeyEvent *)event);
**********************************************************************/
void SimMcl::createOrte()
{
+ orte_generic.strength = 12;
orte_eb2008.strength = 12;
+ generic_roboorte_init(&orte_generic);
eb2008_roboorte_init(&orte_eb2008);
- eb2008_subscriber_laser_meas_create(&orte_eb2008, receiveLaserCallBack, this);
+ generic_subscriber_motion_irc_create(&orte_generic,
+ rcv_motion_irc_cb, this);
+ eb2008_subscriber_laser_meas_create(&orte_eb2008,
+ receiveLaserCallBack, this);
/* set actions to do when we receive data from orte */
connect(this, SIGNAL(laserDataReceivedSignal()),
this, SLOT(laserReceived()));
+ connect(this, SIGNAL(motionIrcReceivedSignal()),
+ this, SLOT(updateOdometry()));
}
void SimMcl::laserReceived()
unsigned int times[LAS_CNT];
int cnt;
- WDBG("ORTE received: laser");
+// WDBG("ORTE received: laser");
cnt = orte_eb2008.laser_meas.cnt;
times[0] = orte_eb2008.laser_meas.period;
updateMcl();
}
+void SimMcl::updateOdometry()
+{
+ static struct motion_irc_type prev_irc;
+ static int firstRun = 1;
+ /* 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 aktk0 = 0;
+ double aktk1 = 0;
+ double deltaU = 0;
+ double dk0 = 0;
+ double dk1 = 0;
+ double deltAlfa = 0;
+ static double k0 = 0;
+ static double k1 = 0;
+
+// WDBG("ORTE received: motion_irc");
+
+ if(firstRun) {
+ prev_irc = orte_generic.motion_irc;
+ firstRun = 0;
+ }
+
+ aktk0 = (prev_irc.left - orte_generic.motion_irc.left) >> 8;
+ aktk1 = (orte_generic.motion_irc.right - prev_irc.right) >> 8;
+ prev_irc = orte_generic.motion_irc;
+ dk0 = aktk0;
+ dk1 = aktk1;
+
+ k0 = aktk0;
+ k1 = aktk1;
+
+ dk0 *= c;
+ dk1 *= c;
+ deltaU = (dk0 + dk1) / 2;
+
+ /* dk1 - dk0 */
+ deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);
+
+ double dx, dy;
+
+ dx = deltaU;
+ dy = deltaU;
+
+ if (dx != 0 || dy != 0 || deltAlfa != 0) {
+ QString dbg = QString("dx=%1 dy=%2 dphi=%3 ")
+ .arg(dx, 0, 'f', 3)
+ .arg(dy, 0, 'f', 3)
+ .arg(RAD2DEG(deltAlfa), 0, 'f', 2);
+ WDBG(dbg);
+
+ mcl.move(&mcl, dx, dy, deltAlfa);
+ }
+}
+
/**********************************************************************
* PLN - Passive Laser Navigation
**********************************************************************/
mcl.cycle++;
/* MCL */
- mcl.move(&mcl, 0.00, 0.00, 0.00);
+// mcl.move(&mcl, 0.00, 0.00, 0.00);
mcl.update(&mcl, mcl.data);
mcl.normalize(&mcl);
mcl.sort(&mcl);
#include "AnglesHistogramPainter.h"
#include "robomon_orte.h"
#include <roboorte_eb2008.h>
+#include <roboorte_generic.h>
#define OPENGL_DEFAULT false
#define ALIASING_DEFAULT true
signals:
void laserDataReceivedSignal();
void updateMclSignal();
+ void motionIrcReceivedSignal();
private slots:
/************************************************************
* ORTE
************************************************************/
void laserReceived();
+ void updateOdometry();
private:
/************************************************************
************************************************************/
void createOrte();
+ struct generic_orte_data orte_generic;
struct eb2008_orte_data orte_eb2008;
/************************************************************
}
}
+void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
+ void *arg)
+{
+ switch (info->status) {
+ case NEW_DATA:
+ POST_QEVENT(arg, QEV_MOTION_IRC);
+ break;
+ case DEADLINE:
+ printf("motion_irc deadline occurred\n");
+ break;
+ }
+}
+
enum {
QEV_MOTION_STATUS = 0,
+ QEV_MOTION_IRC,
QEV_ACTUAL_POSITION,
QEV_ESTIMATED_POSITION,
QEV_SHARP_LONGS,
#define MOTOR_LIMIT 10000.0
-/* ORTE data */
-struct orte_data {
- ORTEDomain *orteDomain;
-
- /* publishers */
- ORTEPublication *publisherPowerControl;
- ORTEPublication *publisherMotor;
- ORTEPublication *publisherServos;
- ORTEPublication *publisherSharpLong;
- ORTEPublication *publisherSharpShort;
- ORTEPublication *publisherDO;
-
- /* ORTE types */
- motion_status orteMotionStatus;
- motion_speed orteMotionSpeed;
- stateInnerIR orteStateInnerIR;
- sharpLongs orteSharpLongs;
- sharpShorts orteSharpShorts;
- stateServa orteServosStatus;
- struct act_pos_type orteActPos;
- struct est_pos_type orteEstPos;
- accumulator_type orteAccumulator;
- stateDigIn orteStateDI;
- laserData orteLaserData;
- pwr_ctrl ortePowerControl;
- pwr_voltage ortePowerVoltage;
-
- bool stateDO[8];
- unsigned int accelRecv[3];
-// uint16_t stateIR;
-// unsigned char stateIR2;
-// short int motorSpeedCmd[2];
-// uint16_t sharpToRecv[4];
-
- /* used to simulate sensors */
- sharpLongs orteSharpOpponent;
- sharpShorts orteSharpWaste;
-};
-
enum {
BOTTLE = 0,
CAN,
UNKNOWN
};
-/* orte initialization */
-int robomon_orte_init(void *arg, struct orte_data *data);
-/* function to create orte subscriber/publisher */
-/* publishers */
-void createMotorPublisher(void *arg, struct orte_data *data);
-void createServoPublisher(void *arg, struct orte_data *data);
-void createSharpLongsPublisher(void *arg, struct orte_data *data);
-void createSharpShortsPublisher(void *arg, struct orte_data *data);
-void createDOPublisher(void *arg, struct orte_data *data);
-void createPowerControlPublisher(void *arg, struct orte_data *data);
-/* subscribers */
-void createMotionStatusSubscriber(void *arg, struct orte_data *data);
-void createActualPositionSubscriber(void *arg, struct orte_data *data);
-void createEstimatedPositionSubscriber(void *arg, struct orte_data *data);
-void createSharpLongsSubscriber(void *arg, struct orte_data *data);
-void createSharpShortsSubscriber(void *arg, struct orte_data *data);
-void createIRSubscriber(void *arg, struct orte_data *data);
-void createDISubscriber(void *arg, struct orte_data *data);
-void createLaserSubscriber(void *arg, struct orte_data *data);
-void createAccelerometerSubscriber(void *arg, struct orte_data *data);
-void createAccumulatorSubscriber(void *arg, struct orte_data *data);
-void createPowerVoltageSubscriber(void *arg, struct orte_data *data);
-
/* callback functions */
/* publishers */
void dummy_publisher_callback(const ORTESendInfo *info,
void *vinstance, void *arg);
void receivePowerVoltageCallBack(const ORTERecvInfo *info,
void *vinstance, void *arg);
+void rcv_motion_irc_cb(const ORTERecvInfo *info,
+ void *vinstance, void *arg);
#endif /* ROBOMON_ORTE_H */
projects / eurobot / public.git / commitdiff