From 89151bed20a5d9073bb304057041b2453eec16bb Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 17 Apr 2005 11:09:18 +0000 Subject: Plein de truc de la pré coupe. --- n/asserv/src/goto.c | 6 +- n/asserv/src/main.c | 12 +++ n/asserv/src/speed.c | 2 +- n/asserv/src/taz.c | 291 ++++++++++++++++++++++++++++++++++++++++++++++----- 4 files changed, 280 insertions(+), 31 deletions(-) (limited to 'n/asserv/src') diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index 553a4f4..64d1eac 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -88,7 +88,7 @@ goto_angular_mode (void) angle_diff <<= 8; angle_diff >>= 8; /* Small angles. */ - if (0x10000 > angle_diff && angle_diff < -0x10000) + if (0x10000L > angle_diff && angle_diff > -0x10000L) { goto_finish = 1; speed_left = 0; @@ -182,12 +182,12 @@ goto_ftw_mode (void) speed_right = 0; speed_right_aim = 0; } - if (!(PINA & (_BV (0) || _BV (7)))) + if (!(PINA & (_BV (0) | _BV (7)))) goto_finish = 1; } /** Update the speed according to the desired destination. */ -static inline void +static void goto_update (void) { switch (goto_mode) diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 1715d5f..1513876 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -38,6 +38,9 @@ #include "postrack.c" #include "goto.c" +/** Use Taz. */ +uint8_t motor_taz; + /** Motor mode : * 0 - pwm setup. * 1 - speed control. @@ -77,6 +80,8 @@ uint16_t motor_sharps[3]; uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3, motor_timer_4; +#include "taz.c" + /* +AutoDec */ /* Main loop. */ @@ -97,6 +102,7 @@ main (void) postrack_init (); rs232_init (); twi_master_init (); + taz_init (); proto_send0 ('z'); sei (); main_loop (); @@ -117,6 +123,9 @@ main_loop (void) /* Position tracking. */ postrack_update (); motor_timer_3 = timer_read (); + /* Taz! */ + if (motor_taz) + taz_update (); /* Position control. */ if (motor_mode >= 2) { @@ -206,6 +215,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) reset (); break; /* Commands. */ + case c ('t', 0): + motor_taz = 1; + break; case c ('l', 2): /* Linear move. * - w: distance (negative to go backward). */ diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c index 84f6f39..8050b29 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 = 4; +uint8_t speed_acc = 8; /** Acceleration counter, speed gets updated when it reachs 0. */ uint8_t speed_acc_cpt; /** Integral term. */ diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index 61187b8..c165e33 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -25,20 +25,45 @@ /** FSM state. */ uint8_t taz_state; uint8_t taz_substate; +uint8_t taz_tempo; /** Positions. */ -#define taz_scale 0.05026548245743669181 +//#define taz_scale (1.0/0.05026548245743669181) +#define taz_scale (1.0/.05090591755319148936) static const uint16_t taz_rear_16 = taz_scale * 270 / 2; -static const uint32_t taz_rear_32 = taz_rear_16 * 256; +static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256; static const uint16_t taz_front_16 = taz_scale * 270 / 2; -static const uint32_t taz_front_32 = taz_front_16 * 256; +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_side_16 * 256; +static const uint32_t taz_side_32 = taz_scale * 340 / 2 * 256; -static const uint16_t taz_start_y_16 = taz_scale * 450 / 2; +static const uint16_t taz_start_y_16 = taz_scale * (450 - 50) / 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); +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_after_bridge_16[4] = +{ + taz_scale * (OR2), + taz_scale * (OR2 + 150), + taz_scale * (OR2 + 150), + taz_scale * (OR2 + 150) +}; +static const uint8_t taz_ang[4] = +{ + -18, + -10, + 13, + -8 +}; +static const uint16_t taz_ang_dist_16[4] = +{ + taz_scale * (496), + taz_scale * (290), + taz_scale * (316), + taz_scale * (306) +}; + +uint8_t taz_pont; /* +AutoDec */ /* -AutoDec */ @@ -51,30 +76,28 @@ static const uint16_t taz_after_bridge_16 = taz_scale * (1500 + 22 + 600 + 22 static inline void taz_init (void) { - taz_state = 0; - taz_substate = 0; } /** Mise en place avant le départ. */ -static inline void +static void taz_state_0 (void) { switch (taz_substate) { case 0: /* Attend que l'on enfonce le jack. */ - if (PINA & _BV (6)) + if (!(PINA & _BV (6))) taz_substate = 1; break; case 1: /* Attend que l'on enlève le jack. */ - if (!(PINA & _BV (6))) + if (PINA & _BV (6)) { taz_substate = 2; /* FTW. */ motor_mode = 2; goto_mode = 10; - goto_s = -0x10; + goto_s = -10; goto_finish = 0; } break; @@ -83,13 +106,14 @@ taz_state_0 (void) { taz_substate = 3; /* Recalage. */ - goto_y = taz_rear_32; - goto_a = 0x00400000; + speed_restart (); + postrack_y = -taz_rear_32; + postrack_a = 0x00c00000; /* On avance juste qu'à l'y de départ. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; - goto_d = taz_start_y - taz_rear; + goto_d = taz_start_y_16 - taz_rear_16; goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; @@ -113,40 +137,243 @@ taz_state_0 (void) /* FTW. */ motor_mode = 2; goto_mode = 10; - goto_s = -0x10; + goto_s = -10; goto_finish = 0; } break; case 5: - if (goto_finish && (PINA & _BV (6))) + if (goto_finish) { taz_substate = 6; /* Recalage. */ - goto_x = taz_rear_32; - goto_a = 0; + speed_restart (); + postrack_x = taz_rear_32; + postrack_a = 0; } + break; case 6: if (!(PINA & _BV (6))) + { + taz_substate = 7; + } + break; + case 7: + if (PINA & _BV (6)) { taz_state = 1; taz_substate = 0; } + break; } } /** Avancée vers le pont. */ -static inline void +static void taz_state_1 (void) { switch (taz_substate) { case 0: + taz_substate = 1; + /* On avance juste qu'au pont. */ + motor_mode = 2; + goto_mode = 0; + goto_sign = 0; + goto_d = taz_before_bridge_16 - taz_rear_16; + goto_x = postrack_x; + goto_y = postrack_y; + goto_finish = 0; + break; + case 1: + if (goto_finish) + { + taz_substate = 2; + taz_tempo = 250; + } + break; + case 2: + taz_substate = 3; + /* Lance la capture. */ + twi_master_read (0x02, (uint8_t *) motor_sharps, 6); + break; + case 3: + if (twi_master_is_finished ()) + { + taz_substate = 4; + proto_send3w ('H', motor_sharps[0], motor_sharps[1], + motor_sharps[2]); + /* Calcule la valeur du pont. */ + if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 + && motor_sharps[2] > 0x117) + taz_pont = 0; + else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 + && motor_sharps[2] > 0x117) + taz_pont = 1; + else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 + && motor_sharps[2] > 0x117) + taz_pont = 2; + else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 + && motor_sharps[2] < 0x117) + taz_pont = 3; + else + { + taz_substate = 1; + break; + } + /* Direction l'axe des y. */ + motor_mode = 2; + goto_mode = 1; + goto_a = 0xc0; + goto_finish = 0; + } + break; + case 4: + if (goto_finish) + { + taz_substate = 5; + /* FTW. */ + motor_mode = 2; + goto_mode = 10; + goto_s = -10; + goto_finish = 0; + } + break; + case 5: + if (goto_finish) + { + taz_substate = 6; + /* Recalage. */ + speed_restart (); + postrack_y = taz_rear_32; + postrack_a = 0x00c00000; + /* On avance juste qu'à l'y de traversée de pont. */ + motor_mode = 2; + goto_mode = 0; + goto_sign = 0; + goto_d = taz_start_y_16 - taz_rear_16 + taz_pont * + taz_brige_interval_16; + goto_x = postrack_x; + goto_y = postrack_y; + goto_finish = 0; + } + break; + case 6: + if (goto_finish) + { + taz_substate = 7; + /* Direction l'axe des x. */ + motor_mode = 2; + goto_mode = 1; + goto_a = 0; + goto_finish = 0; + } + break; + case 7: + if (goto_finish) + { + taz_substate = 8; + /* Reprend une mesure. */ + twi_master_read (0x02, (uint8_t *) motor_sharps, 6); + } + break; + case 8: + if (twi_master_is_finished ()) + { + taz_substate = 9; + proto_send3w ('H', motor_sharps[0], motor_sharps[1], + motor_sharps[2]); + /* Calcule la valeur du pont. */ + if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 + && motor_sharps[2] > 0x117) + { + /* Traversée du pont. */ + motor_mode = 2; + goto_mode = 0; + goto_sign = 0; + goto_d = taz_after_bridge_16[taz_pont] - (postrack_x >> 8); + goto_x = postrack_x; + goto_y = postrack_y; + goto_finish = 0; + break; + } + else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 + && motor_sharps[2] > 0x117 && taz_pont < 3) + { + taz_pont++; + } + else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 + && motor_sharps[2] > 0x117 && taz_pont < 2) + { + taz_pont += 2; + } + else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 + && motor_sharps[2] < 0x117 && taz_pont < 1) + { + taz_pont += 3; + } + else if (motor_sharps[0] > 0x117 && motor_sharps[1] < 0x117 + && motor_sharps[2] < 0x117 && taz_pont > 1) + { + taz_pont -= 2; + } + else if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 + && motor_sharps[2] < 0x117 && taz_pont > 0) + { + taz_pont--; + } + else + { + taz_pont = 0; + } + /* Direction l'axe des y. */ + motor_mode = 2; + goto_mode = 1; + goto_a = 0xc0; + goto_finish = 0; + taz_substate = 4; + } + break; + case 9: + if (goto_finish) + { + taz_substate = 10; + /* Tourne vers les quilles. */ + motor_mode = 2; + goto_mode = 1; + goto_a = taz_ang[taz_pont]; + goto_finish = 0; + } + break; + case 10: + if (goto_finish) + { + taz_substate = 11; + /* Avance la petite distance, pour arriver sur la première quille. */ + motor_mode = 2; + goto_mode = 0; + goto_sign = 0; + goto_d = taz_ang_dist_16[taz_pont]; + goto_x = postrack_x; + goto_y = postrack_y; + goto_finish = 0; + } + break; + case 11: + if (goto_finish) + { + taz_substate = 12; + /* Tourne vers les quilles. */ + motor_mode = 2; + goto_mode = 1; + goto_a = 0; + goto_finish = 0; + } break; } } /** Traversée du pont. */ -static inline void +static void taz_state_2 (void) { switch (taz_substate) @@ -157,7 +384,7 @@ taz_state_2 (void) } /** . */ -static inline void +static void taz_state_3 (void) { switch (taz_substate) @@ -168,7 +395,7 @@ taz_state_3 (void) } /** . */ -static inline void +static void taz_state_4 (void) { switch (taz_substate) @@ -179,7 +406,7 @@ taz_state_4 (void) } /** . */ -static inline void +static void taz_state_5 (void) { switch (taz_substate) @@ -190,9 +417,17 @@ taz_state_5 (void) } /** FSM. */ -static inline void +static void taz_update (void) { + uint8_t old_state, old_substate; + old_state = taz_state; + old_substate = taz_substate; + if (taz_tempo) + { + taz_tempo--; + return; + } switch (taz_state) { case 0: @@ -214,4 +449,6 @@ taz_update (void) taz_state_5 (); break; } + if (old_state != taz_state || old_substate != taz_substate) + proto_send2b ('t', taz_state, taz_substate); } -- cgit v1.2.3