summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/Makefile4
-rw-r--r--n/asserv/src/goto.c39
-rw-r--r--n/asserv/src/main.c25
-rw-r--r--n/asserv/src/postrack.c3
-rw-r--r--n/asserv/src/speed.c25
-rw-r--r--n/asserv/src/taz.c19
6 files changed, 79 insertions, 36 deletions
diff --git a/n/asserv/src/Makefile b/n/asserv/src/Makefile
index 1666979..88e343b 100644
--- a/n/asserv/src/Makefile
+++ b/n/asserv/src/Makefile
@@ -1,10 +1,10 @@
PROGS = asserv test_pwm test_dsp
-asserv_OBJECTS = main.o rs232.o proto.o
+asserv_OBJECTS = main.o rs232.o proto.o twi_master.o
test_pwm_OBJECTS = test_pwm.o rs232.o proto.o
test_dsp_OBJECTS = test_dsp.o rs232.o proto.o
DOC =
EXTRACTDOC =
-MODULES = n/avr/rs232 n/avr/proto n/avr/utils
+MODULES = n/avr/rs232 n/avr/proto n/avr/utils n/avr/twi-master
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
MCU_TARGET = atmega128
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index c427af9..553a4f4 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 = 20L << 8;
+int32_t goto_eps = 200L << 8;
/** Debug values. */
int32_t goto_dx, goto_dy, goto_dl, goto_da;
/** Movement finished. */
@@ -82,16 +82,27 @@ goto_linear_mode (void)
static inline void
goto_angular_mode (void)
{
- int8_t angle_diff;
- int32_t arc;
- /* Compute angle diff. This works because a full circle is 256. */
- angle_diff = goto_a - v32_to_v8 (postrack_a, 2);
- /* Compute arc. */
- arc = angle_diff; /* i8 */
- arc *= postrack_footing / 2; /* i24 */
- arc <<= 8; /* f24.8 */
- /* Change speed. */
- speed_distance (0, arc);
+ int32_t angle_diff;
+ /* Compute angle diff. */
+ angle_diff = v8_to_v32 (0, goto_a, 0, 0) - postrack_a;
+ angle_diff <<= 8;
+ angle_diff >>= 8;
+ /* Small angles. */
+ if (0x10000 > angle_diff && angle_diff < -0x10000)
+ {
+ goto_finish = 1;
+ speed_left = 0;
+ speed_right = 0;
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ }
+ else
+ {
+ /* Compute arc. */
+ goto_da = dsp_mul_f824 (angle_diff, postrack_footing_2pi);
+ /* Set speed. */
+ speed_distance (0, goto_da);
+ }
}
/** Position control mode. */
@@ -153,7 +164,7 @@ static inline void
goto_ftw_mode (void)
{
/* Change speed. */
- if (PINC & _BV (0))
+ if (PINA & _BV (0))
{
speed_left_aim = goto_s;
}
@@ -162,7 +173,7 @@ goto_ftw_mode (void)
speed_left = 0;
speed_left_aim = 0;
}
- if (PINC & _BV (7))
+ if (PINA & _BV (7))
{
speed_right_aim = goto_s;
}
@@ -171,7 +182,7 @@ goto_ftw_mode (void)
speed_right = 0;
speed_right_aim = 0;
}
- if (!(PINC & (_BV (0) || _BV (7))))
+ if (!(PINA & (_BV (0) || _BV (7))))
goto_finish = 1;
}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 9eeca45..1715d5f 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -27,6 +27,7 @@
#include <n/avr/proto/proto.h>
#include <n/avr/utils/utils.h>
#include <n/avr/utils/byte.h>
+#include <n/avr/twi-master/twi_master.h>
#include <avr/interrupt.h>
#include <avr/io.h>
@@ -67,6 +68,10 @@ uint8_t motor_stat_timer, motor_stat_timer_cpt;
/** Print PINC. */
uint8_t motor_print_pinc, motor_print_pinc_cpt;
+/** Print Sharps. */
+uint8_t motor_print_sharps, motor_print_sharps_cpt;
+uint16_t motor_sharps[3];
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3,
@@ -91,6 +96,7 @@ main (void)
speed_init ();
postrack_init ();
rs232_init ();
+ twi_master_init ();
proto_send0 ('z');
sei ();
main_loop ();
@@ -116,9 +122,7 @@ main_loop (void)
{
goto_update ();
if (goto_finish)
- {
proto_send0 ('F');
- }
}
motor_timer_2 = timer_read ();
/* Speed control. */
@@ -168,9 +172,19 @@ main_loop (void)
}
if (motor_print_pinc && !--motor_print_pinc_cpt)
{
- proto_send1b ('P', PINC);
+ proto_send1b ('P', PINA);
motor_print_pinc_cpt = motor_print_pinc;
}
+ if (motor_print_sharps && !--motor_print_sharps_cpt)
+ {
+ if (twi_master_is_finished ())
+ {
+ proto_send3w ('H', motor_sharps[0], motor_sharps[1],
+ motor_sharps[2]);
+ twi_master_read (0x02, (uint8_t *) motor_sharps, 6);
+ }
+ motor_print_sharps_cpt = motor_print_sharps;
+ }
/* Misc. */
if ((motor_loop_cpt & 7) == 0)
{
@@ -294,6 +308,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Input port stats. */
motor_print_pinc_cpt = motor_print_pinc = args[0];
break;
+ case c ('H', 1):
+ /* Sharps stats. */
+ twi_master_read (0x02, (uint8_t *) motor_sharps, 6);
+ motor_print_sharps_cpt = motor_print_sharps = args[0];
+ break;
default:
/* Params. */
if (cmd == 'p')
diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c
index 3cca141..197cc67 100644
--- a/n/asserv/src/postrack.c
+++ b/n/asserv/src/postrack.c
@@ -29,6 +29,8 @@ int32_t postrack_x, postrack_y;
int32_t postrack_a;
/** Distance between the weels, u16. */
uint16_t postrack_footing;
+/** Precomputed footing * 2pi, f24.8. */
+uint32_t postrack_footing_2pi;
/** Precomputed footing factor, f8.24.
* postrack_footing_factor = (1/2pi * 256) / postrack_footing
* Explanations:
@@ -114,6 +116,7 @@ static inline void
postrack_set_footing (uint16_t footing)
{
postrack_footing = footing;
+ postrack_footing_2pi = (uint32_t) (M_PI * 2 * 255) * footing;
postrack_footing_factor =
(int32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing;
}
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 8c67207..84f6f39 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -30,7 +30,7 @@ int8_t speed_left_aim, speed_right_aim;
/** Max speed. */
int8_t speed_max = 0x20;
/** Acceleration value. */
-uint8_t speed_acc = 8;
+uint8_t speed_acc = 4;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
/** Integral term. */
@@ -40,13 +40,15 @@ int16_t speed_int_max = 1024;
/** Last error value. */
int16_t speed_left_e_old, speed_right_e_old;
/** P coeficients. 4.8 fixed point format. */
-uint16_t speed_kp = 5 * 255;
+uint16_t speed_kp = 0x0200;
/** I coeficients. 3.8 fixed point format. */
-uint16_t speed_ki = 1 * 255;
+uint16_t speed_ki = 0x0004;
/** D coeficients. 3.8 fixed point format. */
-uint16_t speed_kd = 1 * 255;
+uint16_t speed_kd = 0x0100;
/** Counter value wanted. */
uint16_t speed_left_counter, speed_right_counter;
+/** Do not update counter. */
+uint8_t speed_counter_updated = 0;
/* +AutoDec */
@@ -77,10 +79,6 @@ speed_restart (void);
static inline void
speed_init (void)
{
- speed_acc = 8;
- speed_int_max = 1024;
- speed_kp = 2 * 255;
- speed_ki = 1 * 255;
}
/** Update speeds according to the wanted speeds and the acceleration. */
@@ -110,8 +108,15 @@ speed_update (void)
speed_right = speed_right_aim;
}
/* Update counter. */
- speed_left_counter += speed_left;
- speed_right_counter += speed_right;
+ if (speed_counter_updated)
+ {
+ speed_counter_updated = 0;
+ }
+ else
+ {
+ speed_left_counter += speed_left;
+ speed_right_counter += speed_right;
+ }
}
/** Compute new pwm value for left motor. */
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index b7e941d..61187b8 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -28,13 +28,18 @@ uint8_t taz_substate;
/** Positions. */
#define taz_scale 0.05026548245743669181
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 uint32_t taz_rear_32 = taz_rear_16 * 256;
static const uint16_t taz_front_16 = taz_scale * 270 / 2;
-static const uint32_t taz_front_32 = taz_scale * 270 / 2 * 256;
+static const uint32_t taz_front_32 = taz_front_16 * 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 uint32_t taz_side_32 = taz_side_16 * 256;
+
static const uint16_t taz_start_y_16 = taz_scale * 450 / 2;
+static const uint16_t taz_before_bridge_16 = taz_scale * 1050;
+static const uint16_t taz_after_bridge_16 = taz_scale * (1500 + 22 + 600 + 22
+ + 300);
+
/* +AutoDec */
/* -AutoDec */
@@ -58,12 +63,12 @@ taz_state_0 (void)
{
case 0:
/* Attend que l'on enfonce le jack. */
- if (PINC & _BV (3))
+ if (PINA & _BV (6))
taz_substate = 1;
break;
case 1:
/* Attend que l'on enlève le jack. */
- if (!(PINC & _BV (3)))
+ if (!(PINA & _BV (6)))
{
taz_substate = 2;
/* FTW. */
@@ -113,7 +118,7 @@ taz_state_0 (void)
}
break;
case 5:
- if (goto_finish && (PINC & _BV (3)))
+ if (goto_finish && (PINA & _BV (6)))
{
taz_substate = 6;
/* Recalage. */
@@ -121,7 +126,7 @@ taz_state_0 (void)
goto_a = 0;
}
case 6:
- if (!(PINC & _BV (3)))
+ if (!(PINA & _BV (6)))
{
taz_state = 1;
taz_substate = 0;