diff --git a/SRT/Servers/SRTMinorServo/test/Makefile b/SRT/Servers/SRTMinorServo/test/Makefile index e58a7b68082b99131ff95901219653870822a371..967e4ef9f4fce24d856754ea026f985980ecd6ab 100644 --- a/SRT/Servers/SRTMinorServo/test/Makefile +++ b/SRT/Servers/SRTMinorServo/test/Makefile @@ -22,7 +22,7 @@ SRPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServ PFPProgramTrackTest_OBJECTS = PFPProgramTrackTest PFPProgramTrackTest_CFLAGS = -std=c++17 -fconcepts -PFPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary ComponentErrors boost_filesystem +PFPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary ComponentErrors MinorServoErrors boost_filesystem DerotatorProgramTrackTest_OBJECTS = DerotatorProgramTrackTest DerotatorProgramTrackTest_CFLAGS = -std=c++17 diff --git a/SRT/Servers/SRTMinorServo/test/PFPProgramTrackTest.cpp b/SRT/Servers/SRTMinorServo/test/PFPProgramTrackTest.cpp index c0d12c2bf3dc0a3b077406823305f4fc5db9a963..de84214f761c81dbaaf609808f771d5de5970645 100644 --- a/SRT/Servers/SRTMinorServo/test/PFPProgramTrackTest.cpp +++ b/SRT/Servers/SRTMinorServo/test/PFPProgramTrackTest.cpp @@ -4,13 +4,15 @@ #include #include #include +#include +#include "SRTMinorServoUtils.h" #include "SRTMinorServoTestingSocket.h" #include "SRTMinorServoCommandLibrary.h" // This address and port are the ones set in the simulator // In order for the test to properly be executed, the simulator should be launched with the following command: // discos-simulator -s minor_servo start & -#define SIMULATION +//#define SIMULATION #ifdef SIMULATION #define ADDRESS std::string("127.0.0.1") #define PORT 12800 @@ -22,8 +24,8 @@ #define ADVANCE_TIMEGAP 2.6 //#define MIN_RANGES std::vector{ -1490, -200, -1 } //#define MAX_RANGES std::vector{ 1490, 50, 76 } -#define MIN_RANGES std::vector{ -1000, -100, 0 } -#define MAX_RANGES std::vector{ 1000, 25, 20 } +#define MIN_RANGES std::vector{ -1000, -150, 0 } +#define MAX_RANGES std::vector{ 1000, 40, 70 } #define MAX_SPEED std::vector{ 25, 5, 0.42 } #define STATUS_PERIOD 0.01 @@ -52,9 +54,20 @@ protected: double tn = CIRATools::getUNIXEpoch(); - while(!terminate) + while(!terminate.load()) { - PFPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("PFP")); + try + { + PFPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("PFP")); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + //MinorServoErrors::MinorServoErrorsExImpl impl(ex); + std::cout << getErrorFromEx(ex) << std::endl; + terminate.store(true); + break; + } + std::string status = serializeStatus(PFPStatus); statusFile << status << std::endl; @@ -72,7 +85,7 @@ protected: static void sigintHandler(int sig_num) { std::cout << std::endl << "Terminating..." << std::endl; - terminate = true; + terminate.store(true); } static std::string serializeStatus(SRTMinorServoAnswerMap map) @@ -288,7 +301,7 @@ protected: EXPECT_TRUE(PFPStatus.checkOutput()); std::cout << serializeStatus(PFPStatus) << std::endl; - if(terminate) + if(terminate.load()) FAIL() << "Aborting test..." << std::endl; } while(PFPStatus.get("PFP_OPERATIVE_MODE") != 40); @@ -322,7 +335,7 @@ protected: void TearDown() override { SRTMinorServoTestingSocket::destroyInstance(); - terminate = false; + terminate.store(false); } }; @@ -355,7 +368,7 @@ TEST_F(PFPProgramTrackTest, ContinuousMovementTest) unsigned int idle_count = 0; bool idle = false; - while(!terminate) + while(!terminate.load()) { next_expected_time += TIMEGAP; @@ -414,7 +427,7 @@ TEST_F(PFPProgramTrackTest, AllAxesMovementTest) std::vector idle_count = { 0, 0, 0 }; std::vector idle = { false, false, false }; - while(!terminate) + while(!terminate.load()) { next_expected_time += TIMEGAP; @@ -493,7 +506,7 @@ TEST_F(PFPProgramTrackTest, SineWaveMovementTest) double next_expected_time = start_time; - while(!terminate) + while(!terminate.load()) { next_expected_time += TIMEGAP; double time_delta = next_expected_time - start_time; @@ -525,24 +538,27 @@ TEST_F(PFPProgramTrackTest, SeparateMovementTest) ofstream programTrackFile; programTrackFile.open(directory + "/trajectory.txt", ios::out); - unsigned int axis_to_move = 0; + unsigned int axis_to_move = 1; int sign = 1; unsigned int idle_count = 0; bool idle = false; - std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl; + std::cout << "PRESET position reached" << std::endl; std::vector programTrackCoordinates = startingCoordinates; bool immediate = false; - while(!terminate) + while(!terminate.load()) { double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "starting PROGRAMTRACK with timestamp " << std::to_string(start_time) << std::endl; long unsigned int trajectory_id = int(start_time); double next_expected_time = start_time; unsigned int point_id = 0; - PFPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, point_id, programTrackCoordinates, start_time)); + std::string command = SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, point_id, programTrackCoordinates, start_time); + //std::cout << command; + PFPStatus = socket.sendCommand(command); EXPECT_TRUE(PFPStatus.checkOutput()); programTrackFile << PFPProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; @@ -552,7 +568,7 @@ TEST_F(PFPProgramTrackTest, SeparateMovementTest) EXPECT_TRUE(PFPStatus.checkOutput()); EXPECT_EQ(PFPStatus.get("PFP_OPERATIVE_MODE"), 50); - while(!terminate) + while(!terminate.load()) { next_expected_time += TIMEGAP; @@ -576,7 +592,10 @@ TEST_F(PFPProgramTrackTest, SeparateMovementTest) idle = true; } - PFPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, point_id, programTrackCoordinates)); + command = SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, point_id, programTrackCoordinates); + PFPStatus = socket.sendCommand(command); + //std::cout << command; + //PFPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, point_id, programTrackCoordinates)); EXPECT_TRUE(PFPStatus.checkOutput()); } @@ -604,7 +623,7 @@ TEST_F(PFPProgramTrackTest, RapidTrajectoryTest) std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl; - while(!terminate) + while(!terminate.load()) { unsigned int axis_to_move = 1; int sign = -1; @@ -627,7 +646,7 @@ TEST_F(PFPProgramTrackTest, RapidTrajectoryTest) EXPECT_TRUE(PFPStatus.checkOutput()); EXPECT_EQ(PFPStatus.get("PFP_OPERATIVE_MODE"), 50); - while(!terminate) + while(!terminate.load()) { next_expected_time += TIMEGAP; diff --git a/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp b/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp index e1033748eb879d786d4ae5696ce16b36ea41c89c..9d2b46c44eb18aea9a66265e5aff973c0f36fc08 100644 --- a/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp +++ b/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp @@ -22,7 +22,7 @@ #define TIMEGAP 0.2 #define ADVANCE_TIMEGAP 2.6 #define MAX_RANGES std::vector{ 40, 40, 40, 0.2, 0.2, 0.2 } -#define MAX_RANGES std::vector{ 50, 110, 50, 0.25, 0.25, 0.25 } +#define MAX_RANGES std::vector{ 50, 120, 120, 0.25, 0.25, 0.25 } #define MAX_SPEED std::vector{ 4, 4, 4, 0.04, 0.04, 0.04 } #define STATUS_PERIOD 0.01 @@ -432,7 +432,7 @@ TEST_F(SRPProgramTrackTest, SineWaveMovementTest) std::vector phase_shift; std::vector period; - for(size_t axis = 0; axis < 6; axis++) + for(size_t axis = 0; axis < 3; axis++) { double period_multiplier = axis < 3 ? 4 : 4; period.push_back(MAX_RANGES[axis] / MAX_SPEED[axis] * period_multiplier); @@ -465,7 +465,7 @@ TEST_F(SRPProgramTrackTest, SineWaveMovementTest) std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); point_id++; - for(size_t axis = 0; axis < 6; axis++) + for(size_t axis = 0; axis < 3; axis++) { programTrackCoordinates[axis] = MAX_RANGES[axis] * sin((time_delta + phase_shift[axis]) * 2 * M_PI / period[axis]); }