summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/counter.c79
-rw-r--r--n/asserv/src/eeprom.c2
-rw-r--r--n/asserv/src/goto.c8
-rw-r--r--n/asserv/src/main.c11
-rw-r--r--n/asserv/src/pwm.c18
-rw-r--r--n/asserv/src/taz.c24
-rw-r--r--n/asserv/src/test_pwm.c2
7 files changed, 87 insertions, 57 deletions
diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c
index 774a9c8..9f8a5d5 100644
--- a/n/asserv/src/counter.c
+++ b/n/asserv/src/counter.c
@@ -45,10 +45,6 @@ static int16_t counter_left_diff, counter_right_diff;
static inline void
counter_init (void);
-/** Poll counters, should be called as often as possible. */
-static inline void
-counter_poll (void);
-
/** Update overall counter values and compute diffs. */
static inline void
counter_update (void);
@@ -75,45 +71,44 @@ counter_init (void)
0, 0, 0, 0, 0, 1, 1, 1);
/* Begin with safe values. */
counter_restart ();
+ /* Interrupts for direction. */
+ EICRB = 0x05;
+ EIFR = _BV (4) | _BV (5);
+ EIMSK = _BV (4) | _BV (5);
}
-/** Poll counters, should be called as often as possible. */
-static inline void
-counter_poll (void)
+/** Left direction change. */
+SIGNAL (SIG_INTERRUPT4)
{
uint8_t c;
- /* Read left counter. */
c = TCNT2;
-#if COUNTER_REVERSE_LEFT == 0
if (PINE & _BV (4))
-#else
- if (!(PINE & _BV (4)))
-#endif
{
- PORTD &= ~0x40;
- counter_left_frw += c - counter_left_old;
+ counter_left_rev += c - counter_left_old;
+ PORTD |= 0x40;
}
else
{
- PORTD |= 0x40;
- counter_left_rev += c - counter_left_old;
+ counter_left_frw += c - counter_left_old;
+ PORTD &= ~0x40;
}
counter_left_old = c;
- /* Read right counter. */
+}
+
+/** Right direction change. */
+SIGNAL (SIG_INTERRUPT5)
+{
+ uint8_t c;
c = TCNT3L;
-#if COUNTER_REVERSE_RIGHT == 0
if (PINE & _BV (5))
-#else
- if (!(PINE & _BV (5)))
-#endif
{
- PORTD &= ~0x20;
- counter_right_frw += c - counter_right_old;
+ counter_right_rev += c - counter_right_old;
+ PORTD |= 0x20;
}
else
{
- PORTD |= 0x20;
- counter_right_rev += c - counter_right_old;
+ counter_right_frw += c - counter_right_old;
+ PORTD &= ~0x20;
}
counter_right_old = c;
}
@@ -122,16 +117,46 @@ counter_poll (void)
static inline void
counter_update (void)
{
+ uint8_t c;
+ /* Disable ints. */
+ EIMSK &= ~(_BV (4) | _BV (5));
+ /* Read left counter. */
+ c = TCNT2;
+ if (PINE & _BV (4))
+ counter_left_frw += c - counter_left_old;
+ else
+ counter_left_rev += c - counter_left_old;
+ counter_left_old = c;
+ /* Read right counter. */
+ c = TCNT3L;
+ if (PINE & _BV (5))
+ counter_right_frw += c - counter_right_old;
+ else
+ counter_right_rev += c - counter_right_old;
+ counter_right_old = c;
+ /* Update counter values and diffs. */
+#if COUNTER_REVERSE_LEFT == 0
counter_left_diff = counter_left_frw;
- counter_left_frw = 0;
counter_left_diff -= counter_left_rev;
+#else
+ counter_left_diff = counter_left_rev;
+ counter_left_diff -= counter_left_frw;
+#endif
+ counter_left_frw = 0;
counter_left_rev = 0;
counter_left += counter_left_diff;
+#if COUNTER_REVERSE_RIGHT == 0
counter_right_diff = counter_right_frw;
- counter_right_frw = 0;
counter_right_diff -= counter_right_rev;
+#else
+ counter_right_diff = counter_right_rev;
+ counter_right_diff -= counter_right_frw;
+#endif
+ counter_right_frw = 0;
counter_right_rev = 0;
counter_right += counter_right_diff;
+ /* Enable ints. */
+ EIMSK |= _BV (4) | _BV (5);
}
/** Restart counting. */
diff --git a/n/asserv/src/eeprom.c b/n/asserv/src/eeprom.c
index 8c98078..3eb0140 100644
--- a/n/asserv/src/eeprom.c
+++ b/n/asserv/src/eeprom.c
@@ -52,7 +52,7 @@ eeprom_write_params (void)
uint8_t *p8 = 0;
uint16_t *p16;
eeprom_write_byte (p8++, 0xa5);
- eeprom_write_byte (p8++, speed_acc_cpt);
+ eeprom_write_byte (p8++, speed_acc);
eeprom_write_byte (p8++, speed_max);
eeprom_write_byte (p8++, pwm_dir);
p16 = (uint16_t *) p8;
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 64d1eac..d833204 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -41,7 +41,7 @@ int8_t goto_a;
/** Travel speed for fixed speed movements, i8. */
int8_t goto_s;
/** Destination epsillon. */
-int32_t goto_eps = 200L << 8;
+int32_t goto_eps = 20L << 8;
/** Debug values. */
int32_t goto_dx, goto_dy, goto_dl, goto_da;
/** Movement finished. */
@@ -65,6 +65,8 @@ goto_linear_mode (void)
/* Change speed. */
if (d > goto_d)
{
+ speed_left = 0;
+ speed_right = 0;
speed_left_aim = 0;
speed_right_aim = 0;
goto_finish = 1;
@@ -90,11 +92,11 @@ goto_angular_mode (void)
/* Small angles. */
if (0x10000L > angle_diff && angle_diff > -0x10000L)
{
- goto_finish = 1;
speed_left = 0;
speed_right = 0;
speed_left_aim = 0;
speed_right_aim = 0;
+ goto_finish = 1;
}
else
{
@@ -116,6 +118,8 @@ goto_position_mode (void)
if (goto_dx < goto_eps && goto_dx > -goto_eps
&& goto_dy < goto_eps && goto_dy > -goto_eps)
{
+ speed_left = 0;
+ speed_right = 0;
speed_left_aim = 0;
speed_right_aim = 0;
goto_finish = 1;
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 7650014..fe99acf 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -30,6 +30,7 @@
#include <n/avr/twi-master/twi_master.h>
#include <avr/interrupt.h>
#include <avr/io.h>
+#include <avr/signal.h>
#include "pwm.c"
#include "timer.c"
@@ -118,8 +119,9 @@ main_loop (void)
while (1)
{
motor_timer_0 = timer_read ();
+ /* Counter update. */
while (!timer_pending ())
- counter_poll ();
+ ;
counter_update ();
motor_timer_4 = timer_read ();
/* Position tracking. */
@@ -214,9 +216,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
reset ();
break;
/* Commands. */
+ case c ('t', 0):
+ proto_send2b ('t', taz_state, taz_substate);
+ break;
case c ('t', 1):
motor_taz = args[0];
break;
+ case c ('t', 2):
+ taz_max_state = args[0];
+ taz_max_substate = args[1];
+ break;
case c ('l', 2):
/* Linear move.
* - w: distance (negative to go backward). */
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
index 093e064..4ab8722 100644
--- a/n/asserv/src/pwm.c
+++ b/n/asserv/src/pwm.c
@@ -73,36 +73,18 @@ pwm_init (void)
static inline uint8_t
pwm_preproc (uint16_t v)
{
- // This is a try to correct the optocoupler problem...
- //v += 0x10;
if (v > 255)
return 255;
else
return v;
}
-extern int8_t speed_left, speed_right;
-
-/** Filter PWM values. */
-static inline void
-pwm_filter (void)
-{
- /* This is a try to correct vibration problem. */
- if ((speed_left > 0 && pwm_left < 0)
- || (speed_left < 0 && pwm_left > 0))
- pwm_left = 0;
- if ((speed_right > 0 && pwm_right < 0)
- || (speed_right < 0 && pwm_right > 0))
- pwm_right = 0;
-}
-
/** Update the hardware PWM values. */
static inline void
pwm_update (void)
{
uint16_t left, right;
uint8_t dir;
- pwm_filter ();
dir = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR));
/* Set left PWM. */
if (pwm_left == 0)
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index 3980ca5..f615ed0 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -22,14 +22,21 @@
* Email: <contact@ni.fr.eu.org>
* }}} */
-/** FSM state. */
+/** FSM state & substate. */
uint8_t taz_state;
uint8_t taz_substate;
+/** FSM tempo. */
uint8_t taz_tempo;
+/** FSM max state & substate. */
+uint8_t taz_max_state = 0xff;
+uint8_t taz_max_substate;
+
/** Positions. */
//#define taz_scale (1.0/0.05026548245743669181)
//#define taz_scale (1.0/.05090591755319148936)
-#define taz_scale (1.0/.05132374757503138194)
+//#define taz_scale (1.0/.05132374757503138194)
+#define taz_scale (1.0/.05124525981346725427)
+static const uint32_t taz_rear_angle = 0x00ff90bf;
static const uint16_t taz_rear_16 = taz_scale * 270 / 2;
static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256;
static const uint16_t taz_front_16 = taz_scale * 270 / 2;
@@ -37,11 +44,12 @@ static const uint32_t taz_front_32 = taz_scale * 270 / 2 * 256;
static const uint16_t taz_side_16 = taz_scale * 340 / 2;
static const uint32_t taz_side_32 = taz_scale * 340 / 2 * 256;
-static const uint16_t taz_start_y_16 = taz_scale * (450 - 50) / 2;
+static const uint16_t taz_start_y_16 = taz_scale * 450 / 2;
static const uint16_t taz_before_bridge_16 = taz_scale * 1200;
-static const uint16_t taz_brige_interval_16 = taz_scale * 140;
-#define OR2 (1500 + 22 + 600 + 22)
+static const uint16_t taz_brige_interval_16 = taz_scale * 150;
+//#define OR2 (1500 + 22 + 600 + 22)
+#define OR2 (1500 + 600)
static const uint16_t taz_after_bridge_16[4] =
{
taz_scale * (OR2),
@@ -96,6 +104,7 @@ taz_state_0 (void)
{
taz_substate = 2;
/* FTW. */
+ speed_restart ();
motor_mode = 2;
goto_mode = 10;
goto_s = -10;
@@ -245,7 +254,7 @@ taz_state_1 (void)
taz_substate = 6;
/* Recalage. */
speed_restart ();
- postrack_y = taz_rear_32;
+ postrack_y = -taz_rear_32;
postrack_a = 0x00c00000;
/* On avance juste qu'à l'y de traversée de pont. */
motor_mode = 2;
@@ -424,6 +433,9 @@ taz_update (void)
uint8_t old_state, old_substate;
old_state = taz_state;
old_substate = taz_substate;
+ if (taz_state > taz_max_state
+ || (taz_state == taz_max_state && taz_substate > taz_max_substate))
+ return;
if (taz_tempo)
{
taz_tempo--;
diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c
index d92150c..ef3b413 100644
--- a/n/asserv/src/test_pwm.c
+++ b/n/asserv/src/test_pwm.c
@@ -30,8 +30,6 @@
/* +AutoDec */
/* -AutoDec */
-int8_t speed_left, speed_right;
-
#include "pwm.c"
void