From 0fcaebc243f457973fcb1a83fe8a00705be72f20 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 17 Apr 2005 02:53:57 +0000 Subject: PINC -> PINA. Move_angular ok. Sharps en twi. --- n/asserv/src/Makefile | 4 ++-- n/asserv/src/goto.c | 39 +++++++++++++++++++++++++-------------- n/asserv/src/main.c | 25 ++++++++++++++++++++++--- n/asserv/src/postrack.c | 3 +++ n/asserv/src/speed.c | 25 +++++++++++++++---------- n/asserv/src/taz.c | 19 ++++++++++++------- 6 files changed, 79 insertions(+), 36 deletions(-) (limited to 'n/asserv/src') 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 #include #include +#include #include #include @@ -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; -- cgit v1.2.3