summaryrefslogtreecommitdiff
path: root/n/asserv/utils/graph/asserv_graph.cc
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/utils/graph/asserv_graph.cc')
-rw-r--r--n/asserv/utils/graph/asserv_graph.cc57
1 files changed, 54 insertions, 3 deletions
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<int> ("asserv.tkp");
@@ -63,10 +71,18 @@ class AsservGraph : public Proto::Receiver
is = config.get<int> ("asserv.isat");
ta = config.get<int> ("asserv.ta");
aa = config.get<int> ("asserv.aa");
+ tspm = config.get<int> ("asserv.tspm");
+ aspm = config.get<int> ("asserv.aspm");
sP = config.get<int> ("asserv.stat_pos");
sS = config.get<int> ("asserv.stat_speed");
sC = config.get<int> ("asserv.stat_count");
sW = config.get<int> ("asserv.stat_pwm");
+ host = config.get<bool> ("host");
+ if (host)
+ {
+ sY = config.get<int> ("asserv.stat_simu");
+ iu_mm = config.get<double> ("iu_mm");
+ }
proto_.open (config.get<std::string> ("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;