diff --git a/Common/Libraries/IRALibrary/src/IRAPy/Connection.py b/Common/Libraries/IRALibrary/src/IRAPy/Connection.py
index 0294b49907eadc7647932788861f1655f9d62cc5..6b10dba7ce2174a05ff18d82543dc24a8856770f 100644
--- a/Common/Libraries/IRALibrary/src/IRAPy/Connection.py
+++ b/Common/Libraries/IRALibrary/src/IRAPy/Connection.py
@@ -22,12 +22,13 @@ class Connection(object):
def __init__(self, address, timeout=2):
self.address = address
+ self.timeout = timeout
def __enter__(self):
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
connected = 1
t0 = time.time()
- while time.time() - t0 < timeout:
+ while time.time() - t0 < self.timeout:
connected = self.s.connect_ex(self.address)
if connected == 0:
break
diff --git a/Common/Libraries/IRALibrary/src/IRAPy/__init__.py b/Common/Libraries/IRALibrary/src/IRAPy/__init__.py
index dfae5e20669b8890b5dfbcf8b81e1b3348093feb..f59db02c90cd7db571e8addb6b2d97516c89c66d 100644
--- a/Common/Libraries/IRALibrary/src/IRAPy/__init__.py
+++ b/Common/Libraries/IRALibrary/src/IRAPy/__init__.py
@@ -11,7 +11,7 @@ list of modules:
"""
import customlogging
import ACSLog
-import Connection
+from Connection import Connection
#Some comments required here. The custom logger mechanism is not working in python.
#do the way to separate the system logs to the ones do be shown to the user is to use different
diff --git a/Medicina/Servers/MedicinaMinorServo/config/CDB/schemas/MinorServoParameters.xsd b/Medicina/Servers/MedicinaMinorServo/config/CDB/schemas/MinorServoParameters.xsd
index 3fca3614147293dc08643c54493519c96cc01eac..e60ee5791ecb6ee38a3f0640baad5e0884578659 100644
--- a/Medicina/Servers/MedicinaMinorServo/config/CDB/schemas/MinorServoParameters.xsd
+++ b/Medicina/Servers/MedicinaMinorServo/config/CDB/schemas/MinorServoParameters.xsd
@@ -25,13 +25,26 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
-
+
diff --git a/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoConstants.hpp b/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoConstants.hpp
index be5350e5fd9e25030c492e8ef89b92ae3fc3e51e..36cba26983682b3c840e5b7d462831452ee36dbf 100755
--- a/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoConstants.hpp
+++ b/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoConstants.hpp
@@ -13,12 +13,24 @@ struct MedMinorServoAxis_tag
double speed_max;
} MedMinorServoAxis;
-const MedMinorServoAxis MINOR_SERVO_X = { "X", -80, 80, 0.0, 0.05, 0.01, 14.0 };
-const MedMinorServoAxis MINOR_SERVO_YP = { "YP", -5, 395, 0.0, 0.05, 0.01, 8.0 };
-const MedMinorServoAxis MINOR_SERVO_Y = { "Y", -80, 80, 0.0, 0.05, 0.01, 14.0 };
-const MedMinorServoAxis MINOR_SERVO_ZP = { "ZP", -5, 345, 0.0, 0.05, 0.01, 8.0 };
-const MedMinorServoAxis MINOR_SERVO_Z1 = { "Z1", -125, 125, 0.0, 0.05, 0.01, 14.0 };
-const MedMinorServoAxis MINOR_SERVO_Z2 = { "Z2", -125, 125, 0.0, 0.05, 0.01, 14.0 };
-const MedMinorServoAxis MINOR_SERVO_Z3 = { "Z3", -125, 125, 0.0, 0.05, 0.01, 14.0 };
+using namespace std;
+
+class MedMinorServoConstants {
+public:
+ static MedMinorServoConstants *getInstance();
+ MedMinorServoAxis MINOR_SERVO_X ;
+ MedMinorServoAxis MINOR_SERVO_YP ;
+ MedMinorServoAxis MINOR_SERVO_Y ;
+ MedMinorServoAxis MINOR_SERVO_ZP ;
+ MedMinorServoAxis MINOR_SERVO_Z1 ;
+ MedMinorServoAxis MINOR_SERVO_Z2 ;
+ MedMinorServoAxis MINOR_SERVO_Z3 ;
+
+private:
+ MedMinorServoConstants();
+ static MedMinorServoConstants *instance;
+
+};
+
#endif
\ No newline at end of file
diff --git a/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoGeometry.hpp b/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoGeometry.hpp
index 6c43fdba41e27c84cf43bd2585d423ea69fad800..b10f2acfcd644f2a3a103771b9da5ba0d7767ac4 100755
--- a/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoGeometry.hpp
+++ b/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoGeometry.hpp
@@ -97,6 +97,9 @@ class MedMinorServoPosition
double theta_y;
MedMinorServoMode mode;
ACS::Time time;
+
+ private:
+ MedMinorServoConstants *medMinorServoConstants;
};
/**
diff --git a/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoParameters.hpp b/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoParameters.hpp
index d1a6f240c563650c58efdec0a60897117b06a764..37600526bcd0d30932ed8829968b527d3baeb57d 100644
--- a/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoParameters.hpp
+++ b/Medicina/Servers/MedicinaMinorServo/include/MedMinorServoParameters.hpp
@@ -14,6 +14,7 @@
#include "MedMinorServoGeometry.hpp"
#include "MedMinorServoOffset.hpp"
+#include "MedMinorServoConstants.hpp"
class VirtualAxis
{
@@ -24,6 +25,8 @@ class VirtualAxis
double min,
double max);
~VirtualAxis();
+ double get_min(){ return _min;};
+ double get_max(){ return _max;};
std::string get_name(){ return _name;};
std::string get_unit(){ return _unit;};
std::vector get_coefficients(){ return _coefficients;};
@@ -42,6 +45,8 @@ class VirtualAxis
*/
VirtualAxis parseAxisLine(const char* name, const char* line);
+std::vector parseLimitsLine(const char* line);
+
std::vector split(const char* line, char delim = ',');
class MedMinorServoParameters
@@ -69,6 +74,7 @@ class MedMinorServoParameters
std::string get_name(){ return _name;};
bool can_track_elevation(){ return _can_track_elevation;};
bool is_primary_focus(){ return _primary_focus;};
+ std::vector getAxesVector(){return _axes;};
std::vector getAxes(){return _axes_names;};
std::vector getUnits(){return _axes_units;};
int getAxisMapping(std::string axis_name);
@@ -78,6 +84,7 @@ class MedMinorServoParameters
std::vector _axes;
std::vector _axes_names;
std::vector _axes_units;
+
};
typedef std::map MedMinorServoConfiguration;
diff --git a/Medicina/Servers/MedicinaMinorServo/include/testutils.hpp b/Medicina/Servers/MedicinaMinorServo/include/testutils.hpp
index d2758df0c47c0187b1f98f11d5c1fca6da7bd6f2..606a549dd232ef124d59e17a43f7c3ad2b80d9e5 100644
--- a/Medicina/Servers/MedicinaMinorServo/include/testutils.hpp
+++ b/Medicina/Servers/MedicinaMinorServo/include/testutils.hpp
@@ -8,6 +8,7 @@
#include "MedMinorServoProtocol.hpp"
#include "MedMinorServoTime.hpp"
#include "MedMinorServoGeometry.hpp"
+#include "MedMinorServoConstants.hpp"
MedMinorServoPosition get_random_position();
MedMinorServoPosition random_offset_position(MedMinorServoPosition);
diff --git a/Medicina/Servers/MedicinaMinorServo/src/Makefile b/Medicina/Servers/MedicinaMinorServo/src/Makefile
index b765ed0d41d2c9bb589c750278475fef4da5800e..2dba14fd6a5ed412764eb74f649a503874203bb7 100644
--- a/Medicina/Servers/MedicinaMinorServo/src/Makefile
+++ b/Medicina/Servers/MedicinaMinorServo/src/Makefile
@@ -16,7 +16,7 @@ LIBRARIES_L =
#
#
-MedMinorServoControl_OBJECTS = MedMinorServoControl MedMinorServoGeometry \
+MedMinorServoControl_OBJECTS = MedMinorServoConstants MedMinorServoControl MedMinorServoGeometry \
MedMinorServoTime MedMinorServoModbus \
PositionQueue
@@ -24,13 +24,13 @@ MedMinorServoControl_LIBS = baci acsnc IRALibrary ModbusChannel ManagmentDefin
ManagementErrors \
ComponentErrors boost_thread-mt boost_system
-MinorServoBossImpl_OBJECTS = MedMinorServoControl MedMinorServoGeometry \
+MinorServoBossImpl_OBJECTS = MedMinorServoConstants MedMinorServoControl MedMinorServoGeometry \
MedMinorServoStatus MedMinorServoParameters \
MedMinorServoTime MedMinorServoModbus \
MedMinorServoScan PositionQueue \
PositionMonitoringThread MSBossPublisher \
SetupThread ScanThread \
- MSBossTracker MedMinorServoOffset MinorServoBossImpl
+ MSBossTracker MedMinorServoOffset MinorServoBossImpl
MinorServoBossImpl_LIBS = MedMinorServoBossStubs MinorServoBossStubs \
ManagmentDefinitionsStubs MinorServoDefinitionsStubs \
@@ -42,7 +42,7 @@ MinorServoBossImpl_LIBS = MedMinorServoBossStubs MinorServoBossStubs \
baci DiscosVersion
EXECUTABLES_L = real_to_virtual# testclient testgeometry testprimary testsecondary testpassive real_to_virtual
-real_to_virtual_OBJECTS = real_to_virtual testutils MedMinorServoGeometry MedMinorServoTime
+real_to_virtual_OBJECTS = real_to_virtual testutils MedMinorServoConstants MedMinorServoGeometry MedMinorServoTime
real_to_virtual_LIBS = IRALibrary
#testclient_OBJECTS = testclient testutils MedMinorServoModbus MedMinorServoGeometry MedMinorServoTime MedMinorServoControl PositionQueue
#testclient_LIBS = IRALibrary boost_thread ModbusChannel
@@ -56,7 +56,7 @@ real_to_virtual_LIBS = IRALibrary
#testgeometry_LIBS = IRALibrary boost_unit_test_framework boost_thread
EXECUTABLES = scudisconnect
-scudisconnect_OBJECTS = scudisconnect MedMinorServoModbus MedMinorServoGeometry MedMinorServoTime MedMinorServoControl PositionQueue
+scudisconnect_OBJECTS = scudisconnect MedMinorServoModbus MedMinorServoConstants MedMinorServoGeometry MedMinorServoTime MedMinorServoControl PositionQueue
scudisconnect_LIBS = IRALibrary ModbusChannel boost_thread-mt boost_system
#
diff --git a/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoConstants.cpp b/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoConstants.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a227bca025c81eda992ba47f98ed60976eaa5bf0
--- /dev/null
+++ b/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoConstants.cpp
@@ -0,0 +1,14 @@
+#include "MedMinorServoConstants.hpp"
+
+MedMinorServoConstants* MedMinorServoConstants::instance = 0;
+
+MedMinorServoConstants* MedMinorServoConstants::getInstance() {
+ if (instance == 0) {
+ instance = new MedMinorServoConstants();
+ }
+ return instance;
+}
+
+
+MedMinorServoConstants::MedMinorServoConstants(){
+}
\ No newline at end of file
diff --git a/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoGeometry.cpp b/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoGeometry.cpp
index 3928ac373ceabdb3a76083b1196a1998bcdf40d0..eae921ec086ecb0bc24f84b00fb96a9abfeee03e 100755
--- a/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoGeometry.cpp
+++ b/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoGeometry.cpp
@@ -1,14 +1,13 @@
-#include "MedMinorServoConstants.hpp"
#include "MedMinorServoGeometry.hpp"
-MedMinorServoPosition::MedMinorServoPosition() :
- x(0),
- y(0),
- z(0),
- theta_x(0),
- theta_y(0),
- time(0)
-{}
+MedMinorServoPosition::MedMinorServoPosition() : x(0),
+ y(0),
+ z(0),
+ theta_x(0),
+ theta_y(0),
+ time(0)
+{
+}
MedMinorServoPosition::MedMinorServoPosition(double x,
double y,
@@ -16,20 +15,22 @@ MedMinorServoPosition::MedMinorServoPosition(double x,
double theta_x,
double theta_y,
MedMinorServoMode mode,
- ACS::Time time):
- x(x),
- y(y),
- z(z),
- theta_x(theta_x),
- theta_y(theta_y),
- mode(mode),
- time(time)
-{}
+ ACS::Time time) : x(x),
+ y(y),
+ z(z),
+ theta_x(theta_x),
+ theta_y(theta_y),
+ mode(mode),
+ time(time)
+{
+ medMinorServoConstants = MedMinorServoConstants::getInstance();
+}
MedMinorServoPosition::~MedMinorServoPosition()
-{}
+{
+}
-MedMinorServoPosition::MedMinorServoPosition(const MedMinorServoPosition& position)
+MedMinorServoPosition::MedMinorServoPosition(const MedMinorServoPosition &position)
{
this->x = position.x;
this->y = position.y;
@@ -40,8 +41,8 @@ MedMinorServoPosition::MedMinorServoPosition(const MedMinorServoPosition& positi
this->time = position.time;
}
-MedMinorServoPosition&
-MedMinorServoPosition::operator=(const MedMinorServoPosition& position)
+MedMinorServoPosition &
+MedMinorServoPosition::operator=(const MedMinorServoPosition &position)
{
this->x = position.x;
this->y = position.y;
@@ -53,8 +54,7 @@ MedMinorServoPosition::operator=(const MedMinorServoPosition& position)
return *this;
}
-bool
-MedMinorServoPosition::operator==(const MedMinorServoPosition& position)
+bool MedMinorServoPosition::operator==(const MedMinorServoPosition &position)
{
return ((this->x == position.x) &&
(this->y == position.y) &&
@@ -65,13 +65,13 @@ MedMinorServoPosition::operator==(const MedMinorServoPosition& position)
(this->time == position.time));
}
-MedMinorServoPosition&
-MedMinorServoPosition::operator+=(const MedMinorServoPosition& position)
+MedMinorServoPosition &
+MedMinorServoPosition::operator+=(const MedMinorServoPosition &position)
{
- if((this->mode != position.mode) &&
- (this->mode != MED_MINOR_SERVO_OFFSET) &&
- (position.mode != MED_MINOR_SERVO_OFFSET))
- throw MinorServoGeometryError("addition of incompatible minor servo positions");
+ if ((this->mode != position.mode) &&
+ (this->mode != MED_MINOR_SERVO_OFFSET) &&
+ (position.mode != MED_MINOR_SERVO_OFFSET))
+ throw MinorServoGeometryError("addition of incompatible minor servo positions");
this->x += position.x;
this->y += position.y;
this->z += position.z;
@@ -80,13 +80,13 @@ MedMinorServoPosition::operator+=(const MedMinorServoPosition& position)
return *this;
}
-MedMinorServoPosition&
-MedMinorServoPosition::operator-=(const MedMinorServoPosition& position)
+MedMinorServoPosition &
+MedMinorServoPosition::operator-=(const MedMinorServoPosition &position)
{
- if((this->mode != position.mode) &&
- (this->mode != MED_MINOR_SERVO_OFFSET) &&
- (position.mode != MED_MINOR_SERVO_OFFSET))
- throw MinorServoGeometryError("subtraction of incompatible minor servo positions");
+ if ((this->mode != position.mode) &&
+ (this->mode != MED_MINOR_SERVO_OFFSET) &&
+ (position.mode != MED_MINOR_SERVO_OFFSET))
+ throw MinorServoGeometryError("subtraction of incompatible minor servo positions");
this->x -= position.x;
this->y -= position.y;
this->z -= position.z;
@@ -95,7 +95,7 @@ MedMinorServoPosition::operator-=(const MedMinorServoPosition& position)
return *this;
}
-MedMinorServoPosition&
+MedMinorServoPosition &
MedMinorServoPosition::operator*=(double scale)
{
this->x *= scale;
@@ -106,28 +106,26 @@ MedMinorServoPosition::operator*=(double scale)
return *this;
}
-bool
-MedMinorServoPosition::is_success_position()
+bool MedMinorServoPosition::is_success_position()
{
- if((mode == MED_MINOR_SERVO_PRIMARY)||
- (mode == MED_MINOR_SERVO_SECONDARY)||
- (mode == MED_MINOR_SERVO_OFFSET)||
- (mode == MED_MINOR_SERVO_TRANSFER_TO_PRIMARY)||
- (mode == MED_MINOR_SERVO_TRANSFER_TO_SECONDARY))
+ if ((mode == MED_MINOR_SERVO_PRIMARY) ||
+ (mode == MED_MINOR_SERVO_SECONDARY) ||
+ (mode == MED_MINOR_SERVO_OFFSET) ||
+ (mode == MED_MINOR_SERVO_TRANSFER_TO_PRIMARY) ||
+ (mode == MED_MINOR_SERVO_TRANSFER_TO_SECONDARY))
return true;
else
return false;
}
-
MedMinorServoPosition
-operator+(MedMinorServoPosition lhs, const MedMinorServoPosition& rhs)
+operator+(MedMinorServoPosition lhs, const MedMinorServoPosition &rhs)
{
return lhs += rhs;
}
MedMinorServoPosition
-operator-(MedMinorServoPosition lhs, const MedMinorServoPosition& rhs)
+operator-(MedMinorServoPosition lhs, const MedMinorServoPosition &rhs)
{
return lhs -= rhs;
}
@@ -144,75 +142,71 @@ operator*(double scale, MedMinorServoPosition rhs)
return rhs *= scale;
}
-bool
-MedMinorServoPosition::earlier(const ACS::Time& time) const
+bool MedMinorServoPosition::earlier(const ACS::Time &time) const
{
return (this->time < time);
}
-bool
-MedMinorServoPosition::earlier(const MedMinorServoPosition& position) const
+bool MedMinorServoPosition::earlier(const MedMinorServoPosition &position) const
{
return earlier(position.time);
}
-bool
-MedMinorServoPosition::later(const ACS::Time& time)
+bool MedMinorServoPosition::later(const ACS::Time &time)
{
return (this->time > time);
}
-bool
-MedMinorServoPosition::later(const MedMinorServoPosition& position)
+bool MedMinorServoPosition::later(const MedMinorServoPosition &position)
{
return later(position.time);
}
-const char*
+const char *
MedMinorServoPosition::getStatusString()
{
- switch(mode)
+ switch (mode)
{
- case(MED_MINOR_SERVO_LOCAL):
- return "LOCAL";
- break;
- case(MED_MINOR_SERVO_INTERLOCK):
- return "INTERLOCK";
- break;
- case(MED_MINOR_SERVO_SR_BLOCK):
- return "SR BLOCK";
- break;
- case(MED_MINOR_SERVO_PFR_BLOCK):
- return "PFR BLOCK";
- break;
- case(MED_MINOR_SERVO_SYSTEM_BLOCK):
- return "SYSTEM BLOCK";
- break;
- case(MED_MINOR_SERVO_PRIMARY_DISABLED):
- return "PRIMARY DISABLED";
- break;
- case(MED_MINOR_SERVO_SECONDARY_DISABLED):
- return "SECONDARY DISABLED";
- break;
-
- case(MED_MINOR_SERVO_PRIMARY):
- return "PRIMARY";
- break;
- case(MED_MINOR_SERVO_SECONDARY):
- return "SECONDARY";
- break;
- case(MED_MINOR_SERVO_OFFSET):
- return "OFFSET";
- break;
- case(MED_MINOR_SERVO_TRANSFER_TO_PRIMARY):
- return "TRANSFER TO PRIMARY";
- break;
- case(MED_MINOR_SERVO_TRANSFER_TO_SECONDARY):
- return "TRANSFER TO SECONDARY";
- break;
- default:
- return "UNDEINED";
- break;
+ case (MED_MINOR_SERVO_LOCAL):
+ return "LOCAL";
+ break;
+ case (MED_MINOR_SERVO_INTERLOCK):
+ return "INTERLOCK";
+ break;
+ case (MED_MINOR_SERVO_SR_BLOCK):
+ return "SR BLOCK";
+ break;
+ case (MED_MINOR_SERVO_PFR_BLOCK):
+ return "PFR BLOCK";
+ break;
+ case (MED_MINOR_SERVO_SYSTEM_BLOCK):
+ return "SYSTEM BLOCK";
+ break;
+ case (MED_MINOR_SERVO_PRIMARY_DISABLED):
+ return "PRIMARY DISABLED";
+ break;
+ case (MED_MINOR_SERVO_SECONDARY_DISABLED):
+ return "SECONDARY DISABLED";
+ break;
+
+ case (MED_MINOR_SERVO_PRIMARY):
+ return "PRIMARY";
+ break;
+ case (MED_MINOR_SERVO_SECONDARY):
+ return "SECONDARY";
+ break;
+ case (MED_MINOR_SERVO_OFFSET):
+ return "OFFSET";
+ break;
+ case (MED_MINOR_SERVO_TRANSFER_TO_PRIMARY):
+ return "TRANSFER TO PRIMARY";
+ break;
+ case (MED_MINOR_SERVO_TRANSFER_TO_SECONDARY):
+ return "TRANSFER TO SECONDARY";
+ break;
+ default:
+ return "UNDEINED";
+ break;
}
}
@@ -220,12 +214,14 @@ std::vector
MedMinorServoPosition::get_axes_positions()
{
std::vector res;
- if((mode == MED_MINOR_SERVO_PRIMARY)||
- (mode == MED_MINOR_SERVO_TRANSFER_TO_PRIMARY))
+ if ((mode == MED_MINOR_SERVO_PRIMARY) ||
+ (mode == MED_MINOR_SERVO_TRANSFER_TO_PRIMARY))
{
res.push_back(y);
res.push_back(z);
- }else{
+ }
+ else
+ {
res.push_back(x);
res.push_back(y);
res.push_back(z);
@@ -235,261 +231,287 @@ MedMinorServoPosition::get_axes_positions()
return res;
}
-double
-MedMinorServoPosition::get_axis_position(const char* axis_name)
-throw (MinorServoAxisNameError)
+double
+MedMinorServoPosition::get_axis_position(const char *axis_name) throw(MinorServoAxisNameError)
{
- if((mode == MED_MINOR_SERVO_PRIMARY)||
- (mode == MED_MINOR_SERVO_TRANSFER_TO_PRIMARY))
+ if ((mode == MED_MINOR_SERVO_PRIMARY) ||
+ (mode == MED_MINOR_SERVO_TRANSFER_TO_PRIMARY))
{
- if((strcmp(axis_name, "yp") == 0) ||
- (strcmp(axis_name, "YP") ==0))
+ if ((strcmp(axis_name, "yp") == 0) ||
+ (strcmp(axis_name, "YP") == 0))
return y;
- if((strcmp(axis_name, "zp") == 0) ||
- (strcmp(axis_name, "ZP") ==0))
+ if ((strcmp(axis_name, "zp") == 0) ||
+ (strcmp(axis_name, "ZP") == 0))
return z;
throw MinorServoAxisNameError(axis_name);
- }else{
- if((strcmp(axis_name, "x") == 0) ||
- (strcmp(axis_name, "X") ==0))
+ }
+ else
+ {
+ if ((strcmp(axis_name, "x") == 0) ||
+ (strcmp(axis_name, "X") == 0))
return x;
- if((strcmp(axis_name, "y") == 0) ||
- (strcmp(axis_name, "Y") ==0))
+ if ((strcmp(axis_name, "y") == 0) ||
+ (strcmp(axis_name, "Y") == 0))
return y;
- if((strcmp(axis_name, "z") == 0) ||
- (strcmp(axis_name, "Z") ==0))
+ if ((strcmp(axis_name, "z") == 0) ||
+ (strcmp(axis_name, "Z") == 0))
return z;
- if((strcmp(axis_name, "theta_x") == 0) ||
- (strcmp(axis_name, "THETA_X") ==0))
+ if ((strcmp(axis_name, "theta_x") == 0) ||
+ (strcmp(axis_name, "THETA_X") == 0))
return theta_x;
- if((strcmp(axis_name, "theta_y") == 0) ||
- (strcmp(axis_name, "THETA_Y") ==0))
+ if ((strcmp(axis_name, "theta_y") == 0) ||
+ (strcmp(axis_name, "THETA_Y") == 0))
return theta_y;
throw MinorServoAxisNameError(axis_name);
}
}
MedMinorServoPosition
-MedMinorServoGeometry::interpolate(const MedMinorServoPosition& before_position,
- const MedMinorServoPosition& after_position,
+MedMinorServoGeometry::interpolate(const MedMinorServoPosition &before_position,
+ const MedMinorServoPosition &after_position,
ACS::Time time)
{
- if(after_position.earlier(before_position))
- throw MinorServoGeometryError("cannot interpolate with previous time value");
- double linear_factor = (double)(time - before_position.time) /
+ if (after_position.earlier(before_position))
+ throw MinorServoGeometryError("cannot interpolate with previous time value");
+ double linear_factor = (double)(time - before_position.time) /
(double)(after_position.time - before_position.time);
MedMinorServoPosition difference = after_position - before_position;
MedMinorServoPosition augment = linear_factor * difference;
MedMinorServoPosition interpolation = before_position + augment;
- //MedMinorServoPosition interpolation = before_position +
- // (linear_factor *
- // (after_position - before_position));
+ // MedMinorServoPosition interpolation = before_position +
+ // (linear_factor *
+ // (after_position - before_position));
interpolation.time = time;
interpolation.mode = after_position.mode;
return interpolation;
}
-bool
-MedMinorServoGeometry::check_axes_limits(const MedMinorServoPosition& position)
+bool MedMinorServoGeometry::check_axes_limits(const MedMinorServoPosition &position)
{
- switch(position.mode)
+ switch (position.mode)
{
- case MED_MINOR_SERVO_PRIMARY:
- try{
- _get_inverse_yp(position);
- _get_inverse_zp(position);
- }catch(MinorServoAxisLimitError& ex)
- {
- return false;
- }
- break;
- case MED_MINOR_SERVO_SECONDARY:
- try{
- _get_inverse_x(position);
- _get_inverse_y(position);
- _get_inverse_z1(position);
- _get_inverse_z2(position);
- _get_inverse_z3(position);
- }catch(MinorServoAxisLimitError& ex)
- {
- return false;
- }
- break;
- default:
- return true;
+ case MED_MINOR_SERVO_PRIMARY:
+ try
+ {
+ _get_inverse_yp(position);
+ _get_inverse_zp(position);
+ }
+ catch (MinorServoAxisLimitError &ex)
+ {
+ return false;
+ }
+ break;
+ case MED_MINOR_SERVO_SECONDARY:
+ try
+ {
+ _get_inverse_x(position);
+ _get_inverse_y(position);
+ _get_inverse_z1(position);
+ _get_inverse_z2(position);
+ _get_inverse_z3(position);
+ }
+ catch (MinorServoAxisLimitError &ex)
+ {
+ return false;
+ }
+ break;
+ default:
+ return true;
}
return true;
}
-double
+double
MedMinorServoGeometry::_check_axis_limit(const double position,
const MedMinorServoAxis axis)
{
- int iposition = (int)position;
-
- if(iposition < axis.position_min){
+ if ((int)position < axis.position_min)
+ {
throw MinorServoAxisLimitError("axis too low");
- //return axis.position_min;
+ /*
+ CUSTOM_LOG(LM_FULL_INFO, "MinorServo::MinorServoGeometry::_check_axis_limit",
+ (LM_DEBUG, "axis %s too low, setting to min position %f",axis.name,axis.position_min));
+ return axis.position_min;
+ */
}
- if(iposition > axis.position_max){
- std::cout< axis.position_max)
+ {
throw MinorServoAxisLimitError("axis too high ");
- //return axis.position_max;
+ /*
+ CUSTOM_LOG(LM_FULL_INFO, "MinorServo::MinorServoGeometry::_check_axis_limit",
+ (LM_DEBUG, "axis %s too high, setting to max position %f",axis.name,axis.position_max));
+ return axis.position_max;
+ */
}
return position;
}
MEDMINORSERVOSETPOS
-MedMinorServoGeometry::positionToAxes(const MedMinorServoPosition& position)
+MedMinorServoGeometry::positionToAxes(const MedMinorServoPosition &position)
{
MEDMINORSERVOSETPOS command;
- switch(position.mode)
+ switch (position.mode)
{
- case(MED_MINOR_SERVO_PRIMARY):
- command.mode = 0;
+ case (MED_MINOR_SERVO_PRIMARY):
+ try
+ {
+ command.mode = 0;
command.pos_x_yp = _get_inverse_yp(position);
command.pos_y_zp = _get_inverse_zp(position);
- break;
- case(MED_MINOR_SERVO_SECONDARY):
- command.mode = 1;
+ }
+ catch (MinorServoAxisLimitError &ex)
+ {
+ throw MinorServoGeometryError("axis out of range");
+ }
+ break;
+ case (MED_MINOR_SERVO_SECONDARY):
+ try
+ {
+ command.mode = 1;
command.pos_x_yp = _get_inverse_x(position);
command.pos_y_zp = _get_inverse_y(position);
command.pos_z1 = _get_inverse_z1(position);
command.pos_z2 = _get_inverse_z2(position);
command.pos_z3 = _get_inverse_z3(position);
- break;
- default:
- throw MinorServoGeometryError("Invalid mode converting position to command");
+ }
+ catch (MinorServoAxisLimitError &ex)
+ {
+ throw MinorServoGeometryError("axis out of range");
+ }
+ break;
+ default:
+ throw MinorServoGeometryError("Invalid mode converting position to command");
}
+
command.time = MedMinorServoTime::ACSToServoTime(position.time);
return command;
}
-
MedMinorServoPosition
-MedMinorServoGeometry::positionFromAxes(const MEDMINORSERVOSTATUS& status)
+MedMinorServoGeometry::positionFromAxes(const MEDMINORSERVOSTATUS &status)
{
MedMinorServoPosition result;
- switch(status.system_status)
+ switch (status.system_status)
{
- case(MED_MINOR_SERVO_STATUS_PRIMARY): //PRIMARY FOCUS
- result = MedMinorServoPosition(
- 0,
- _get_direct_yp(status),
- _get_direct_zp(status),
- 0,
- 0,
- MED_MINOR_SERVO_PRIMARY,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_TRANSFER_TO_PRIMARY): //PRIMARY FOCUS
- result = MedMinorServoPosition(
- 0,
- _get_direct_yp(status),
- _get_direct_zp(status),
- 0,
- 0,
- MED_MINOR_SERVO_TRANSFER_TO_PRIMARY,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_PRIMARY_DISABLED): //PRIMARY FOCUS
- result = MedMinorServoPosition(
- 0,
- _get_direct_yp(status),
- _get_direct_zp(status),
- 0,
- 0,
- MED_MINOR_SERVO_PRIMARY_DISABLED,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_PFR_BLOCK): //PRIMARY FOCUS
- result = MedMinorServoPosition(
- 0,
- _get_direct_yp(status),
- _get_direct_zp(status),
- 0,
- 0,
- MED_MINOR_SERVO_PFR_BLOCK,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_SECONDARY):
- result = MedMinorServoPosition(
- _get_direct_x(status),
- _get_direct_y(status),
- _get_direct_z(status),
- _get_direct_theta_x(status),
- _get_direct_theta_y(status),
- MED_MINOR_SERVO_SECONDARY,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_TRANSFER_TO_SECONDARY):
- result = MedMinorServoPosition(
- _get_direct_x(status),
- _get_direct_y(status),
- _get_direct_z(status),
- _get_direct_theta_x(status),
- _get_direct_theta_y(status),
- MED_MINOR_SERVO_TRANSFER_TO_SECONDARY,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_SECONDARY_DISABLED):
- result = MedMinorServoPosition(
- _get_direct_x(status),
- _get_direct_y(status),
- _get_direct_z(status),
- _get_direct_theta_x(status),
- _get_direct_theta_y(status),
- MED_MINOR_SERVO_SECONDARY_DISABLED,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_SR_BLOCK):
- result = MedMinorServoPosition(
- _get_direct_x(status),
- _get_direct_y(status),
- _get_direct_z(status),
- _get_direct_theta_x(status),
- _get_direct_theta_y(status),
- MED_MINOR_SERVO_SR_BLOCK,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_SYSTEM_BLOCK):
- result = MedMinorServoPosition(
- _get_direct_x(status),
- _get_direct_y(status),
- _get_direct_z(status),
- _get_direct_theta_x(status),
- _get_direct_theta_y(status),
- MED_MINOR_SERVO_SYSTEM_BLOCK,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_LOCAL):
- result = MedMinorServoPosition(0,0,0,0,0, MED_MINOR_SERVO_LOCAL,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- case(MED_MINOR_SERVO_STATUS_INTERLOCK):
- result = MedMinorServoPosition(0,0,0,0,0, MED_MINOR_SERVO_INTERLOCK,
- MedMinorServoTime::servoToACSTime(status.time));
- //IRA::CIRATools::getACSTime());
- break;
- default:
- throw MinorServoGeometryError("Invalid system status converting status to position");
+ case (MED_MINOR_SERVO_STATUS_PRIMARY): // PRIMARY FOCUS
+ result = MedMinorServoPosition(
+ 0,
+ _get_direct_yp(status),
+ _get_direct_zp(status),
+ 0,
+ 0,
+ MED_MINOR_SERVO_PRIMARY,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_TRANSFER_TO_PRIMARY): // PRIMARY FOCUS
+ result = MedMinorServoPosition(
+ 0,
+ _get_direct_yp(status),
+ _get_direct_zp(status),
+ 0,
+ 0,
+ MED_MINOR_SERVO_TRANSFER_TO_PRIMARY,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_PRIMARY_DISABLED): // PRIMARY FOCUS
+ result = MedMinorServoPosition(
+ 0,
+ _get_direct_yp(status),
+ _get_direct_zp(status),
+ 0,
+ 0,
+ MED_MINOR_SERVO_PRIMARY_DISABLED,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_PFR_BLOCK): // PRIMARY FOCUS
+ result = MedMinorServoPosition(
+ 0,
+ _get_direct_yp(status),
+ _get_direct_zp(status),
+ 0,
+ 0,
+ MED_MINOR_SERVO_PFR_BLOCK,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_SECONDARY):
+ result = MedMinorServoPosition(
+ _get_direct_x(status),
+ _get_direct_y(status),
+ _get_direct_z(status),
+ _get_direct_theta_x(status),
+ _get_direct_theta_y(status),
+ MED_MINOR_SERVO_SECONDARY,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_TRANSFER_TO_SECONDARY):
+ result = MedMinorServoPosition(
+ _get_direct_x(status),
+ _get_direct_y(status),
+ _get_direct_z(status),
+ _get_direct_theta_x(status),
+ _get_direct_theta_y(status),
+ MED_MINOR_SERVO_TRANSFER_TO_SECONDARY,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_SECONDARY_DISABLED):
+ result = MedMinorServoPosition(
+ _get_direct_x(status),
+ _get_direct_y(status),
+ _get_direct_z(status),
+ _get_direct_theta_x(status),
+ _get_direct_theta_y(status),
+ MED_MINOR_SERVO_SECONDARY_DISABLED,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_SR_BLOCK):
+ result = MedMinorServoPosition(
+ _get_direct_x(status),
+ _get_direct_y(status),
+ _get_direct_z(status),
+ _get_direct_theta_x(status),
+ _get_direct_theta_y(status),
+ MED_MINOR_SERVO_SR_BLOCK,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_SYSTEM_BLOCK):
+ result = MedMinorServoPosition(
+ _get_direct_x(status),
+ _get_direct_y(status),
+ _get_direct_z(status),
+ _get_direct_theta_x(status),
+ _get_direct_theta_y(status),
+ MED_MINOR_SERVO_SYSTEM_BLOCK,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_LOCAL):
+ result = MedMinorServoPosition(0, 0, 0, 0, 0, MED_MINOR_SERVO_LOCAL,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ case (MED_MINOR_SERVO_STATUS_INTERLOCK):
+ result = MedMinorServoPosition(0, 0, 0, 0, 0, MED_MINOR_SERVO_INTERLOCK,
+ MedMinorServoTime::servoToACSTime(status.time));
+ // IRA::CIRATools::getACSTime());
+ break;
+ default:
+ throw MinorServoGeometryError("Invalid system status converting status to position");
}
return result;
}
MedMinorServoPosition
-MedMinorServoGeometry::abs(const MedMinorServoPosition& position)
+MedMinorServoGeometry::abs(const MedMinorServoPosition &position)
{
MedMinorServoPosition _abs_position = position;
_abs_position.x = std::abs(position.x);
@@ -500,99 +522,121 @@ MedMinorServoGeometry::abs(const MedMinorServoPosition& position)
return _abs_position;
}
-double
-MedMinorServoGeometry::min_time(const MedMinorServoPosition& start,
- const MedMinorServoPosition& stop)
+double
+MedMinorServoGeometry::min_time(const MedMinorServoPosition &start,
+ const MedMinorServoPosition &stop)
{
double min = 0;
MEDMINORSERVOSETPOS start_axes = positionToAxes(start);
MEDMINORSERVOSETPOS stop_axes = positionToAxes(stop);
- if(start.mode == MED_MINOR_SERVO_PRIMARY)
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
+ if (start.mode == MED_MINOR_SERVO_PRIMARY)
{
- if(start_axes.pos_x_yp != stop_axes.pos_x_yp)
+ if (start_axes.pos_x_yp != stop_axes.pos_x_yp)
{
- double t_yp = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / MINOR_SERVO_YP.speed_max;
- if(t_yp > min) min = t_yp;
+ double t_yp = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / medMinorServoConstants->MINOR_SERVO_YP.speed_max;
+ if (t_yp > min)
+ min = t_yp;
}
- if(start_axes.pos_y_zp != stop_axes.pos_y_zp)
+ if (start_axes.pos_y_zp != stop_axes.pos_y_zp)
{
- double t_zp = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / MINOR_SERVO_ZP.speed_max;
- if(t_zp > min) min = t_zp;
+ double t_zp = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / medMinorServoConstants->MINOR_SERVO_ZP.speed_max;
+ if (t_zp > min)
+ min = t_zp;
}
- }else{//MED_MINOR_SERVO_SECONDARY
- if(start_axes.pos_x_yp != stop_axes.pos_x_yp)
+ }
+ else
+ { // MED_MINOR_SERVO_SECONDARY
+ if (start_axes.pos_x_yp != stop_axes.pos_x_yp)
{
- double t_x = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / MINOR_SERVO_X.speed_max;
- if(t_x > min) min = t_x;
+ double t_x = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / medMinorServoConstants->MINOR_SERVO_X.speed_max;
+ if (t_x > min)
+ min = t_x;
}
- if(start_axes.pos_y_zp != stop_axes.pos_y_zp)
+ if (start_axes.pos_y_zp != stop_axes.pos_y_zp)
{
- double t_y = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / MINOR_SERVO_Y.speed_max;
- if(t_y > min) min = t_y;
+ double t_y = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / medMinorServoConstants->MINOR_SERVO_Y.speed_max;
+ if (t_y > min)
+ min = t_y;
}
- if(start_axes.pos_z1 != stop_axes.pos_z1)
+ if (start_axes.pos_z1 != stop_axes.pos_z1)
{
- double t_z1 = std::abs(start_axes.pos_z1 - stop_axes.pos_z1) / MINOR_SERVO_Z1.speed_max;
- if(t_z1 > min) min = t_z1;
+ double t_z1 = std::abs(start_axes.pos_z1 - stop_axes.pos_z1) / medMinorServoConstants->MINOR_SERVO_Z1.speed_max;
+ if (t_z1 > min)
+ min = t_z1;
}
- if(start_axes.pos_z2 != stop_axes.pos_z2)
+ if (start_axes.pos_z2 != stop_axes.pos_z2)
{
- double t_z2 = std::abs(start_axes.pos_z2 - stop_axes.pos_z2) / MINOR_SERVO_Z2.speed_max;
- if(t_z2 > min) min = t_z2;
+ double t_z2 = std::abs(start_axes.pos_z2 - stop_axes.pos_z2) / medMinorServoConstants->MINOR_SERVO_Z2.speed_max;
+ if (t_z2 > min)
+ min = t_z2;
}
- if(start_axes.pos_z3 != stop_axes.pos_z3)
+ if (start_axes.pos_z3 != stop_axes.pos_z3)
{
- double t_z3 = std::abs(start_axes.pos_z3 - stop_axes.pos_z3) / MINOR_SERVO_Z3.speed_max;
- if(t_z3 > min) min = t_z3;
+ double t_z3 = std::abs(start_axes.pos_z3 - stop_axes.pos_z3) / medMinorServoConstants->MINOR_SERVO_Z3.speed_max;
+ if (t_z3 > min)
+ min = t_z3;
}
}
return min;
}
-double
-MedMinorServoGeometry::max_time(const MedMinorServoPosition& start,
- const MedMinorServoPosition& stop)
+double
+MedMinorServoGeometry::max_time(const MedMinorServoPosition &start,
+ const MedMinorServoPosition &stop)
{
double max = std::numeric_limits::infinity();
MEDMINORSERVOSETPOS start_axes = positionToAxes(start);
MEDMINORSERVOSETPOS stop_axes = positionToAxes(stop);
- if(start.mode == MED_MINOR_SERVO_PRIMARY)
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
+ if (start.mode == MED_MINOR_SERVO_PRIMARY)
{
- if(start_axes.pos_x_yp != stop_axes.pos_x_yp)
+ if (start_axes.pos_x_yp != stop_axes.pos_x_yp)
{
- double t_yp = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / MINOR_SERVO_YP.speed_min;
- if(t_yp < max) max = t_yp;
+ double t_yp = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / medMinorServoConstants->MINOR_SERVO_YP.speed_min;
+ if (t_yp < max)
+ max = t_yp;
}
- if(start_axes.pos_y_zp != stop_axes.pos_y_zp)
+ if (start_axes.pos_y_zp != stop_axes.pos_y_zp)
{
- double t_zp = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / MINOR_SERVO_ZP.speed_min;
- if(t_zp < max) max = t_zp;
+ double t_zp = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / medMinorServoConstants->MINOR_SERVO_ZP.speed_min;
+ if (t_zp < max)
+ max = t_zp;
}
- }else{//MED_MINOR_SERVO_SECONDARY
- if(start_axes.pos_x_yp != stop_axes.pos_x_yp)
+ }
+ else
+ { // MED_MINOR_SERVO_SECONDARY
+ if (start_axes.pos_x_yp != stop_axes.pos_x_yp)
{
- double t_x = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / MINOR_SERVO_X.speed_min;
- if(t_x < max) max = t_x;
+ double t_x = std::abs(start_axes.pos_x_yp - stop_axes.pos_x_yp) / medMinorServoConstants->MINOR_SERVO_X.speed_min;
+ if (t_x < max)
+ max = t_x;
}
- if(start_axes.pos_y_zp != stop_axes.pos_y_zp)
+ if (start_axes.pos_y_zp != stop_axes.pos_y_zp)
{
- double t_y = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / MINOR_SERVO_Y.speed_min;
- if(t_y < max) max = t_y;
+ double t_y = std::abs(start_axes.pos_y_zp - stop_axes.pos_y_zp) / medMinorServoConstants->MINOR_SERVO_Y.speed_min;
+ if (t_y < max)
+ max = t_y;
}
- if(start_axes.pos_z1 != stop_axes.pos_z1)
+ if (start_axes.pos_z1 != stop_axes.pos_z1)
{
- double t_z1 = std::abs(start_axes.pos_z1 - stop_axes.pos_z1) / MINOR_SERVO_Z1.speed_min;
- if(t_z1 < max) max = t_z1;
+ double t_z1 = std::abs(start_axes.pos_z1 - stop_axes.pos_z1) / medMinorServoConstants->MINOR_SERVO_Z1.speed_min;
+ if (t_z1 < max)
+ max = t_z1;
}
- if(start_axes.pos_z2 != stop_axes.pos_z2)
+ if (start_axes.pos_z2 != stop_axes.pos_z2)
{
- double t_z2 = std::abs(start_axes.pos_z2 - stop_axes.pos_z2) / MINOR_SERVO_Z2.speed_min;
- if(t_z2 < max) max = t_z2;
+ double t_z2 = std::abs(start_axes.pos_z2 - stop_axes.pos_z2) / medMinorServoConstants->MINOR_SERVO_Z2.speed_min;
+ if (t_z2 < max)
+ max = t_z2;
}
- if(start_axes.pos_z3 != stop_axes.pos_z3)
+ if (start_axes.pos_z3 != stop_axes.pos_z3)
{
- double t_z3 = std::abs(start_axes.pos_z3 - stop_axes.pos_z3) / MINOR_SERVO_Z3.speed_min;
- if(t_z3 < max) max = t_z3;
+ double t_z3 = std::abs(start_axes.pos_z3 - stop_axes.pos_z3) / medMinorServoConstants->MINOR_SERVO_Z3.speed_min;
+ if (t_z3 < max)
+ max = t_z3;
}
}
return max;
@@ -601,11 +645,12 @@ MedMinorServoGeometry::max_time(const MedMinorServoPosition& start,
MedMinorServoPosition
MedMinorServoGeometry::get_primary_tolerance()
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
MEDMINORSERVOSTATUS status;
status.mode = 0;
status.time = 0;
- status.pos_x_yp = MINOR_SERVO_YP.position_error + MINOR_SERVO_YP.position_zero;
- status.pos_y_zp = MINOR_SERVO_ZP.position_error + MINOR_SERVO_ZP.position_zero;
+ status.pos_x_yp = medMinorServoConstants->MINOR_SERVO_YP.position_error + medMinorServoConstants->MINOR_SERVO_YP.position_zero;
+ status.pos_y_zp = medMinorServoConstants->MINOR_SERVO_ZP.position_error + medMinorServoConstants->MINOR_SERVO_ZP.position_zero;
status.system_status = MED_MINOR_SERVO_STATUS_PRIMARY;
MedMinorServoPosition position = positionFromAxes(status);
position.mode = MED_MINOR_SERVO_OFFSET;
@@ -616,27 +661,28 @@ MedMinorServoGeometry::get_primary_tolerance()
MedMinorServoPosition
MedMinorServoGeometry::get_secondary_tolerance()
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
MEDMINORSERVOSTATUS status;
status.mode = 1;
status.system_status = MED_MINOR_SERVO_STATUS_PRIMARY;
status.time = 0;
MedMinorServoPosition position;
position.mode = MED_MINOR_SERVO_OFFSET;
- status.pos_x_yp = MINOR_SERVO_X.position_error + MINOR_SERVO_X.position_zero;
- status.pos_y_zp = MINOR_SERVO_Y.position_error + MINOR_SERVO_Y.position_zero;
+ status.pos_x_yp = medMinorServoConstants->MINOR_SERVO_X.position_error + medMinorServoConstants->MINOR_SERVO_X.position_zero;
+ status.pos_y_zp = medMinorServoConstants->MINOR_SERVO_Y.position_error + medMinorServoConstants->MINOR_SERVO_Y.position_zero;
position.x = std::abs(_get_direct_x(status));
position.y = std::abs(_get_direct_y(status));
- status.pos_y_zp = MINOR_SERVO_Y.position_zero - MINOR_SERVO_Y.position_error;
- status.pos_z1 = MINOR_SERVO_Z1.position_zero + MINOR_SERVO_Z1.position_error;
- status.pos_z2 = MINOR_SERVO_Z2.position_zero + MINOR_SERVO_Z2.position_error;
- status.pos_z3 = MINOR_SERVO_Z3.position_zero + MINOR_SERVO_Z3.position_error;
+ status.pos_y_zp = medMinorServoConstants->MINOR_SERVO_Y.position_zero - medMinorServoConstants->MINOR_SERVO_Y.position_error;
+ status.pos_z1 = medMinorServoConstants->MINOR_SERVO_Z1.position_zero + medMinorServoConstants->MINOR_SERVO_Z1.position_error;
+ status.pos_z2 = medMinorServoConstants->MINOR_SERVO_Z2.position_zero + medMinorServoConstants->MINOR_SERVO_Z2.position_error;
+ status.pos_z3 = medMinorServoConstants->MINOR_SERVO_Z3.position_zero + medMinorServoConstants->MINOR_SERVO_Z3.position_error;
position.z = std::abs(_get_direct_z(status));
- status.pos_z1 = MINOR_SERVO_Z1.position_zero + MINOR_SERVO_Z1.position_error;
- status.pos_z2 = MINOR_SERVO_Z2.position_zero - MINOR_SERVO_Z2.position_error;
- status.pos_z3 = MINOR_SERVO_Z3.position_zero - MINOR_SERVO_Z3.position_error;
+ status.pos_z1 = medMinorServoConstants->MINOR_SERVO_Z1.position_zero + medMinorServoConstants->MINOR_SERVO_Z1.position_error;
+ status.pos_z2 = medMinorServoConstants->MINOR_SERVO_Z2.position_zero - medMinorServoConstants->MINOR_SERVO_Z2.position_error;
+ status.pos_z3 = medMinorServoConstants->MINOR_SERVO_Z3.position_zero - medMinorServoConstants->MINOR_SERVO_Z3.position_error;
position.theta_x = std::abs(_get_direct_theta_x(status));
- status.pos_z2 = MINOR_SERVO_Z2.position_zero + MINOR_SERVO_Z2.position_error;
- status.pos_z3 = MINOR_SERVO_Z3.position_zero - MINOR_SERVO_Z3.position_error;
+ status.pos_z2 = medMinorServoConstants->MINOR_SERVO_Z2.position_zero + medMinorServoConstants->MINOR_SERVO_Z2.position_error;
+ status.pos_z3 = medMinorServoConstants->MINOR_SERVO_Z3.position_zero - medMinorServoConstants->MINOR_SERVO_Z3.position_error;
position.theta_y = std::abs(_get_direct_theta_y(status));
position.time = 0;
return position;
@@ -648,22 +694,28 @@ MedMinorServoGeometry::get_secondary_tolerance()
double
MedMinorServoGeometry::_get_direct_yp(const MEDMINORSERVOSTATUS actual_position)
{
- if(actual_position.mode == 1) //GREGORIAN
- //TODO: raise exception? warning?
- return actual_position.pos_x_yp - MINOR_SERVO_X.position_zero;
- else{
- return actual_position.pos_x_yp - MINOR_SERVO_YP.position_zero;
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
+ if (actual_position.mode == 1) // GREGORIAN
+ // TODO: raise exception? warning?
+ return actual_position.pos_x_yp - medMinorServoConstants->MINOR_SERVO_X.position_zero;
+ else
+ {
+ return actual_position.pos_x_yp - medMinorServoConstants->MINOR_SERVO_YP.position_zero;
}
}
double
MedMinorServoGeometry::_get_direct_zp(const MEDMINORSERVOSTATUS actual_position)
{
- if(actual_position.mode == 1) //GREGORIAN
- //TODO: raise exception? warning?
- return 0.9903 * (actual_position.pos_y_zp - MINOR_SERVO_Y.position_zero);
- else{
- return actual_position.pos_y_zp - MINOR_SERVO_ZP.position_zero;
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
+ if (actual_position.mode == 1) // GREGORIAN
+ // TODO: raise exception? warning?
+ return 0.9903 * (actual_position.pos_y_zp - medMinorServoConstants->MINOR_SERVO_Y.position_zero);
+ else
+ {
+ return actual_position.pos_y_zp - medMinorServoConstants->MINOR_SERVO_ZP.position_zero;
}
}
@@ -674,36 +726,41 @@ MedMinorServoGeometry::_get_direct_zp(const MEDMINORSERVOSTATUS actual_position)
double
MedMinorServoGeometry::_get_direct_x(const MEDMINORSERVOSTATUS actual_position)
{
- if(actual_position.mode == 1) //GREGORIAN
- return actual_position.pos_x_yp - MINOR_SERVO_X.position_zero;
- else{
- //TODO: raise exception? warning?
- return actual_position.pos_x_yp - MINOR_SERVO_YP.position_zero;
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
+ if (actual_position.mode == 1) // GREGORIAN
+ return actual_position.pos_x_yp - medMinorServoConstants->MINOR_SERVO_X.position_zero;
+ else
+ {
+ // TODO: raise exception? warning?
+ return actual_position.pos_x_yp - medMinorServoConstants->MINOR_SERVO_YP.position_zero;
}
}
double
MedMinorServoGeometry::_get_direct_y(const MEDMINORSERVOSTATUS actual_position)
{
- if(actual_position.mode == 1) //GREGORIAN
- return 0.9903 * (actual_position.pos_y_zp - MINOR_SERVO_Y.position_zero);
- else{
- //TODO: raise exception? warning?
- return actual_position.pos_y_zp - MINOR_SERVO_ZP.position_zero;
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
+ if (actual_position.mode == 1) // GREGORIAN
+ return 0.9903 * (actual_position.pos_y_zp - medMinorServoConstants->MINOR_SERVO_Y.position_zero);
+ else
+ {
+ // TODO: raise exception? warning?
+ return actual_position.pos_y_zp - medMinorServoConstants->MINOR_SERVO_ZP.position_zero;
}
}
double
MedMinorServoGeometry::_get_direct_z(const MEDMINORSERVOSTATUS actual_position)
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
double z = 0.0;
- if(actual_position.mode == 1) //GREGORIAN
- z = - 0.1392 * (actual_position.pos_y_zp - MINOR_SERVO_Y.position_zero)
- + 0.3333 * (actual_position.pos_z1 - MINOR_SERVO_Z1.position_zero)
- + 0.3333 * (actual_position.pos_z2 - MINOR_SERVO_Z2.position_zero)
- + 0.3333 * (actual_position.pos_z3 - MINOR_SERVO_Z3.position_zero);
- else{
- //TODO: raise exception? warning?
+ if (actual_position.mode == 1) // GREGORIAN
+ z = -0.1392 * (actual_position.pos_y_zp - medMinorServoConstants->MINOR_SERVO_Y.position_zero) + 0.3333 * (actual_position.pos_z1 - medMinorServoConstants->MINOR_SERVO_Z1.position_zero) + 0.3333 * (actual_position.pos_z2 - medMinorServoConstants->MINOR_SERVO_Z2.position_zero) + 0.3333 * (actual_position.pos_z3 - medMinorServoConstants->MINOR_SERVO_Z3.position_zero);
+ else
+ {
+ // TODO: raise exception? warning?
}
return z;
}
@@ -711,13 +768,13 @@ MedMinorServoGeometry::_get_direct_z(const MEDMINORSERVOSTATUS actual_position)
double
MedMinorServoGeometry::_get_direct_theta_x(const MEDMINORSERVOSTATUS actual_position)
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
double theta_x = 0.0;
- if(actual_position.mode == 1) //GREGORIAN
- theta_x = (actual_position.pos_z1 - MINOR_SERVO_Z1.position_zero) / 1791
- -(actual_position.pos_z2 - MINOR_SERVO_Z2.position_zero) / 3582
- -(actual_position.pos_z3 - MINOR_SERVO_Z3.position_zero) / 3582;
- else{
- //TODO: raise exception? warning?
+ if (actual_position.mode == 1) // GREGORIAN
+ theta_x = (actual_position.pos_z1 - medMinorServoConstants->MINOR_SERVO_Z1.position_zero) / 1791 - (actual_position.pos_z2 - medMinorServoConstants->MINOR_SERVO_Z2.position_zero) / 3582 - (actual_position.pos_z3 - medMinorServoConstants->MINOR_SERVO_Z3.position_zero) / 3582;
+ else
+ {
+ // TODO: raise exception? warning?
}
return theta_x;
}
@@ -725,12 +782,13 @@ MedMinorServoGeometry::_get_direct_theta_x(const MEDMINORSERVOSTATUS actual_posi
double
MedMinorServoGeometry::_get_direct_theta_y(const MEDMINORSERVOSTATUS actual_position)
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
double theta_y = 0.0;
- if(actual_position.mode == 1) //GREGORIAN
- theta_y = (actual_position.pos_z2 - MINOR_SERVO_Z2.position_zero) / 2068
- -(actual_position.pos_z3 - MINOR_SERVO_Z3.position_zero) / 2068;
- else{
- //TODO: raise exception? warning?
+ if (actual_position.mode == 1) // GREGORIAN
+ theta_y = (actual_position.pos_z2 - medMinorServoConstants->MINOR_SERVO_Z2.position_zero) / 2068 - (actual_position.pos_z3 - medMinorServoConstants->MINOR_SERVO_Z3.position_zero) / 2068;
+ else
+ {
+ // TODO: raise exception? warning?
}
return theta_y;
}
@@ -739,67 +797,62 @@ MedMinorServoGeometry::_get_direct_theta_y(const MEDMINORSERVOSTATUS actual_posi
* INVERSE EQUATIONS FOR PRIMARY FOCUS AXES POSITIONS
****************************************************/
-double
-MedMinorServoGeometry::_get_inverse_yp(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_yp(const MedMinorServoPosition &position)
{
- double yp = position.y + MINOR_SERVO_YP.position_zero;
- return _check_axis_limit(yp, MINOR_SERVO_YP);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double yp = position.y + medMinorServoConstants->MINOR_SERVO_YP.position_zero;
+ return _check_axis_limit(yp, medMinorServoConstants->MINOR_SERVO_YP);
}
-double
-MedMinorServoGeometry::_get_inverse_zp(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_zp(const MedMinorServoPosition &position)
{
- double zp = position.z + MINOR_SERVO_ZP.position_zero;
- return _check_axis_limit(zp, MINOR_SERVO_ZP);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double zp = position.z + medMinorServoConstants->MINOR_SERVO_ZP.position_zero;
+ return _check_axis_limit(zp, medMinorServoConstants->MINOR_SERVO_ZP);
}
/******************************************************
* INVERSE EQUATIONS FOR SECONDARY FOCUS AXES POSITIONS
******************************************************/
-double
-MedMinorServoGeometry::_get_inverse_x(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_x(const MedMinorServoPosition &position)
{
- double x = position.x + MINOR_SERVO_X.position_zero;
- return _check_axis_limit(x, MINOR_SERVO_X);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double x = position.x + medMinorServoConstants->MINOR_SERVO_X.position_zero;
+ return _check_axis_limit(x, medMinorServoConstants->MINOR_SERVO_X);
}
-double
-MedMinorServoGeometry::_get_inverse_y(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_y(const MedMinorServoPosition &position)
{
- double y = 1.0098 * position.y + MINOR_SERVO_Y.position_zero;
- return _check_axis_limit(y, MINOR_SERVO_Y);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double y = 1.0098 * position.y + medMinorServoConstants->MINOR_SERVO_Y.position_zero;
+ return _check_axis_limit(y, medMinorServoConstants->MINOR_SERVO_Y);
}
-double
-MedMinorServoGeometry::_get_inverse_z1(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_z1(const MedMinorServoPosition &position)
{
- double z1 = 0.1405 * position.y
- + position.z
- + 1194 * position.theta_x
- + MINOR_SERVO_Z1.position_zero;
- return _check_axis_limit(z1, MINOR_SERVO_Z1);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double z1 = 0.1405 * position.y + position.z + 1194 * position.theta_x + medMinorServoConstants->MINOR_SERVO_Z1.position_zero;
+ return _check_axis_limit(z1, medMinorServoConstants->MINOR_SERVO_Z1);
}
-double
-MedMinorServoGeometry::_get_inverse_z2(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_z2(const MedMinorServoPosition &position)
{
- double z2 = 0.1405 * position.y
- + position.z
- - 597 * position.theta_x
- + 1034 * position.theta_y
- + MINOR_SERVO_Z2.position_zero;
- return _check_axis_limit(z2, MINOR_SERVO_Z2);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double z2 = 0.1405 * position.y + position.z - 597 * position.theta_x + 1034 * position.theta_y + medMinorServoConstants->MINOR_SERVO_Z2.position_zero;
+ return _check_axis_limit(z2, medMinorServoConstants->MINOR_SERVO_Z2);
}
-double
-MedMinorServoGeometry::_get_inverse_z3(const MedMinorServoPosition& position)
+double
+MedMinorServoGeometry::_get_inverse_z3(const MedMinorServoPosition &position)
{
- double z3 = 0.1405 * position.y
- + position.z
- - 597 * position.theta_x
- - 1034 * position.theta_y
- + MINOR_SERVO_Z3.position_zero;
- return _check_axis_limit(z3, MINOR_SERVO_Z3);
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+ double z3 = 0.1405 * position.y + position.z - 597 * position.theta_x - 1034 * position.theta_y + medMinorServoConstants->MINOR_SERVO_Z3.position_zero;
+ return _check_axis_limit(z3, medMinorServoConstants->MINOR_SERVO_Z3);
}
-
diff --git a/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoParameters.cpp b/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoParameters.cpp
index 4a730fcd074fc09fb63142e7956bd954ea4f2dcc..f71ff12e2b13cf996ca514019f8f1d7a0ad09186 100644
--- a/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoParameters.cpp
+++ b/Medicina/Servers/MedicinaMinorServo/src/MedMinorServoParameters.cpp
@@ -14,6 +14,17 @@ VirtualAxis::VirtualAxis(const char* name,
_coefficients(coefficients)
{}
+std::vector parseLimitsLine(const char* line)
+{
+ std::vector limits;
+ std::vector tokens = split(line);
+ for(int i=0; i<6; ++i)
+ {
+ limits.push_back(boost::lexical_cast(tokens[i]));
+ }
+ return limits;
+}
+
VirtualAxis
parseAxisLine(const char* name, const char* line)
{
@@ -174,8 +185,30 @@ MedMinorServoConfiguration
get_configuration_from_CDB(maci::ContainerServices* services)
{
MedMinorServoConfiguration configuration;
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
IRA::CError error;
error.Reset();
+
+ IRA::CDBTable minor_servo_limits_table(services,
+ "MinorServoLimits",
+ "DataBlock/MinorServoParameters");
+
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_X", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_X not found", 0);
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_YP", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_YP not found", 0);
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_Y", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_Y not found", 0);
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_ZP", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_ZP not found", 0);
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_Z1", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_Z1 not found", 0);
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_Z2", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_Z2 not found", 0);
+ if(!minor_servo_limits_table.addField(error, "MINOR_SERVO_Z3", IRA::CDataField::STRING))
+ error.setExtra("code MINOR_SERVO_Z3 not found", 0);
+
IRA::CDBTable minor_servo_table(services,
"MinorServo",
"DataBlock/MinorServoParameters");
@@ -206,6 +239,78 @@ get_configuration_from_CDB(maci::ContainerServices* services)
dummy.setDescription((const char*)error.getDescription());
throw dummy;
}
+
+ if(!minor_servo_limits_table.openTable(error)){
+ _EXCPT_FROM_ERROR(ComponentErrors::CDBAccessExImpl, dummy, error);
+ throw dummy;
+ }
+
+ minor_servo_limits_table.First();
+ std::vector limits;
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_YP"]->asString());
+ medMinorServoConstants->MINOR_SERVO_YP.name= "YP";
+ medMinorServoConstants->MINOR_SERVO_YP.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_YP.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_YP.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_YP.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_YP.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_YP.speed_max= limits[5];
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_ZP"]->asString());
+ medMinorServoConstants->MINOR_SERVO_ZP.name= "ZP";
+ medMinorServoConstants->MINOR_SERVO_ZP.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_ZP.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_ZP.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_ZP.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_ZP.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_ZP.speed_max= limits[5];
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_X"]->asString());
+ medMinorServoConstants->MINOR_SERVO_X.name= "X";
+ medMinorServoConstants->MINOR_SERVO_X.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_X.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_X.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_X.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_X.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_X.speed_max= limits[5];
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_Y"]->asString());
+ medMinorServoConstants->MINOR_SERVO_Y.name= "Y";
+ medMinorServoConstants->MINOR_SERVO_Y.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_Y.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_Y.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_Y.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_Y.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_Y.speed_max= limits[5];
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_Z1"]->asString());
+ medMinorServoConstants->MINOR_SERVO_Z1.name= "Z1";
+ medMinorServoConstants->MINOR_SERVO_Z1.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_Z1.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_Z1.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_Z1.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_Z1.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_Z1.speed_max= limits[5];
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_Z2"]->asString());
+ medMinorServoConstants->MINOR_SERVO_Z2.name= "Z2";
+ medMinorServoConstants->MINOR_SERVO_Z2.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_Z2.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_Z2.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_Z2.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_Z2.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_Z2.speed_max= limits[5];
+
+ limits=parseLimitsLine ((const char*)minor_servo_limits_table["MINOR_SERVO_Z3"]->asString());
+ medMinorServoConstants->MINOR_SERVO_Z3.name= "Z3";
+ medMinorServoConstants->MINOR_SERVO_Z3.position_min= limits[0];
+ medMinorServoConstants->MINOR_SERVO_Z3.position_max= limits[1];
+ medMinorServoConstants->MINOR_SERVO_Z3.position_zero= limits[2];
+ medMinorServoConstants->MINOR_SERVO_Z3.position_error= limits[3];
+ medMinorServoConstants->MINOR_SERVO_Z3.speed_min= limits[4];
+ medMinorServoConstants->MINOR_SERVO_Z3.speed_max= limits[5];
+
if(!minor_servo_table.openTable(error)){
_EXCPT_FROM_ERROR(ComponentErrors::CDBAccessExImpl, dummy, error);
throw dummy;
@@ -230,12 +335,14 @@ get_configuration_from_CDB(maci::ContainerServices* services)
(const char*)minor_servo_table["YPaxis"]->asString()
)
);
+
parameters.add_axis(
parseAxisLine(
"ZP",
(const char*)minor_servo_table["ZPaxis"]->asString()
)
);
+
}else{ //secondary focus
parameters.add_axis(
parseAxisLine(
@@ -243,12 +350,14 @@ get_configuration_from_CDB(maci::ContainerServices* services)
(const char*)minor_servo_table["Xaxis"]->asString()
)
);
+
parameters.add_axis(
parseAxisLine(
"Y",
(const char*)minor_servo_table["Yaxis"]->asString()
)
);
+
parameters.add_axis(
parseAxisLine(
"Z",
diff --git a/Medicina/Servers/MedicinaMinorServo/src/testutils.cpp b/Medicina/Servers/MedicinaMinorServo/src/testutils.cpp
index 476ef2aa6f3fa254c99f2afbb8e557d879ce6a52..27934eea5f1b2979a14989a96099131ce514825a 100644
--- a/Medicina/Servers/MedicinaMinorServo/src/testutils.cpp
+++ b/Medicina/Servers/MedicinaMinorServo/src/testutils.cpp
@@ -26,6 +26,8 @@ get_random_axis_value(const MedMinorServoAxis& axis)
MEDMINORSERVOSTATUS
get_random_status()
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
+
MEDMINORSERVOSTATUS status;
int mode = std::rand() % 2;
if(mode == 0)
@@ -33,8 +35,8 @@ get_random_status()
status.mode = 0;
status.time = MedMinorServoTime::ACSToServoNow();
status.system_status = MED_MINOR_SERVO_STATUS_PRIMARY;
- status.pos_x_yp = get_random_axis_value(MINOR_SERVO_YP);
- status.pos_y_zp = get_random_axis_value(MINOR_SERVO_ZP);
+ status.pos_x_yp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_YP);
+ status.pos_y_zp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_ZP);
status.pos_z1 = 0.0;
status.pos_z2 = 0.0;
status.pos_z3 = 0.0;
@@ -42,11 +44,11 @@ get_random_status()
status.mode = 1;
status.time = MedMinorServoTime::ACSToServoNow();
status.system_status = MED_MINOR_SERVO_STATUS_SECONDARY;
- status.pos_x_yp = get_random_axis_value(MINOR_SERVO_X);
- status.pos_y_zp = get_random_axis_value(MINOR_SERVO_Y);
- status.pos_z1 = get_random_axis_value(MINOR_SERVO_Z1);
- status.pos_z2 = get_random_axis_value(MINOR_SERVO_Z2);
- status.pos_z3 = get_random_axis_value(MINOR_SERVO_Z3);
+ status.pos_x_yp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_X);
+ status.pos_y_zp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Y);
+ status.pos_z1 = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Z1);
+ status.pos_z2 = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Z2);
+ status.pos_z3 = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Z3);
}
return status;
}
@@ -54,12 +56,13 @@ get_random_status()
MedMinorServoPosition
get_random_primary_position()
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
MEDMINORSERVOSTATUS status;
status.mode = 0;
status.time = MedMinorServoTime::ACSToServoNow();
status.system_status = MED_MINOR_SERVO_STATUS_PRIMARY;
- status.pos_x_yp = get_random_axis_value(MINOR_SERVO_YP);
- status.pos_y_zp = get_random_axis_value(MINOR_SERVO_ZP);
+ status.pos_x_yp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_YP);
+ status.pos_y_zp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_ZP);
status.pos_z1 = 0.0;
status.pos_z2 = 0.0;
status.pos_z3 = 0.0;
@@ -69,15 +72,16 @@ get_random_primary_position()
MedMinorServoPosition
get_random_secondary_position()
{
+ MedMinorServoConstants *medMinorServoConstants = MedMinorServoConstants::getInstance();
MEDMINORSERVOSTATUS status;
status.mode = 1;
status.time = MedMinorServoTime::ACSToServoNow();
status.system_status = MED_MINOR_SERVO_STATUS_SECONDARY;
- status.pos_x_yp = get_random_axis_value(MINOR_SERVO_X);
- status.pos_y_zp = get_random_axis_value(MINOR_SERVO_Y);
- status.pos_z1 = get_random_axis_value(MINOR_SERVO_Z1);
- status.pos_z2 = get_random_axis_value(MINOR_SERVO_Z2);
- status.pos_z3 = get_random_axis_value(MINOR_SERVO_Z3);
+ status.pos_x_yp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_X);
+ status.pos_y_zp = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Y);
+ status.pos_z1 = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Z1);
+ status.pos_z2 = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Z2);
+ status.pos_z3 = get_random_axis_value(medMinorServoConstants->MINOR_SERVO_Z3);
return MedMinorServoGeometry::positionFromAxes(status);
}
diff --git a/Noto/CDB/alma/DataBlock/MMC/NormalModeSetup/NormalModeSetup.xml b/Noto/CDB/alma/DataBlock/MMC/NormalModeSetup/NormalModeSetup.xml
index 234122febc550ece553b3b836b3bde6be3d8d627..c4b0eece4184934766370b21fea6ea365225a743 100644
--- a/Noto/CDB/alma/DataBlock/MMC/NormalModeSetup/NormalModeSetup.xml
+++ b/Noto/CDB/alma/DataBlock/MMC/NormalModeSetup/NormalModeSetup.xml
@@ -18,7 +18,7 @@
Feeds="1"
IFs="2"
Polarization="L R"
- DefaultLO="6956.0 6956.0"
+ DefaultLO="6936.0 6936.0"
LOMultiplier="1 1"
FixedLO2="0.0 0.0"
LOMin="5000.0 5000.0"
diff --git a/Noto/CDB/alma/DataBlock/MinorServoParameters/MinorServoParameters.xml b/Noto/CDB/alma/DataBlock/MinorServoParameters/MinorServoParameters.xml
index 923bdda5d46047318b3d14186986ae26d51817f9..366203c9aa2df06a39a67d11097f520ce8fb2bd9 100644
--- a/Noto/CDB/alma/DataBlock/MinorServoParameters/MinorServoParameters.xml
+++ b/Noto/CDB/alma/DataBlock/MinorServoParameters/MinorServoParameters.xml
@@ -8,12 +8,26 @@
xmlns:cdb="urn:schemas-cosylab-com:CDB:1.0"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+
+
+ -80,80,0.0,0.05,0.01,14
+ -5,395,0.0,0.05,0.01,8
+ -80,80,0.0,0.05,0.01,14
+ -5,345,0.0,0.05,0.01,8
+ -125,125,0.0,0.05,0.01,14
+ -125,125,0.0,0.05,0.01,14
+ -125,125,0.0,0.05,0.01,14
+
+
SEC
0
1
+
+ -80,80,0.0,0.05,0.01,14
+ -5,395,0.0,0.05,0.01,8
+ -80,80,0.0,0.05,0.01,14
+ -5,345,0.0,0.05,0.01,8
+ -125,125,0.0,0.05,0.01,14
+ -125,125,0.0,0.05,0.01,14
+ -125,125,0.0,0.05,0.01,14
+
+
SEC
0
1
-
-
-
-