Commit 6338629c authored by ics's avatar ics

Prepare to real mode in ShieldingBlockController

parent 91e5f12c
......@@ -35,6 +35,9 @@ ShieldingBlockController::ShieldingBlockController(const std::string& name) :
rightAxis.init(this, SAVE, "right_axis");
m_A2.init(this, "A2");
m_stateDriver.init(this, "state_driver");
m_simulatedState = true;
registerRefresher(numberOfBlocks, &ShieldingBlockController::refreshNumberOfBlocks, this);
registerRefresher(numberOfIntervals, &ShieldingBlockController::refreshNumberOfIntervals, this);
......@@ -45,6 +48,9 @@ ShieldingBlockController::~ShieldingBlockController() {
void ShieldingBlockController::postConfiguration() {
// Init th state.
//m_simulatedState = (m_stateDriver->state() != REAL_DEVICE_CONTAINER);
registerUpdater(m_A2->position, &ShieldingBlockController::updateA2Position, this);
// Initialises the positions.
......@@ -118,25 +124,80 @@ void ShieldingBlockController::updateA2Position() {
// Get the angle and the rotation direction.
float64 currentA2 = m_A2->position();
// Search for closest smaller angle with tolerance
// Recalculate currentA2 in case of real mode.
// A2 > Ai - 4, A2 = Ai + 1 (to ensure > )
int numberOfSideBlocks = numberOfBlocks() / 2;
// First axes are the right.
for (int i = 0; i < numberOfSideBlocks; i++) {
bool up = (currentA2 > m_rightAxisAngle[m_blockAxes[i]->getName()]);
moveAxis(i, up);
bool up = upRightAxisPredicate(currentA2, i);
if (isAxisUp(i) != up) {
moveAxis(i, up);
}
}
// The following axes are the left.
for (int i = numberOfSideBlocks; i < numberOfSideBlocks * 2; i++) {
bool down = (currentA2 > m_leftAxisAngle[m_blockAxes[i]->getName()]);
moveAxis(i, !down);
bool down = downLeftAxisPredicate(currentA2, i);
if (isAxisDown(i) != down) {
moveAxis(i, !down);
}
}
m_previousA2 = currentA2;
}
bool ShieldingBlockController::upRightAxisPredicate(float64 currentA2, int index) {
if (m_simulatedState) {
// Simulated mode.
float64 offset = 0.0;
if (currentA2 < m_previousA2) {
offset = 0.7;
}
return (currentA2 > m_rightAxisAngle[m_blockAxes[index]->getName()] + offset);
}
else {
// Real mode.
//m_A2->axisStatus() == axis::AxisCommon::BRAKE_ACTIVATED;
return false;
}
}
bool ShieldingBlockController::downLeftAxisPredicate(float64 currentA2, int index) {
if (m_simulatedState) {
// Simulated mode.
float64 offset = 0.0;
if (currentA2 < m_previousA2) {
offset = 0.7;
}
return (currentA2 > m_leftAxisAngle[m_blockAxes[index]->getName()] + offset);
}
else {
// Real mode.
//m_A2->axisStatus() == axis::AxisCommon::BRAKE_ACTIVATED;
return false;
}
}
bool ShieldingBlockController::isAxisUp(int index) {
return (m_blockAxes[index]->position() == m_blockAxes[index]->maxPosition());
}
bool ShieldingBlockController::isAxisDown(int index) {
return (m_blockAxes[index]->position() == m_blockAxes[index]->minPosition());
}
void ShieldingBlockController::moveAxis(int index, bool up) {
// Index -1 indicates that there is no axis.
......@@ -148,12 +209,12 @@ void ShieldingBlockController::moveAxis(int index, bool up) {
if (up) {
m_blockAxes[index]->position = m_blockAxes[index]->maxPosition();
//cout << m_blockAxes[index]->getName() << " up" << endl;
cout << m_blockAxes[index]->getName() << " up" << endl;
}
else {
m_blockAxes[index]->position = m_blockAxes[index]->minPosition();
//cout << m_blockAxes[index]->getName() << " down" << endl;
cout << m_blockAxes[index]->getName() << " down" << endl;
}
}
......
......@@ -21,6 +21,7 @@
#include "controllers/common/axis/VirtualAxisController.h"
#include "controllers/common/axis/AxisController.h"
#include "drivers/utilities/null/NullDriver.h"
#include <vector>
#include <unordered_map>
......@@ -88,6 +89,26 @@ protected:
*/
void updateA2Position();
/**
* Returns true if the right axis has to be up. The predicate is different in the real mode.
*/
bool upRightAxisPredicate(float64 currentA2, int index);
/**
* Returns true if the left axis has to be up. The predicate is different in the real mode.
*/
bool downLeftAxisPredicate(float64 currentA2, int index);
/**
* Returns true if the axis is up.
*/
bool isAxisUp(int index);
/**
* Returns true if the axis is down.
*/
bool isAxisDown(int index);
/**
* Moves the axis.
*/
......@@ -96,9 +117,15 @@ protected:
// Axis A2.
ControllerPtr<axis::AxisController> m_A2;
// Null driver to determine the real, perfect or simulated state.
DriverPtr<driver::NullDriver> m_stateDriver;
// Block axes.
std::vector<ControllerPtr<axis::VirtualAxisController> > m_blockAxes;
// State.
bool m_simulatedState;
// Previous A2 position.
float64 m_previousA2;
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment