Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
59 lines (45 sloc) 1.27 KB
#ifndef __NAVIGATION_H_
#define __NAVIGATION_H_
#include "NXCDefs.h"
#include "position_t.h"
#define NAVIGATION_DELAY 200
position_t absolute_position;
mutex navigation_mutex;
void update_navigation( )
{
float SCALE_XY = 0.01;
float SCALE_PHI = (PI/180);
string msg;
int char_pos;
string substring_x, substring_y, substring_phi;
string robot_id;
// get message from bluetooth stack
ReceiveMessage(0, TRUE, msg);
// get robot id
char_pos = Pos(";", msg);
robot_id = SubStr(msg, 0, char_pos);
msg = SubStr(msg, char_pos+1, StrLen(msg));
// get x
char_pos = Pos(";", msg);
substring_x = SubStr(msg, 0, char_pos);
msg = SubStr(msg, char_pos+1, StrLen(msg));
// get y
char_pos = Pos(";", msg);
substring_y = SubStr(msg, 0, char_pos);
msg = SubStr(msg, char_pos+1, StrLen(msg));
// get theta
substring_phi = SubStr(msg, 0, StrLen(msg));
Acquire(navigation_mutex);
absolute_position.x = SCALE_XY*StrToNum(substring_x);
absolute_position.y = SCALE_XY*StrToNum(substring_y);
absolute_position.phi = SCALE_PHI*StrToNum(substring_phi);
Release(navigation_mutex);
}
task navigation ()
{
while(1) {
update_navigation( );
Wait(NAVIGATION_DELAY);
}
}
#endif