summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2006-05-16 19:35:51 +0000
committerschodet2006-05-16 19:35:51 +0000
commitbc5944190b7ae0af55caa84f86d63bf95e40712f (patch)
treee77a381b322871cb6048a77ecbf694dfc72b04d8
parentcd40c9287d4b1624657ca784a640aa2678b52357 (diff)
Ajout de plein de trucs bien pour les trous.
Ajout du système de ack.
-rw-r--r--n/asserv/src/asserv/main.c33
-rw-r--r--n/asserv/src/asserv/simu.host.c37
-rw-r--r--n/asserv/src/asserv/speed.c2
-rw-r--r--n/asserv/src/asserv/traj.c50
-rw-r--r--n/asserv/utils/graph/asserv_graph.cc45
-rw-r--r--n/asserv/utils/graph/plot/host.plot3
-rw-r--r--n/asserv/utils/graph/rc/config.avr17
-rw-r--r--n/asserv/utils/graph/rc/config.host15
8 files changed, 147 insertions, 55 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index aec9c8e..54b9794 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -32,6 +32,9 @@
#include "misc.h"
+/** Motor command sequence. */
+uint8_t main_sequence, main_sequence_ack = 0xff;
+
/* This is implementation include. */
#ifndef HOST
# include "timer.avr.c"
@@ -55,8 +58,8 @@
* 3: trajectory control. */
int8_t main_mode;
-/** Motor command sequence. */
-uint8_t main_sequence;
+/** Report trajectory end. */
+uint8_t main_sequence_ack_cpt = 2;
/** Report of counters. */
uint8_t main_stat_counter, main_stat_counter_cpt;
@@ -144,6 +147,11 @@ main_loop (void)
speed_update ();
main_timer[3] = timer_read ();
/* Stats. */
+ if (main_sequence_ack == main_sequence && !--main_sequence_ack_cpt)
+ {
+ proto_send1b ('A', main_sequence);
+ main_sequence_ack_cpt = 2;
+ }
if (main_stat_counter && !--main_stat_counter_cpt)
{
proto_send2w ('C', counter_left, counter_right);
@@ -189,7 +197,7 @@ main_loop (void)
}
if (main_print_pin && !--main_print_pin_cpt)
{
- proto_send1b ('I', PINA);
+ proto_send2b ('I', PINA, PINF);
main_print_pin_cpt = main_print_pin;
}
/* Misc. */
@@ -274,6 +282,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
main_mode = 3;
traj_mode = 10;
+ traj_holes_filter = 0;
+ main_sequence = args[0];
+ break;
+ case c ('A', 1):
+ /* Set acknoledge.
+ * - b: new sequence number (0 for example). */
+ main_sequence = args[0];
break;
/* Stats.
* - b: interval between stats. */
@@ -350,12 +365,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta_acc = v8_to_v16 (args[1], args[2]);
speed_alpha_acc = v8_to_v16 (args[3], args[4]);
break;
- case c ('s', 3):
- /* Set maximum speed.
- * - b: theta.
- * - b: alpha. */
+ case c ('s', 5):
+ /* Set maximum and slow speed.
+ * - b: theta max.
+ * - b: alpha max.
+ * - b: theta slow.
+ * - b: alpha slow. */
speed_theta_max = args[1];
speed_alpha_max = args[2];
+ speed_theta_slow = args[3];
+ speed_alpha_slow = args[4];
break;
case c ('p', 3):
pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]);
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 64573bb..bb204c1 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -103,12 +103,13 @@ struct simu_rgb_t
{
double x, y;
};
-static const struct simu_rgb_t simu_rgbs[4] =
+static const struct simu_rgb_t simu_rgbs[5] =
{
- { -55, 70 },
- { +55, 70 },
- { 0, 65 + 55 },
- { 0, 65 - 55 },
+ { 65, +55 },
+ { 65, -55 },
+ { 65 + 45, 0 },
+ { 65 - 45, 0 },
+ { 65, 0 },
};
/** Initialise simulation. */
@@ -164,9 +165,9 @@ static void
simu_rgb_update (void)
{
unsigned i, j;
- int rgb_x, rgb_y;
- int dx, dy, d2;
- int md2 = -1;
+ double rgb_x, rgb_y;
+ double dx, dy, d2;
+ double md2 = -1;
PINF &= 0xf0;
for (i = 0; i < UTILS_COUNT (simu_rgbs); i++)
{
@@ -181,16 +182,24 @@ simu_rgb_update (void)
dx = simu_holes[j].x - rgb_x;
dy = simu_holes[j].y - rgb_y;
d2 = dx * dx + dy * dy;
- if (d2 < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
+ if (i != UTILS_COUNT (simu_rgbs) - 1)
{
- PINF |= 1 << i;
- break;
+ /* Rgb. */
+ if (d2 < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
+ {
+ PINF |= 1 << i;
+ break;
+ }
+ }
+ else
+ {
+ /* Exit hole. */
+ if (md2 == -1 || d2 < md2)
+ md2 = d2;
}
- if (md2 == -1 || d2 < md2)
- md2 = d2;
}
}
- simu_nearest_hole = md2;
+ simu_nearest_hole = sqrt (md2);
}
/** Do a simulation step. */
diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c
index eb9b568..f9b1d93 100644
--- a/n/asserv/src/asserv/speed.c
+++ b/n/asserv/src/asserv/speed.c
@@ -35,6 +35,8 @@ int16_t speed_theta_cur, speed_alpha_cur;
int16_t speed_theta_cons, speed_alpha_cons;
/** Maximum speed for position consign, u8. */
int8_t speed_theta_max, speed_alpha_max;
+/** Slow speed for position consign, u8. */
+int8_t speed_theta_slow, speed_alpha_slow;
/** Consign position. */
uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
/** Weither to use the consign position (1) or not (0). */
diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c
index 8f2413b..cfdbbc4 100644
--- a/n/asserv/src/asserv/traj.c
+++ b/n/asserv/src/asserv/traj.c
@@ -28,6 +28,9 @@
*/
uint8_t traj_mode;
+/** Holes filter. */
+uint8_t traj_holes_filter;
+
/* Find a hole in the table.
* French: On avance, et si on voit un "pas vert" à gauche, on tourne à
* gauche. Pareil pour la droite. Au final, on centre le "pas vert" entre deux
@@ -41,33 +44,62 @@ static void
traj_holes (void)
{
int16_t speed;
+ uint8_t in;
speed_pos = 0;
+ in = PINF;
/* Change state if needed. */
- if (traj_mode == 10 && (PINF & 0xf))
- traj_mode++;
- else if (traj_mode == 11 && (PINF & 0xf) == 0)
- traj_mode++;
+ if (traj_mode == 10)
+ {
+ if (in & 0xf)
+ traj_holes_filter++;
+ else
+ traj_holes_filter = 0;
+ if (traj_holes_filter >= 8)
+ {
+ traj_holes_filter = 255;
+ traj_mode = 11;
+ }
+ }
+ else if (traj_mode == 11)
+ {
+ /* Return to cruise mode if no hole for too long. */
+ if (!(in & 0xf))
+ traj_holes_filter--;
+ if (traj_holes_filter == 0)
+ traj_mode = 10;
+ }
/* Determine speed. */
- speed = traj_mode == 10 ? speed_theta_max : speed_theta_max / 2;
+ speed = traj_mode == 10 ? speed_theta_max : speed_theta_slow;
speed *= 256;
- /* Left or right. */
- if (PINF & 1)
+ if ((in & 0xf) == 0xc)
+ {
+ /* Stop. */
+ speed_theta_cons = 0;
+ speed_alpha_cons = 0;
+ main_sequence_ack = main_sequence;
+ traj_mode = 12;
+ }
+ else if (in & 1)
{
+ /* Left. */
speed_theta_cons = speed / 2;
speed_alpha_cons = speed / 2;
}
- else if (PINF & 2)
+ else if (in & 2)
{
+ /* Right. */
speed_theta_cons = speed / 2;
speed_alpha_cons = -speed / 2;
}
- else if (PINF & 8)
+ else if (in & 8)
{
+ /* Back. */
speed_theta_cons = -speed;
speed_alpha_cons = 0;
}
else
{
+ /* Front. */
speed_theta_cons = speed;
speed_alpha_cons = 0;
}
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 ();
diff --git a/n/asserv/utils/graph/plot/host.plot b/n/asserv/utils/graph/plot/host.plot
index d138757..a2163aa 100644
--- a/n/asserv/utils/graph/plot/host.plot
+++ b/n/asserv/utils/graph/plot/host.plot
@@ -1,3 +1,2 @@
set style data line
-plot 'host' using 1 title "te", 'host' using 3 title "ti", 'host' using 3 title "ae", 'host' using 4 title "ai", 'host' using 5 title "pl", 'host' using 6 title "pr", 'host' using 7 title "tsc", 'host' using 8 title "asc"
-#, 'host' using 12 title "cl", 'host' using 13 title "cr"
+plot 'host' using 1 title "te", 'host' using 3 title "ti", 'host' using 3 title "ae", 'host' using 4 title "ai", 'host' using 5 title "pl", 'host' using 6 title "pr", 'host' using 7 title "tsc", 'host' using 8 title "asc", 'host' using 9 title "hx", 'host' using 10 title "hy", 'host' using 11 title "ha", 'host' using 12 title "dh", 'host' using 13 title "cl", 'host' using 14 title "cr", 'host' using 15 title "z0"
diff --git a/n/asserv/utils/graph/rc/config.avr b/n/asserv/utils/graph/rc/config.avr
index cbfc174..a17dc84 100644
--- a/n/asserv/utils/graph/rc/config.avr
+++ b/n/asserv/utils/graph/rc/config.avr
@@ -11,15 +11,18 @@ asserv.akd = 0
asserv.esat = 1023
asserv.isat = 3072
-asserv.ta = 256
-asserv.aa = 256
-asserv.tspm = 96
-asserv.aspm = 96
+asserv.ta = 1024
+asserv.aa = 2048
+asserv.tspm = 64
+asserv.aspm = 64
+asserv.tsps = 8
+asserv.asps = 8
-asserv.stat_pos = 1
-asserv.stat_speed = 0
-asserv.stat_count = 1
+asserv.stat_pos = 0
+asserv.stat_speed = 1
+asserv.stat_count = 0
asserv.stat_pwm = 1
+asserv.stat_in = 1
host = false
iu_mm = 75.78789091394133410940
diff --git a/n/asserv/utils/graph/rc/config.host b/n/asserv/utils/graph/rc/config.host
index 5272743..ea331d8 100644
--- a/n/asserv/utils/graph/rc/config.host
+++ b/n/asserv/utils/graph/rc/config.host
@@ -11,16 +11,19 @@ asserv.akd = 0
asserv.esat = 1023
asserv.isat = 128
-asserv.ta = 256
-asserv.aa = 256
-asserv.tspm = 96
-asserv.aspm = 96
+asserv.ta = 1024
+asserv.aa = 2048
+asserv.tspm = 64
+asserv.aspm = 64
+asserv.tsps = 16
+asserv.asps = 16
-asserv.stat_pos = 1
+asserv.stat_pos = 0
asserv.stat_speed = 1
-asserv.stat_count = 1
+asserv.stat_count = 0
asserv.stat_pwm = 1
asserv.stat_simu = 1
+asserv.stat_in = 1
host = true
iu_mm = 75.78789091394133410940