mlib - multimedia libraries Code

--- a
+++ b/bibabot.cpp
@@ -0,0 +1,275 @@
+#include <mlib.h>
+#include "bibabot.h"
+
+static const string bburl = "http://192.168.5.78/cgi/WtoPC.";
+
+void bibabot::init(const bibabot_par &par)
+{
+	_par = par;
+
+	_calibrated = false;
+	myhttp_options hopt;
+	hopt.timeout = 1;
+	_phttp = new myhttp(hopt);
+	send_sync("InitOdo");
+
+	_pbird = new laserbird;
+	while(!_pbird->read_raw()) wait(1);
+	orientation_reset();
+}
+
+void bibabot::cleanup()
+{
+	SAFE_DELETE(_pbird);
+	SAFE_DELETE(_phttp);
+}
+
+bool bibabot::send(const string &cmd)
+{
+	return _phttp->send(bburl + cmd);
+}
+
+bool bibabot::ready()
+{
+	return _phttp->ready();
+}
+
+bool bibabot::result(vector<double> &data, double *ptime)
+{
+	string data_str;
+	if(!_phttp->result(data_str, ptime)) return false;
+	string comment;
+	if(!get_data(data_str, comment, data)) return false;
+	return true;
+}
+
+bool bibabot::send(const string &cmd, vector<double> &data, double *ptime)
+{
+	string data_str;
+	if(!_phttp->send(bburl + cmd, data_str, ptime)) return false;
+	string comment;
+	if(!get_data(data_str, comment, data)) return false;
+	return true;
+}
+
+bool bibabot::send_sync(const string &cmd, double *ptime)
+{
+	return _phttp->send_sync(bburl + cmd, ptime);
+}
+
+bool bibabot::get_data(const string &in, string &comment, vector<double> &data)
+{
+	const string raw = lower_case(in);
+	string::size_type p;
+	if((p = raw.find("<body")) == string::npos) return false;
+	const string::size_type p_body_tag = p + 5;
+	if((p = raw.find(">", p_body_tag)) == string::npos) return false;
+	const string::size_type p_body = p + 1;
+	if((p = raw.find("<pre>", p_body)) == string::npos) return false;
+	const string::size_type p_begin = p + 5;
+	if((p = raw.find("</pre>", p_begin)) == string::npos) return false;
+	const string::size_type p_end = p;
+	const string reply = raw.substr(p_begin, p_end - p_begin);
+
+	if((p = reply.find(":")) == string::npos) return false;
+	const string::size_type p_colon = p;
+	comment = reply.substr(0, p);
+	if((p = reply.find("%", p_colon + 1)) == string::npos) return false;
+	const string::size_type p_data_begin = p + 1;
+	if((p = reply.find("%", p_data_begin)) == string::npos) return false;
+	const string::size_type p_data_end = p - 1;
+	const string data_string = reply.substr(p_data_begin, p_data_end - p_data_begin);
+	istringstream data_stream(data_string);
+	data.clear();
+	for(int i = 0; i < 16; i++) {
+		double x;
+		data_stream >> x;
+		data.push_back(x);
+		if(!data_stream) return false;
+	}
+
+	return true;
+}
+
+bool bibabot::is_moving(bool *pmove)
+{
+	vector<double> data;
+	for(int i = 0; i < 5; i++) {
+		if(send("SetMotor", data)) {
+			if(pmove) *pmove = (data[6] != 0);
+			return true;
+		}
+	}
+	return false;
+}
+
+void bibabot::wait_until_stop()
+{
+	bool moving;
+	while(true) {
+		if(is_moving(&moving)) {
+			if(!moving) return;
+		}
+	}
+}
+
+double bibabot::robot_pos()
+{
+	vector<double> data;
+	bool ok = false;
+	for(int i = 0; i < _par.max_connection_attempts; i++) {
+		if(send("SetMotor", data)) {
+			ok = true;
+			break;
+		}
+	}
+	if(ok) {
+		return data[11]*100;
+	}
+	else {
+		return BAD_POS;
+	}
+}
+
+bool bibabot::move_raw(double position, double velocity, bool async)
+{
+	const int dir = sign(velocity);
+	const string cmd = "SetMotor?" + str(velocity/100) + " " + str(position/100) + " " + str(dir);
+	if(async) {
+		send(cmd);
+		return true;
+	}
+	else
+		return send_sync(cmd);
+}
+
+bool bibabot::calibrate()
+{
+	bibabot_cal_par par;
+	return calibrate(par);
+}
+
+bool bibabot::calibrate(const bibabot_cal_par &par)
+{
+	send_sync("InitOdo");
+	
+	vector<v3> traj;
+	const double dur = fabs(par.amplitude/par.speed);
+	const unsigned int sleep_msec = round(1000.0/par.sample_freq);
+	v3 sum_neg, sum_pos;
+	int n_sum_neg = 0, n_sum_pos = 0;
+	timer t;
+	for(int i = 0; i < par.n_cycles; i++) {
+		for(int phase = 0; phase < 4; phase++) {
+			bool move_ok;
+			if	   (phase == 0) move_ok = move_raw(-par.amplitude, -par.speed, false);
+			else if(phase == 1)	move_ok = move_raw(				0, +par.speed, false);
+			else if(phase == 2) move_ok = move_raw(+par.amplitude, +par.speed, false);
+			else if(phase == 3)	move_ok = move_raw(				0, -par.speed, false);
+			if(!move_ok) return false;
+			for(t = 0; t < dur*par.duration_security;) {
+				v3 pos;
+				if(_pbird->read_raw(&pos)) {
+					bool ok = true;
+					if(traj.size() > 0)
+						ok = ((pos - traj.back()).length() >= par.min_step);
+					if(ok) traj.push_back(pos);
+				}
+				SleepEx(sleep_msec, FALSE);
+			}
+			wait_until_stop();
+			if(phase == 0) {
+				sum_neg += traj.back();
+				n_sum_neg++;
+			}
+			else if(phase == 2) {
+				sum_pos += traj.back();
+				n_sum_pos++;
+			}
+		}
+	}
+
+	if(par.debug_file != "") {
+		datadump dd(par.debug_file, "w");
+		dd << traj;
+		dd.write();
+		dd.close();
+	}
+
+	if(traj.size() < 3) return false;
+	if(n_sum_neg == 0 || n_sum_pos == 0) return false;
+	const v3 mean_neg = sum_neg/n_sum_neg;
+	const v3 mean_pos = sum_pos/n_sum_pos;
+	v3 eval;
+	m3 cov, evec;
+	covariance(cov, traj);
+	cov.eigensystem(eval, evec);
+	const double var_tot = eval(0) + eval(1) + eval(2);
+	if(eval(0)/var_tot < par.min_variance_fraction) return false;
+	_calib_dir = evec.row(0).normalize();
+	if(dot(_calib_dir, mean_pos - mean_neg) < 0) _calib_dir *= -1;
+	if(!_pbird->read_raw(&_calib_zero)) return false;
+	send_sync("InitOdo");
+	_calibrated = true;
+	return true;
+}
+
+double bibabot::pos()
+{
+	/*
+	if(!_calibrated) return BAD_POS;
+	v3 raw;
+	if(!_pbird->read_raw(&raw)) return BAD_POS;
+	return dot(raw - _calib_zero, _calib_dir);
+	*/
+	double z;
+	if(!pos(&z))
+		return BAD_POS;
+	else
+		return z;
+}
+
+bool bibabot::pos(double *pz, v3 *praw)
+{
+	bool ok = true;
+	v3 raw;
+	if(!_pbird->read_raw(&raw))
+		ok = false;
+	else {
+		if(praw) *praw = raw;
+		if(!_calibrated)
+			ok = false;
+		else {
+			const double z = dot(raw - _calib_zero, _calib_dir);
+			if(pz) *pz = z;
+		}
+	}
+	return ok;
+}
+
+bool bibabot::move(double position, double speed, bool async)
+{
+	double p = position;	bound(&p, -_par.max_pos, +_par.max_pos);
+	double s = speed;		bound(&s, -_par.max_speed, +_par.max_speed);
+	const double lpos = pos();
+	if(lpos == BAD_POS) return false;
+	const double rpos = robot_pos();
+	if(rpos == BAD_POS) return false;
+	const double command_pos = p - (lpos - rpos);
+	const double velocity = fabs(s)*sign(command_pos - rpos);
+	return move_raw(command_pos, velocity, async);
+}
+
+void bibabot::orientation_reset()
+{
+	if(!_pbird->read_raw(0, &_init_orient))
+		throw(error("bibabot", "could not read laserbird in orientation_reset"));
+}
+
+double bibabot::orientation_deviation()
+{
+	m3 orient;
+	if(!_pbird->read_raw(0, &orient))
+		throw(error("bibabot", "could not read laserbird in orientation_deviation"));
+	return angle_between_rotations(_init_orient, orient);
+}