summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-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
4 files changed, 92 insertions, 30 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;
}