Kyx Stark / Manche_a_air

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?

UserRevisionLine numberNew 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 }