diff --git a/stm32/include/actionneurs/benne.h b/stm32/include/actionneurs/benne.h index b12c6e8c4a92d790b504dff71588bcd6aed0c023..bed305e760841a96012076c12021ebaa4e1621af 100644 --- a/stm32/include/actionneurs/benne.h +++ b/stm32/include/actionneurs/benne.h @@ -8,11 +8,23 @@ class Benne /** * The ID of the AX12 used to drive the belts */ - static const int SERVO_ID = 0; // To update + static const int BELTS_SERVO_ID = 0; // To update static const int FORWARD_SPEED = 1023; static const int BACKWARD_SPEED = 2046; + /** + * The ID of the AX12 used to deploy/retract the front ramp + */ + static const int RAMP_LEFT_SERVO_ID = 0; // To update + static const int RAMP_RIGHT_SERVO_ID = 0; // To update + + static const int RAMP_LEFT_DEPLOYED_ANGLE = 0; + static const int RAMP_LEFT_RETRACTED_ANGLE = 1023; + + static const int RAMP_RIGHT_DEPLOYED_ANGLE = 1023; + static const int RAMP_RIGHT_RETRACTED_ANGLE = 0; + public: struct Status { @@ -43,11 +55,15 @@ class Benne void stop(); - bool isFrontSwitchActive() const; - bool isBackSwitchActive() const; + void deployRamp(); + void retractRamp(); Benne::Status::Enum getStatus() const; + protected: + bool isFrontSwitchActive() const; + bool isBackSwitchActive() const; + private: Benne(); diff --git a/stm32/src/actionneurs/benne.cpp b/stm32/src/actionneurs/benne.cpp index f9831299ad061fac027b944643cd513423acf1cc..943a086b30f7c1f4d6368858d23b0af67af92071 100644 --- a/stm32/src/actionneurs/benne.cpp +++ b/stm32/src/actionneurs/benne.cpp @@ -31,7 +31,10 @@ Benne::Benne() m_status = Status::UNKNOWN; #ifdef ROBOTHW - ServosNumeriques::changeContinuousRotationMode(SERVO_ID, true); + ServosNumeriques::changeContinuousRotationMode(BELTS_SERVO_ID, true); + + ServosNumeriques::changeContinuousRotationMode(RAMP_LEFT_SERVO_ID, false); + ServosNumeriques::changeContinuousRotationMode(RAMP_RIGHT_SERVO_ID, false); #endif } @@ -74,7 +77,7 @@ void Benne::empty() if(getStatus() != Status::CLOSED && getStatus() != Status::CLOSING) { #ifdef ROBOTHW - ServosNumeriques::moveAtSpeed(FORWARD_SPEED, SERVO_ID); + ServosNumeriques::moveAtSpeed(FORWARD_SPEED, BELTS_SERVO_ID); #else qDebug() << "Bin is closing"; #endif @@ -87,7 +90,7 @@ void Benne::open() if(getStatus() != Status::OPEN && getStatus() != Status::OPENING) { #ifdef ROBOTHW - ServosNumeriques::moveAtSpeed(BACKWARD_SPEED, SERVO_ID); + ServosNumeriques::moveAtSpeed(BACKWARD_SPEED, BELTS_SERVO_ID); #else qDebug() << "Bin is opening"; #endif @@ -95,6 +98,26 @@ void Benne::open() } } +void Benne::deployRamp() +{ + #ifdef ROBOTHW + ServosNumeriques::moveTo(RAMP_LEFT_DEPLOYED_ANGLE, RAMP_LEFT_SERVO_ID); + ServosNumeriques::moveTo(RAMP_RIGHT_DEPLOYED_ANGLE, RAMP_RIGHT_SERVO_ID); + #else + qDebug() << "Deploying ramp"; + #endif +} + +void Benne::retractRamp() +{ + #ifdef ROBOTHW + ServosNumeriques::moveTo(RAMP_LEFT_RETRACTED_ANGLE, RAMP_LEFT_SERVO_ID); + ServosNumeriques::moveTo(RAMP_RIGHT_RETRACTED_ANGLE, RAMP_RIGHT_SERVO_ID); + #else + qDebug() << "Retracting ramp"; + #endif +} + void Benne::update() { if(getStatus() == Status::CLOSING && isFrontSwitchActive()) @@ -121,7 +144,7 @@ void Benne::update() void Benne::stop() { #ifdef ROBOTHW - ServosNumeriques::moveAtSpeed(0, SERVO_ID); + ServosNumeriques::moveAtSpeed(0, BELTS_SERVO_ID); #endif }