From 5a534ef4253328eec26acfabc8d8cbcb5641b67c Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 5 May 2006 19:41:47 +0000 Subject: Plus de trous, plus de stats et plus de coefs qui vont bien :) --- n/asserv/utils/graph/asserv_graph.cc | 57 ++++++++++++++++++++++++++++++++++-- 1 file changed, 54 insertions(+), 3 deletions(-) (limited to 'n/asserv/utils/graph/asserv_graph.cc') diff --git a/n/asserv/utils/graph/asserv_graph.cc b/n/asserv/utils/graph/asserv_graph.cc index 6de6c52..2453031 100644 --- a/n/asserv/utils/graph/asserv_graph.cc +++ b/n/asserv/utils/graph/asserv_graph.cc @@ -39,18 +39,26 @@ class AsservGraph : public Proto::Receiver int akp, aki, akd; int es, is; int ta, aa; + int tspm, aspm; int te, ti, ae, ai; int tsc, asc; int cl, cr; + int hx, hy, ha, dh; + int z0, z1, z2; int pl, pr; int n; - int sP, sS, sC, sW; + int sP, sS, sC, sW, sY; + bool host; + double iu_mm; + int seq; public: AsservGraph (int argc, char **argv) : proto_ (*this), argc_ (argc), argv_ (argv), te (0), ti (0), ae (0), ai (0), tsc (0), asc (0), cl (0), cr (0), - n (0) + hx (0), hy (0), ha (0), dh (0), + z0 (0), z1 (0), z2 (0), + n (0), seq (1) { Config &config = Config::getInstance (); tkp = config.get ("asserv.tkp"); @@ -63,10 +71,18 @@ class AsservGraph : public Proto::Receiver is = config.get ("asserv.isat"); ta = config.get ("asserv.ta"); aa = config.get ("asserv.aa"); + tspm = config.get ("asserv.tspm"); + aspm = config.get ("asserv.aspm"); sP = config.get ("asserv.stat_pos"); sS = config.get ("asserv.stat_speed"); sC = config.get ("asserv.stat_count"); sW = config.get ("asserv.stat_pwm"); + host = config.get ("host"); + if (host) + { + sY = config.get ("asserv.stat_simu"); + iu_mm = config.get ("iu_mm"); + } proto_.open (config.get ("asserv.tty")); init (); } @@ -83,10 +99,14 @@ class AsservGraph : public Proto::Receiver proto_.send ('p', "bw", 'E', es); proto_.send ('p', "bw", 'I', is); proto_.send ('p', "bww", 'a', ta, aa); + proto_.send ('p', "bbb", 's', tspm, aspm); proto_.send ('P', "b", sP); proto_.send ('S', "b", sS); proto_.send ('C', "b", sC); proto_.send ('W', "b", sW); + if (host) + proto_.send ('Y', "b", sY); + ///proto_.send ('p', "bw", 'h', (int) ((110 - 0) * iu_mm)); } void receive (char command, const Proto::Frame &frame) { @@ -101,15 +121,26 @@ class AsservGraph : public Proto::Receiver case 'C': proto_.decode (frame, "WW", cl, cr); break; + case 'Y': + proto_.decode (frame, "WWWW", hx, hy, ha, dh); + break; + case 'Z': + proto_.decode (frame, "BBB", z0, z1, z2); + break; case 'W': if (proto_.decode (frame, "WW", pl, pr)) { std::cout << te << ' ' << ti << ' ' << ae << ' ' << ai << ' ' << pl << ' ' << pr << ' ' << tsc << ' ' << asc << ' ' - << cl << ' ' << cr << std::endl; + << hx << ' ' << hy << ' ' << ((double) ha / 1024) << ' ' + << dh << ' ' + << cl << ' ' << cr << ' ' + << z0 << ' ' << z1 << ' ' << z2 << std::endl; te = ti = ae = ai = 0; tsc = asc = 0; cl = cr = 0; + hx = hy = ha = dh = 0; + z0 = z1 = z2 = 0; n++; } break; @@ -124,8 +155,25 @@ class AsservGraph : public Proto::Receiver " c teste le pas\n" " s DUR teste la vitesse\n" " p teste la position à vitesse controlée\n" + " h teste la detection de trous\n" << std::endl; } + void find_holes (void) + { + if (host) + { + proto_.send ('y', "bwww", 'X', 1050 + 50, 400, (int) (M_PI_2 * 1024)); + proto_.send ('h', "b", seq++); + for (n = 0; n < 1000;) + proto_.wait (-1); + } + else + { + proto_.send ('h', "b", seq++); + for (n = 0; n < 1000;) + proto_.wait (-1); + } + } int main (void) { if (argc_ == 1) @@ -168,6 +216,9 @@ class AsservGraph : public Proto::Receiver for (n = 0; n < 500;) proto_.wait (-1); break; + case 'h': + find_holes (); + break; default: throw std::runtime_error ("bad command line"); break; -- cgit v1.2.3