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.cc45
1 files changed, 35 insertions, 10 deletions
diff --git a/n/asserv/utils/graph/asserv_graph.cc b/n/asserv/utils/graph/asserv_graph.cc
index 2edb612..068fd72 100644
--- a/n/asserv/utils/graph/asserv_graph.cc
+++ b/n/asserv/utils/graph/asserv_graph.cc
@@ -40,7 +40,7 @@ class AsservGraph : public Proto::Receiver
int akp, aki, akd;
int es, is;
int ta, aa;
- int tspm, aspm;
+ int tspm, aspm, tsps, asps;
int te, ti, ae, ai;
int tsc, asc;
int cl, cr;
@@ -48,10 +48,11 @@ class AsservGraph : public Proto::Receiver
int z0, z1, z2;
int pl, pr;
int n;
- int sP, sS, sC, sW, sY;
+ int sP, sS, sC, sW, sY, sI;
bool host;
double iu_mm, footing;
int seq;
+ bool ack;
public:
AsservGraph (int argc, char **argv)
: proto_ (*this), argc_ (argc), argv_ (argv),
@@ -59,7 +60,7 @@ class AsservGraph : public Proto::Receiver
tsc (0), asc (0), cl (0), cr (0),
hx (0), hy (0), ha (0), dh (0),
z0 (0), z1 (0), z2 (0),
- n (0), seq (1)
+ n (0), seq (1), ack (true)
{
Config &config = Config::getInstance ();
tkp = config.get<int> ("asserv.tkp");
@@ -74,10 +75,13 @@ class AsservGraph : public Proto::Receiver
aa = config.get<int> ("asserv.aa");
tspm = config.get<int> ("asserv.tspm");
aspm = config.get<int> ("asserv.aspm");
+ tsps = config.get<int> ("asserv.tsps");
+ asps = config.get<int> ("asserv.asps");
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");
+ sI = config.get<int> ("asserv.stat_in", 0);
host = config.get<bool> ("host");
if (host)
{
@@ -101,11 +105,12 @@ 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', "bbbbb", 's', tspm, aspm, tsps, asps);
proto_.send ('P', "b", sP);
proto_.send ('S', "b", sS);
proto_.send ('C', "b", sC);
proto_.send ('W', "b", sW);
+ proto_.send ('I', "b", sI);
if (host)
proto_.send ('Y', "b", sY);
}
@@ -113,6 +118,9 @@ class AsservGraph : public Proto::Receiver
{
switch (command)
{
+ case 'A':
+ ack = true;
+ break;
case 'P':
proto_.decode (frame, "WWWW", te, ti, ae, ai);
break;
@@ -128,6 +136,9 @@ class AsservGraph : public Proto::Receiver
case 'Z':
proto_.decode (frame, "BBB", z0, z1, z2);
break;
+ case 'I':
+ proto_.decode (frame, "bb", z0, z1);
+ break;
case 'W':
if (proto_.decode (frame, "WW", pl, pr))
{
@@ -136,7 +147,7 @@ class AsservGraph : public Proto::Receiver
<< hx << ' ' << hy << ' ' << ((double) ha / 1024) << ' '
<< dh << ' '
<< cl << ' ' << cr << ' '
- << z0 << ' ' << z1 << ' ' << z2 << std::endl;
+ << z0 << ' ' << std::hex << z1 << ' ' << z2 << ' ' << ack << std::endl;
te = ti = ae = ai = 0;
tsc = asc = 0;
cl = cr = 0;
@@ -164,15 +175,27 @@ class AsservGraph : public Proto::Receiver
{
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);
+ unsigned int i;
+ static const int d[] =
+ { 50, 25, 0, 75 };
+ for (i = 0; i < sizeof (d) / sizeof (d[0]); i++)
+ {
+ proto_.send ('y', "bwww", 'X', 1050 + d[i], 300,
+ (int) (M_PI_2 * 1024));
+ proto_.send ('h', "b", seq++);
+ for (n = 0; n < 1000;)
+ {
+ proto_.wait (-1);
+ if (ack)
+ proto_.send ('A', "b", 0);
+ ack = false;
+ }
+ }
}
else
{
proto_.send ('h', "b", seq++);
- for (n = 0; n < 1000;)
+ for (n = 0; n < 3000;)
proto_.wait (-1);
}
}
@@ -225,10 +248,12 @@ class AsservGraph : public Proto::Receiver
proto_.wait (-1);
break;
case 'R':
+ proto_.send ('p', "bbb", 's', 96, 12);
proto_.send ('s', "ddb", (int) (iu_mm * 512),
(int) (iu_mm * -64), 1);
for (n = 0; n < 500;)
proto_.wait (-1);
+ proto_.send ('p', "bbb", 's', tspm, aspm);
break;
case 'h':
find_holes ();