UTCoupe / Gobeur

gobeur de gobelets, gestion de la turbine et du servomoteur en automateet du message ROS lié

Dependents:   stm32_actuator_gobeur

Gobeur.cpp@3:44a200543540, 2021-07-08 (annotated)

Committer:
kyxstark
Date:
Thu Jul 08 14:09:26 2021 +0000
Revision:
3:44a200543540
Child:
5:64a88f154e65
automate turbine+ servo;

Who changed what in which revision?

UserRevisionLine numberNew 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 3:44a200543540 24 if( (_order == suck ) || ((clock_s() - _time) > 2*GOBEUR_PAUSE_TIMEOUT)&&1 )
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 3:44a200543540 52 if( (_order == release ) || ((clock_s() - _time) > GOBEUR_PAUSE_TIMEOUT)&&1 )
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 }