gestion de servomoteur individuelement, et du message ROS associe
Dependents: stm32_actuator_gobeur
Manche_a_air.cpp@5:04d111f63a0f, 2021-07-08 (annotated)
- Committer:
- kyxstark
- Date:
- Thu Jul 08 22:46:14 2021 +0000
- Revision:
- 5:04d111f63a0f
- Parent:
- 3:40988c59e571
peut de modif
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kyxstark | 3:40988c59e571 | 1 | #include "Manche_a_air.h" |
kyxstark | 3:40988c59e571 | 2 | |
kyxstark | 3:40988c59e571 | 3 | |
kyxstark | 3:40988c59e571 | 4 | Manche_a_air::Manche_a_air(PinName pin_servo, float servo_open, float servo_close): |
kyxstark | 3:40988c59e571 | 5 | _order(close), _servo_open(servo_open), _servo_close(servo_close), _flag_auto_open(0), _flag_auto_close(0) |
kyxstark | 3:40988c59e571 | 6 | { |
kyxstark | 3:40988c59e571 | 7 | _servo = new Servo(pin_servo); |
kyxstark | 3:40988c59e571 | 8 | *_servo = _servo_close; |
kyxstark | 3:40988c59e571 | 9 | |
kyxstark | 3:40988c59e571 | 10 | } |
kyxstark | 3:40988c59e571 | 11 | |
kyxstark | 3:40988c59e571 | 12 | void Manche_a_air::update_order(int order) |
kyxstark | 3:40988c59e571 | 13 | { |
kyxstark | 3:40988c59e571 | 14 | if(order == 0) _order = close; |
kyxstark | 3:40988c59e571 | 15 | if(order == 1) _order = open; |
kyxstark | 3:40988c59e571 | 16 | } |
kyxstark | 3:40988c59e571 | 17 | |
kyxstark | 3:40988c59e571 | 18 | void Manche_a_air::set_auto_open(int flag, float timeout) |
kyxstark | 3:40988c59e571 | 19 | { |
kyxstark | 3:40988c59e571 | 20 | _flag_auto_open = flag; |
kyxstark | 3:40988c59e571 | 21 | _timeout_auto_open = timeout; |
kyxstark | 3:40988c59e571 | 22 | _time = clock_s(); |
kyxstark | 3:40988c59e571 | 23 | } |
kyxstark | 3:40988c59e571 | 24 | void Manche_a_air::set_auto_close(int flag, float timeout) |
kyxstark | 3:40988c59e571 | 25 | { |
kyxstark | 3:40988c59e571 | 26 | _flag_auto_close = flag; |
kyxstark | 3:40988c59e571 | 27 | _timeout_auto_close = timeout; |
kyxstark | 3:40988c59e571 | 28 | _time = clock_s(); |
kyxstark | 3:40988c59e571 | 29 | } |
kyxstark | 3:40988c59e571 | 30 | |
kyxstark | 3:40988c59e571 | 31 | |
kyxstark | 3:40988c59e571 | 32 | |
kyxstark | 3:40988c59e571 | 33 | void Manche_a_air::automate(void) |
kyxstark | 3:40988c59e571 | 34 | { |
kyxstark | 3:40988c59e571 | 35 | float elapsed = clock_s() - _time; |
kyxstark | 3:40988c59e571 | 36 | switch(_state) |
kyxstark | 3:40988c59e571 | 37 | { |
kyxstark | 3:40988c59e571 | 38 | case closed: |
kyxstark | 5:04d111f63a0f | 39 | if( (_order == open && 1) || ( (elapsed>_timeout_auto_open) && _flag_auto_open ) ) |
kyxstark | 3:40988c59e571 | 40 | { |
kyxstark | 3:40988c59e571 | 41 | _state = opening; |
kyxstark | 3:40988c59e571 | 42 | *_servo = _servo_open; |
kyxstark | 3:40988c59e571 | 43 | _time = clock_s(); |
kyxstark | 5:04d111f63a0f | 44 | _order = open; |
kyxstark | 3:40988c59e571 | 45 | } |
kyxstark | 3:40988c59e571 | 46 | break; |
kyxstark | 3:40988c59e571 | 47 | case opening: |
kyxstark | 3:40988c59e571 | 48 | if( elapsed > MAA_OPENING_TIMEOUT) //TODO add ack |
kyxstark | 3:40988c59e571 | 49 | { |
kyxstark | 3:40988c59e571 | 50 | _state = opened; |
kyxstark | 3:40988c59e571 | 51 | _time = clock_s(); |
kyxstark | 3:40988c59e571 | 52 | } |
kyxstark | 3:40988c59e571 | 53 | break; |
kyxstark | 3:40988c59e571 | 54 | case opened: |
kyxstark | 5:04d111f63a0f | 55 | if( (_order == close && 1) || ( (elapsed>_timeout_auto_close) && _flag_auto_close ) ) |
kyxstark | 3:40988c59e571 | 56 | { |
kyxstark | 3:40988c59e571 | 57 | _state = closing; |
kyxstark | 3:40988c59e571 | 58 | *_servo = _servo_close; |
kyxstark | 3:40988c59e571 | 59 | _time = clock_s(); |
kyxstark | 5:04d111f63a0f | 60 | _order = close; |
kyxstark | 3:40988c59e571 | 61 | } |
kyxstark | 3:40988c59e571 | 62 | break; |
kyxstark | 3:40988c59e571 | 63 | case closing: |
kyxstark | 3:40988c59e571 | 64 | if( elapsed > MAA_CLOSING_TIMEOUT) //TODO add ack |
kyxstark | 3:40988c59e571 | 65 | { |
kyxstark | 3:40988c59e571 | 66 | _state = closed; |
kyxstark | 3:40988c59e571 | 67 | _time = clock_s(); |
kyxstark | 3:40988c59e571 | 68 | // TODO send ack |
kyxstark | 3:40988c59e571 | 69 | } |
kyxstark | 3:40988c59e571 | 70 | break; |
kyxstark | 3:40988c59e571 | 71 | } |
kyxstark | 3:40988c59e571 | 72 | } |