gestion de servomoteur individuelement, et du message ROS associe
Dependents: stm32_actuator_gobeur
Manche_a_air.cpp
- Committer:
- kyxstark
- Date:
- 2021-07-08
- Revision:
- 5:04d111f63a0f
- Parent:
- 3:40988c59e571
File content as of revision 5:04d111f63a0f:
#include "Manche_a_air.h" Manche_a_air::Manche_a_air(PinName pin_servo, float servo_open, float servo_close): _order(close), _servo_open(servo_open), _servo_close(servo_close), _flag_auto_open(0), _flag_auto_close(0) { _servo = new Servo(pin_servo); *_servo = _servo_close; } void Manche_a_air::update_order(int order) { if(order == 0) _order = close; if(order == 1) _order = open; } void Manche_a_air::set_auto_open(int flag, float timeout) { _flag_auto_open = flag; _timeout_auto_open = timeout; _time = clock_s(); } void Manche_a_air::set_auto_close(int flag, float timeout) { _flag_auto_close = flag; _timeout_auto_close = timeout; _time = clock_s(); } void Manche_a_air::automate(void) { float elapsed = clock_s() - _time; switch(_state) { case closed: if( (_order == open && 1) || ( (elapsed>_timeout_auto_open) && _flag_auto_open ) ) { _state = opening; *_servo = _servo_open; _time = clock_s(); _order = open; } break; case opening: if( elapsed > MAA_OPENING_TIMEOUT) //TODO add ack { _state = opened; _time = clock_s(); } break; case opened: if( (_order == close && 1) || ( (elapsed>_timeout_auto_close) && _flag_auto_close ) ) { _state = closing; *_servo = _servo_close; _time = clock_s(); _order = close; } break; case closing: if( elapsed > MAA_CLOSING_TIMEOUT) //TODO add ack { _state = closed; _time = clock_s(); // TODO send ack } break; } }