summaryrefslogtreecommitdiff
path: root/n
diff options
context:
space:
mode:
Diffstat (limited to 'n')
-rw-r--r--n/asserv/src/asserv/eeprom.avr.c4
-rw-r--r--n/asserv/src/asserv/main.c3
-rw-r--r--n/asserv/src/asserv/traj.c78
3 files changed, 56 insertions, 29 deletions
diff --git a/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c
index 3abba92..dcd567c 100644
--- a/n/asserv/src/asserv/eeprom.avr.c
+++ b/n/asserv/src/asserv/eeprom.avr.c
@@ -25,7 +25,7 @@
#include <avr/eeprom.h>
/* Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x45
+#define EEPROM_KEY 0x44
#define EEPROM_START 256
/* +AutoDec */
@@ -54,6 +54,7 @@ eeprom_read_params (void)
pos_alpha_kd = eeprom_read_word (p16++);
pos_e_sat = eeprom_read_word (p16++);
pos_int_sat = eeprom_read_word (p16++);
+ pos_blocked = eeprom_read_word (p16++);
}
/* Write parameters to eeprom. */
@@ -78,6 +79,7 @@ eeprom_write_params (void)
eeprom_write_word (p16++, pos_alpha_kd);
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_int_sat);
+ eeprom_write_word (p16++, pos_blocked);
}
/* Clear eeprom parameters. */
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 63fd580..943c2e8 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -405,6 +405,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('I', 3):
pos_int_sat = v8_to_v16 (args[1], args[2]);
break;
+ case c ('b', 3):
+ pos_blocked = v8_to_v16 (args[1], args[2]);
+ break;
case c ('w', 3):
/* Set PWM direction.
* - b: inverse left direction.
diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c
index eb0ef92..d60f7c9 100644
--- a/n/asserv/src/asserv/traj.c
+++ b/n/asserv/src/asserv/traj.c
@@ -31,6 +31,9 @@ uint8_t traj_mode;
/** Holes filter. */
uint8_t traj_holes_filter;
+/** Holes ok filter. */
+uint8_t traj_holes_ok;
+
/* 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
@@ -73,6 +76,7 @@ traj_holes (void)
{
traj_holes_filter = 255;
traj_mode = 12;
+ traj_holes_ok = 0;
}
}
else if (traj_mode == 12)
@@ -84,39 +88,57 @@ traj_holes (void)
traj_mode = 11;
}
/* Determine speed. */
- speed = traj_mode <= 11 ? speed_theta_max : speed_theta_slow;
- speed *= 256;
- if ((in & 0xf) == 0xc)
- {
- /* Stop. */
- speed_theta_cons = 0;
- speed_alpha_cons = 0;
- main_sequence_finish = main_sequence;
- traj_mode = 13;
- }
- else if (in & 1)
- {
- /* Left. */
- speed_theta_cons = 0;
- speed_alpha_cons = speed;
- }
- else if (in & 2)
- {
- /* Right. */
- speed_theta_cons = 0;
- speed_alpha_cons = -speed;
- }
- else if (in & 8)
+ if (traj_mode <= 11)
{
- /* Back. */
- speed_theta_cons = -speed;
+ /* Cruise mode. */
+ speed = speed_theta_max;
+ speed *= 256;
+ speed_theta_cons = speed;
speed_alpha_cons = 0;
}
else
{
- /* Front. */
- speed_theta_cons = speed;
- speed_alpha_cons = 0;
+ speed = speed_theta_slow;
+ speed *= 256;
+ if ((in & 0xf) == 0xc || (in & 0x7) == 0x7)
+ {
+ traj_holes_ok++;
+ if (traj_holes_ok >= 32)
+ {
+ /* Stop. */
+ speed_theta_cons = 0;
+ speed_alpha_cons = 0;
+ main_sequence_finish = main_sequence;
+ traj_mode = 13;
+ }
+ }
+ else
+ {
+ if (in & 1)
+ {
+ /* Left. */
+ speed_theta_cons = 0;
+ speed_alpha_cons = speed;
+ }
+ else if (in & 2)
+ {
+ /* Right. */
+ speed_theta_cons = 0;
+ speed_alpha_cons = -speed;
+ }
+ else if (in & 8)
+ {
+ /* Back. */
+ speed_theta_cons = -speed;
+ speed_alpha_cons = 0;
+ }
+ else
+ {
+ /* Front. */
+ speed_theta_cons = speed;
+ speed_alpha_cons = 0;
+ }
+ }
}
}