gobeur de gobelets, gestion de la turbine et du servomoteur en automateet du message ROS lié
Dependents: stm32_actuator_gobeur
Gobeur.cpp
- Committer:
- kyxstark
- Date:
- 2021-07-08
- Revision:
- 3:44a200543540
- Child:
- 5:64a88f154e65
File content as of revision 3:44a200543540:
#include "Gobeur.h" Gobeur::Gobeur(PinName pin_turbine, PinName pin_servo, float turbine_on, float turbine_off, float servo_open, float servo_close): _order(release), _turbine_on(turbine_on), _turbine_off(turbine_off), _servo_open(servo_open), _servo_close(servo_close) { _turbine = new Servo(pin_turbine); _servo = new Servo(pin_servo); *_turbine = _turbine_off; *_servo = _servo_close; } void Gobeur::update_order(int order) { if(order == 0) _order = suck; if(order == 1) _order = release; } void Gobeur::automate(void) { switch(_state) { case released: if( (_order == suck ) || ((clock_s() - _time) > 2*GOBEUR_PAUSE_TIMEOUT)&&1 ) { _state = sucking; // *_servo = GOBEUR_SERVO_POS_OPEN; *_turbine = _turbine_on; _time = clock_s(); } break; case sucking: if( (clock_s() - _time) > GOBEUR_SUCKING_TIMEOUT) //TODO add switch { _state = closing; *_servo = _servo_close; // *_turbine = GOBEUR_TURBINE_TH_ON; _time = clock_s(); } break; case closing: if( (clock_s() - _time) > GOBEUR_CLOSING_TIMEOUT) //TODO add switch { _state = sucked; // *_servo = GOBEUR_SERVO_POS_CLOSE; *_turbine = _turbine_off; _time = clock_s(); // TODO send ack } break; case sucked: if( (_order == release ) || ((clock_s() - _time) > GOBEUR_PAUSE_TIMEOUT)&&1 ) { _state = releasing; *_servo = _servo_open; // *_turbine = GOBEUR_TURBINE_TH_OFF; _time = clock_s(); } break; case releasing: if((clock_s() - _time) > GOBEUR_OPENING_TIMEOUT) //TODO add switch { _state = released; // *_servo = GOBEUR_SERVO_POS_OPEN; // *_turbine = GOBEUR_TURBINE_TH_OFF; _time = clock_s(); // TODO send ack } break; } }