From 1aff85cbaa8174f4aef9ded81c2156c8f1c5a508 Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 25 May 2006 22:01:18 +0000 Subject: Petite correction d'un facteur deux sur le calcul de l'angle de rotation. --- n/asserv/src/asserv/postrack.c | 10 ++++++---- n/asserv/utils/graph/asserv_graph.cc | 17 ++++++++++++++--- n/asserv/utils/graph/rc/config.avr | 1 + n/asserv/utils/graph/rc/config.host | 3 ++- 4 files changed, 23 insertions(+), 8 deletions(-) diff --git a/n/asserv/src/asserv/postrack.c b/n/asserv/src/asserv/postrack.c index 2f8aae5..bd4373c 100644 --- a/n/asserv/src/asserv/postrack.c +++ b/n/asserv/src/asserv/postrack.c @@ -30,14 +30,16 @@ int32_t postrack_a; /** Distance between the weels, u16. */ uint16_t postrack_footing; /** Precomputed footing factor, f8.24. - * postrack_footing_factor = (1/2pi * 256) / postrack_footing + * postrack_footing_factor = (2 * 1/2pi * 256) / postrack_footing * Explanations: * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we * must divide by 2pi to convert unit (Arc=Angle * Radius only works for * radians). - * - dd (see postrack_update) is in f11.16 format, we multiply by 256 to have + * - dd (see postrack_update) is in f10.16 format, we multiply by 256 to have * a angle in f8.24 format. - * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */ + * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). + * - il y a un facteur deux à cause de la somme effectuée dans + * l'asservissement pid. */ uint32_t postrack_footing_factor; /* +AutoDec */ @@ -111,5 +113,5 @@ postrack_set_footing (uint16_t footing) { postrack_footing = footing; postrack_footing_factor = - (uint32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing; + (uint32_t) (M_1_PI * (1L << 8) * (1L << 24)) / footing; } diff --git a/n/asserv/utils/graph/asserv_graph.cc b/n/asserv/utils/graph/asserv_graph.cc index 900276d..45a836d 100644 --- a/n/asserv/utils/graph/asserv_graph.cc +++ b/n/asserv/utils/graph/asserv_graph.cc @@ -46,9 +46,10 @@ class AsservGraph : public Proto::Receiver int cl, cr; int hx, hy, ha, dh; int z0, z1, z2; + int px, py, pa; int pl, pr; int n; - int sP, sS, sC, sW, sY, sI; + int sP, sS, sC, sX, sW, sY, sI; bool host; double iu_mm, footing; int seq; @@ -60,6 +61,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), + px (0), py (0), pa (0), n (0), seq (1), ack (true) { Config &config = Config::getInstance (); @@ -80,6 +82,7 @@ class AsservGraph : public Proto::Receiver sP = config.get ("asserv.stat_pos"); sS = config.get ("asserv.stat_speed"); sC = config.get ("asserv.stat_count"); + sX = config.get ("asserv.stat_postrack"); sW = config.get ("asserv.stat_pwm"); sI = config.get ("asserv.stat_in", 0); host = config.get ("host"); @@ -106,9 +109,11 @@ class AsservGraph : public Proto::Receiver proto_.send ('p', "bw", 'I', is); proto_.send ('p', "bww", 'a', ta, aa); proto_.send ('p', "bbbbb", 's', tspm, aspm, tsps, asps); + proto_.send ('p', "bw", 'f', footing * iu_mm); proto_.send ('P', "b", sP); proto_.send ('S', "b", sS); proto_.send ('C', "b", sC); + proto_.send ('X', "b", sX); proto_.send ('W', "b", sW); proto_.send ('I', "b", sI); if (host) @@ -130,6 +135,9 @@ class AsservGraph : public Proto::Receiver case 'C': proto_.decode (frame, "WW", cl, cr); break; + case 'X': + proto_.decode (frame, "DDD", px, py, pa); + break; case 'Y': proto_.decode (frame, "WWWW", hx, hy, ha, dh); break; @@ -147,12 +155,15 @@ class AsservGraph : public Proto::Receiver << hx << ' ' << hy << ' ' << ((double) ha / 1024) << ' ' << dh << ' ' << cl << ' ' << cr << ' ' - << z0 << ' ' << z1 << ' ' << z2 << ' ' << ack << std::endl; + << z0 << ' ' << z1 << ' ' << z2 << ' ' << ack << ' ' + << (px / 256) << ' ' << (py / 256) << ' ' + << (360.0 * pa / 256 / 256 / 256) << std::endl; te = ti = ae = ai = 0; tsc = asc = 0; cl = cr = 0; hx = hy = ha = dh = 0; z0 = z1 = z2 = 0; + px = py = pa = 0; n++; } break; @@ -187,7 +198,7 @@ class AsservGraph : public Proto::Receiver { proto_.wait (-1); if (ack) - proto_.send ('A', "b", 0); + proto_.send ('a', "b", seq); ack = false; } } diff --git a/n/asserv/utils/graph/rc/config.avr b/n/asserv/utils/graph/rc/config.avr index c2da1ca..5448772 100644 --- a/n/asserv/utils/graph/rc/config.avr +++ b/n/asserv/utils/graph/rc/config.avr @@ -21,6 +21,7 @@ asserv.asps = 8 asserv.stat_pos = 0 asserv.stat_speed = 1 asserv.stat_count = 0 +asserv.stat_postrack = 8 asserv.stat_pwm = 1 asserv.stat_in = 1 diff --git a/n/asserv/utils/graph/rc/config.host b/n/asserv/utils/graph/rc/config.host index ea331d8..128aab6 100644 --- a/n/asserv/utils/graph/rc/config.host +++ b/n/asserv/utils/graph/rc/config.host @@ -20,7 +20,8 @@ asserv.asps = 16 asserv.stat_pos = 0 asserv.stat_speed = 1 -asserv.stat_count = 0 +asserv.stat_count = 1 +asserv.stat_postrack = 1 asserv.stat_pwm = 1 asserv.stat_simu = 1 asserv.stat_in = 1 -- cgit v1.2.3