From 805d0b0005a4d9ecdb3d764e7f989e2debbfc1b9 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 01:36:40 +0200 Subject: * digital/io/src * main - move the place where the updates of the sharp and switch modules is made. * eeprom - add command to dump the eeprom configuration. * sharp - integrate sharp module update into main ; - add function to get raw and interpreted stats of sharps ; --- digital/io/src/main.c | 132 +++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 110 insertions(+), 22 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 608a0bad..46b171c6 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -45,7 +45,7 @@ #include "top.h" /* top_* */ #include "chrono.h" /* chrono_end_match */ #include "gutter.h" /* gutter_generate_wait_finished_event */ -#include "sharp.h" +#include "sharp.h" /* sharp module */ #include "io.h" @@ -73,6 +73,17 @@ uint8_t main_post_event_for_top_fsm = 0xFF; * Sharps stats counters. */ uint8_t main_stats_sharps, main_stats_sharps_cpt; +uint8_t main_stats_sharps_interpreted_, main_stats_sharps_interpreted_cpt_; + +/** + * Update frequency of sharps. + */ +#define MAIN_SHARP_UPDATE_FREQ 20 + +/** + * Sharps frequency counter. + */ +uint8_t main_sharp_freq_counter_; /** * Initialize the main and all its subsystems. @@ -139,9 +150,6 @@ main_loop (void) /* Update TWI module to get new data from the asserv board */ asserv_update_status (); - /* Update switch module */ - switch_update (); - /* Is last command has been acknowledged? */ if (asserv_last_cmd_ack () == 0) { @@ -150,12 +158,25 @@ main_loop (void) } else { - /* Update */ + /* First update modules */ + /* Update switch module */ + switch_update (); + /* Sharps module */ + if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ) + { + /* Update sharps */ + sharp_update (0xff); + /* Reset counter */ + main_sharp_freq_counter_ = 0; + } + + /* Update main */ uint8_t main_asserv_arm_position_reached = asserv_arm_position_reached (); uint8_t main_top_generate_settings_ack_event = top_generate_settings_ack_event (); uint8_t main_gutter_generate_wait_finished_event = gutter_generate_wait_finished_event (); asserv_status_e move_status = asserv_last_cmd_ack () ? asserv_move_cmd_status () : none; + /* Check commands move status */ if (move_status == success) { @@ -225,16 +246,35 @@ main_loop (void) /* TODO: Check other sensors */ } - /* Send Sharps stats. */ + /* Send Sharps raw stats. */ if (main_stats_sharps && !--main_stats_sharps_cpt) { + uint8_t count; + uint8_t cache[SHARP_NUMBER * 2]; + /* Reset counter */ main_stats_sharps_cpt = main_stats_sharps; - /* XXX: Temporary put here. */ - sharp_update (0xff); - proto_send4w ('H', sharp_get_raw (0), - sharp_get_raw (1), - sharp_get_raw (2), - sharp_get_raw (3)); + for (count = 0; count < SHARP_NUMBER; count++) + { + uint16_t tmp = sharp_get_raw (count); + cache[count * 2] = v16_to_v8 (tmp, 1); + cache[count * 2 + 1] = v16_to_v8 (tmp, 0); + } + proto_send ('H', 2 * SHARP_NUMBER, cache); + } + /* Send Sharps interpreted stats. */ + if (main_stats_sharps_interpreted_ && + !--main_stats_sharps_interpreted_cpt_) + { + uint8_t count; + uint8_t cache[SHARP_NUMBER]; + /* Reset counter */ + main_stats_sharps_interpreted_cpt_ = + main_stats_sharps_interpreted_; + for (count = 0; count < SHARP_NUMBER; count++) + { + cache[count] = sharp_get_interpreted (count); + } + proto_send ('I', SHARP_NUMBER, cache); } } } @@ -300,22 +340,70 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('H', 1): + /* Print raw stats for sharps. + * - 1b: frequency of sharp stats. + */ main_stats_sharps_cpt = main_stats_sharps = args[0]; break; - /* EEPROM command */ - case c ('e', 1): - /* Save/clear config - * - 1b: - * - 00: clear config - * - other values: save config + case c ('I', 1): + /* Print interpreted stats for sharps. + * - 1b: frequency of sharp stats. */ - if (args[0] == 0) - eeprom_clear_param (); - else - eeprom_save_param (); + main_stats_sharps_interpreted_cpt_ = main_stats_sharps_interpreted_ = + args[0]; + break; + + case c ('h', 5): + /* Configure sharps threshold. + * - 1b: sharp id number; + * - 2b: sharp low threshold; + * - 2b: sharp high threshold. + */ + sharp_set_threshold (args[0], v8_to_v16 (args[1], args[2]), + v8_to_v16 (args[3], args[4])); break; + /* EEPROM command */ + case c ('e', 1): + { + /* Save/clear config + * - 1b: + * - 00, 'c': clear config + * - 01, 's': save config + * - 02, 'd': dump config + */ + switch (args[0]) + { + case 0: + case 'c': + eeprom_clear_param (); + break; + case 1: + case 's': + eeprom_save_param (); + break; + case 2: + case 'd': + { + uint8_t compt; + /* Trap */ + for (compt = 0; compt < SERVO_NUMBER; compt++) + { + proto_send2b ('t', trap_high_time_pos[0][compt], + trap_high_time_pos[1][compt]); + } + /* Sharp */ + for (compt = 0; compt < SHARP_NUMBER; compt++) + { + proto_send2w ('h', sharp_threshold[0][compt], + sharp_threshold[1][compt]); + } + } + break; + } + break; + } /* FSM commands */ case c ('g', 2): /* Start the get samples FSM -- cgit v1.2.3