From 2e745143d2b02249ac268b6372c4f976b4b03ed2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 12 May 2010 16:19:46 +0200 Subject: digital/asserv, digital/io: set zero PWM when elevator down --- digital/asserv/src/asserv/twi_proto.c | 13 +++++++++++++ digital/io/src/ai_loader_cb.c | 2 ++ digital/io/src/asserv.c | 18 ++++++++++++++++++ digital/io/src/asserv.h | 8 ++++++++ digital/io/src/loader.fsm | 1 + 5 files changed, 42 insertions(+) (limited to 'digital') diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 1be7dd92..60b8bdc8 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -227,6 +227,19 @@ twi_proto_callback (u8 *buf, u8 size) * - b: speed. */ aux_traj_find_zero_reverse_start (&aux[1], buf[2], 0); break; + case c ('r', 1): + /* Set aux zero pwm. + * - b: aux index. + */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + pos_reset (&pos_aux[buf[2]]); + state_aux[buf[2]].mode = MODE_PWM; + pwm_set (&pwm_aux[buf[2]], 0); + } + else + buf[0] = 0; + break; case c ('p', x): /* Set parameters. */ if (twi_proto_params (&buf[2], size - 2) != 0) diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 0df20434..7f1a6073 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -189,11 +189,13 @@ ai__LOADER_UPING__elevator_failed (void) /* * LOADER_DOWNING =elevator_succeed=> * => LOADER_DOWN + * release elevator motor * post loader_downed event */ fsm_branch_t ai__LOADER_DOWNING__elevator_succeed (void) { + asserv_motor0_free (); main_post_event_for_top_fsm = AI_EVENT_loader_downed; return ai_next (LOADER_DOWNING, elevator_succeed); } diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index f826ba15..a60ca55b 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -432,3 +432,21 @@ asserv_motor1_zero_position (int8_t speed) twi_master_send (2); } +void +asserv_motor0_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); + buffer[0] = 'r'; + buffer[1] = 0; + twi_master_send (2); +} + +void +asserv_motor1_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); + buffer[0] = 'r'; + buffer[1] = 1; + twi_master_send (2); +} + diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 14bd959d..a92d95f7 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -293,4 +293,12 @@ asserv_motor0_zero_position (int8_t speed); void asserv_motor1_zero_position (int8_t speed); +/** Set PWM to zero for motor0. */ +void +asserv_motor0_free (void); + +/** Set PWM to zero for motor1. */ +void +asserv_motor1_free (void); + #endif /* asserv_h */ diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index c97941b9..98b18da2 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -105,6 +105,7 @@ LOADER_ERROR: LOADER_DOWNING: elevator_succeed -> LOADER_DOWN + release elevator motor post loader_downed event elevator_failed -> LOADER_UPING something is blocking, move it up -- cgit v1.2.3