gobeur de gobelets, gestion de la turbine et du servomoteur en automateet du message ROS lié
Dependents: stm32_actuator_gobeur
Gobeur.cpp@5:64a88f154e65, 2021-07-08 (annotated)
- Committer:
- kyxstark
- Date:
- Thu Jul 08 22:45:59 2021 +0000
- Revision:
- 5:64a88f154e65
- Parent:
- 3:44a200543540
peut de modif;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kyxstark | 3:44a200543540 | 1 | #include "Gobeur.h" |
kyxstark | 3:44a200543540 | 2 | |
kyxstark | 3:44a200543540 | 3 | |
kyxstark | 3:44a200543540 | 4 | Gobeur::Gobeur(PinName pin_turbine, PinName pin_servo, float turbine_on, float turbine_off, float servo_open, float servo_close): |
kyxstark | 3:44a200543540 | 5 | _order(release), _turbine_on(turbine_on), _turbine_off(turbine_off), _servo_open(servo_open), _servo_close(servo_close) |
kyxstark | 3:44a200543540 | 6 | { |
kyxstark | 3:44a200543540 | 7 | _turbine = new Servo(pin_turbine); |
kyxstark | 3:44a200543540 | 8 | _servo = new Servo(pin_servo); |
kyxstark | 3:44a200543540 | 9 | *_turbine = _turbine_off; |
kyxstark | 3:44a200543540 | 10 | *_servo = _servo_close; |
kyxstark | 3:44a200543540 | 11 | |
kyxstark | 3:44a200543540 | 12 | } |
kyxstark | 3:44a200543540 | 13 | |
kyxstark | 3:44a200543540 | 14 | void Gobeur::update_order(int order) |
kyxstark | 3:44a200543540 | 15 | { |
kyxstark | 3:44a200543540 | 16 | if(order == 0) _order = suck; |
kyxstark | 3:44a200543540 | 17 | if(order == 1) _order = release; |
kyxstark | 3:44a200543540 | 18 | } |
kyxstark | 3:44a200543540 | 19 | void Gobeur::automate(void) |
kyxstark | 3:44a200543540 | 20 | { |
kyxstark | 3:44a200543540 | 21 | switch(_state) |
kyxstark | 3:44a200543540 | 22 | { |
kyxstark | 3:44a200543540 | 23 | case released: |
kyxstark | 5:64a88f154e65 | 24 | if( (_order == suck ) || ((clock_s() - _time) > 2*GOBEUR_PAUSE_TIMEOUT)&&0 ) |
kyxstark | 3:44a200543540 | 25 | { |
kyxstark | 3:44a200543540 | 26 | _state = sucking; |
kyxstark | 3:44a200543540 | 27 | // *_servo = GOBEUR_SERVO_POS_OPEN; |
kyxstark | 3:44a200543540 | 28 | *_turbine = _turbine_on; |
kyxstark | 3:44a200543540 | 29 | _time = clock_s(); |
kyxstark | 3:44a200543540 | 30 | } |
kyxstark | 3:44a200543540 | 31 | break; |
kyxstark | 3:44a200543540 | 32 | case sucking: |
kyxstark | 3:44a200543540 | 33 | if( (clock_s() - _time) > GOBEUR_SUCKING_TIMEOUT) //TODO add switch |
kyxstark | 3:44a200543540 | 34 | { |
kyxstark | 3:44a200543540 | 35 | _state = closing; |
kyxstark | 3:44a200543540 | 36 | *_servo = _servo_close; |
kyxstark | 3:44a200543540 | 37 | // *_turbine = GOBEUR_TURBINE_TH_ON; |
kyxstark | 3:44a200543540 | 38 | _time = clock_s(); |
kyxstark | 3:44a200543540 | 39 | } |
kyxstark | 3:44a200543540 | 40 | break; |
kyxstark | 3:44a200543540 | 41 | case closing: |
kyxstark | 3:44a200543540 | 42 | if( (clock_s() - _time) > GOBEUR_CLOSING_TIMEOUT) //TODO add switch |
kyxstark | 3:44a200543540 | 43 | { |
kyxstark | 3:44a200543540 | 44 | _state = sucked; |
kyxstark | 3:44a200543540 | 45 | // *_servo = GOBEUR_SERVO_POS_CLOSE; |
kyxstark | 3:44a200543540 | 46 | *_turbine = _turbine_off; |
kyxstark | 3:44a200543540 | 47 | _time = clock_s(); |
kyxstark | 3:44a200543540 | 48 | // TODO send ack |
kyxstark | 3:44a200543540 | 49 | } |
kyxstark | 3:44a200543540 | 50 | break; |
kyxstark | 3:44a200543540 | 51 | case sucked: |
kyxstark | 5:64a88f154e65 | 52 | if( (_order == release ) || ((clock_s() - _time) > GOBEUR_PAUSE_TIMEOUT)&&0) |
kyxstark | 3:44a200543540 | 53 | { |
kyxstark | 3:44a200543540 | 54 | _state = releasing; |
kyxstark | 3:44a200543540 | 55 | *_servo = _servo_open; |
kyxstark | 3:44a200543540 | 56 | // *_turbine = GOBEUR_TURBINE_TH_OFF; |
kyxstark | 3:44a200543540 | 57 | _time = clock_s(); |
kyxstark | 3:44a200543540 | 58 | } |
kyxstark | 3:44a200543540 | 59 | break; |
kyxstark | 3:44a200543540 | 60 | case releasing: |
kyxstark | 3:44a200543540 | 61 | if((clock_s() - _time) > GOBEUR_OPENING_TIMEOUT) //TODO add switch |
kyxstark | 3:44a200543540 | 62 | { |
kyxstark | 3:44a200543540 | 63 | _state = released; |
kyxstark | 3:44a200543540 | 64 | // *_servo = GOBEUR_SERVO_POS_OPEN; |
kyxstark | 3:44a200543540 | 65 | // *_turbine = GOBEUR_TURBINE_TH_OFF; |
kyxstark | 3:44a200543540 | 66 | _time = clock_s(); |
kyxstark | 3:44a200543540 | 67 | // TODO send ack |
kyxstark | 3:44a200543540 | 68 | } |
kyxstark | 3:44a200543540 | 69 | break; |
kyxstark | 3:44a200543540 | 70 | } |
kyxstark | 3:44a200543540 | 71 | } |