Skip to content
Snippets Groups Projects
Commit e1fe250d authored by Giuseppe Carboni's avatar Giuseppe Carboni
Browse files

Updated PFP tests

parent e2365184
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
SRT/Servers/SRTMinorServo/test/PFPProgramTrackTest.cpp
+ 99
− 67
View file @ e1fe250d
......@@ -18,11 +18,12 @@
#define ADDRESS std::string("192.168.200.13")
#define PORT 4758
#endif
#define NOISE_THRESHOLD 1
#define TIMEGAP 0.2
#define ADVANCE_TIMEGAP 2.6
#define MIN_RANGES std::vector<double>{ -1490, -200, -1 }
#define MAX_RANGES std::vector<double>{ 1490, 50, 76 }
//#define MIN_RANGES std::vector<double>{ -1490, -200, -1 }
//#define MAX_RANGES std::vector<double>{ 1490, 50, 76 }
#define MIN_RANGES std::vector<double>{ -1000, -100, 0 }
#define MAX_RANGES std::vector<double>{ 1000, 25, 20 }
#define MAX_SPEED std::vector<double>{ 25, 5, 0.42 }
#define STATUS_PERIOD 0.01
......@@ -155,29 +156,82 @@ protected:
return currentElongations;
}
static bool moveAxis(std::vector<double> &coordinates, int axis_to_move, int sign)
bool moveAxis(std::vector<double>& coordinates, unsigned int& axis_to_move, int& sign)
{
double starting_sign = coordinates[axis_to_move] > 0 ? 1 : (coordinates[axis_to_move] < 0 ? -1 : 0);
bool last_segment = false;
bool go_idle = false;
double starting_position = coordinates[axis_to_move];
if(startingCoordinates[axis_to_move] == MIN_RANGES[axis_to_move] || startingCoordinates[axis_to_move] == MAX_RANGES[axis_to_move])
{
if(starting_position == startingCoordinates[axis_to_move])
{
// We just started the trajectory for the current axis, correct the sign eventually
if(startingCoordinates[axis_to_move] == MIN_RANGES[axis_to_move])
{
// Only above
sign = 1;
}
else
{
// Only below
sign = -1;
}
}
else
{
// We are executing the trajectory, check if we are already headed towards the starting position
if((startingCoordinates[axis_to_move] == MIN_RANGES[axis_to_move] && sign < 0) || (startingCoordinates[axis_to_move] == MAX_RANGES[axis_to_move] && sign > 0))
{
last_segment = true;
}
}
}
else if(starting_position < startingCoordinates[axis_to_move] && sign > 0)
{
last_segment = true;
}
double offset_to_add = MAX_SPEED[axis_to_move] / 5;
coordinates[axis_to_move] += sign * offset_to_add;
double ending_sign = coordinates[axis_to_move] > 0 ? 1 : (coordinates[axis_to_move] < 0 ? -1 : 0);
if(starting_sign == -1 && ending_sign >= 0)
double ending_position = starting_position + sign * offset_to_add;
if(ending_position >= MAX_RANGES[axis_to_move])
{
// Zero crossed
coordinates[axis_to_move] = 0.0;
return false;
// Top reached, go down
ending_position = MAX_RANGES[axis_to_move];
go_idle = true;
}
if(coordinates[axis_to_move] >= MAX_RANGES[axis_to_move])
else if(ending_position <= MIN_RANGES[axis_to_move])
{
coordinates[axis_to_move] = MAX_RANGES[axis_to_move];
return true;
// Bottom reached, go up
ending_position = MIN_RANGES[axis_to_move];
go_idle = true;
}
else if(coordinates[axis_to_move] <= MIN_RANGES[axis_to_move])
// Check if we finished the current axis' trajectory
if(sign == 1 && starting_position < startingCoordinates[axis_to_move] && startingCoordinates[axis_to_move] <= ending_position)
{
coordinates[axis_to_move] = MIN_RANGES[axis_to_move];
return true;
// Crossed zero while going up, round to zero and change axis
ending_position = startingCoordinates[axis_to_move];
go_idle = true;
}
coordinates[axis_to_move] = ending_position;
if(go_idle)
{
if(last_segment)
{
sign = 1;
axis_to_move == 2 ? axis_to_move = 0 : axis_to_move++;
}
else
{
sign *= -1;
}
return false;
}
return go_idle;
}
void SetUp() override
......@@ -301,8 +355,6 @@ TEST_F(PFPProgramTrackTest, ContinuousMovementTest)
unsigned int idle_count = 0;
bool idle = false;
while(!terminate)
{
while(!terminate)
{
next_expected_time += TIMEGAP;
......@@ -321,7 +373,6 @@ TEST_F(PFPProgramTrackTest, ContinuousMovementTest)
}
else if(moveAxis(programTrackCoordinates, axis_to_move, sign))
{
sign *= -1;
idle = true;
}
......@@ -329,15 +380,6 @@ TEST_F(PFPProgramTrackTest, ContinuousMovementTest)
EXPECT_TRUE(PFPStatus.checkOutput());
//std::cout << PFPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl;
programTrackFile << PFPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl;
if(round(programTrackCoordinates[axis_to_move] * 100) == 0 && sign == 1)
{
programTrackCoordinates[axis_to_move] = 0.0;
break;
}
}
axis_to_move == 2 ? axis_to_move = 0 : axis_to_move++;
}
programTrackFile.close();
......@@ -383,12 +425,15 @@ TEST_F(PFPProgramTrackTest, AllAxesMovementTest)
{
if(!idle[axis])
{
if(moveAxis(programTrackCoordinates, axis, sign[axis]))
unsigned int axis_to_move = axis;
if(moveAxis(programTrackCoordinates, axis_to_move, sign[axis]))
{
if(programTrackCoordinates[axis] != startingCoordinates[axis] || programTrackCoordinates[axis] == MIN_RANGES[axis] || programTrackCoordinates[axis] == MAX_RANGES[axis])
{
sign[axis] *= -1;
idle[axis] = true;
}
}
}
else
{
idle_count[axis]++;
......@@ -404,14 +449,6 @@ TEST_F(PFPProgramTrackTest, AllAxesMovementTest)
EXPECT_TRUE(PFPStatus.checkOutput());
//std::cout << PFPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl;
programTrackFile << PFPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl;
for(size_t axis = 0; axis < 3; axis++)
{
if(round(programTrackCoordinates[axis] * 100) == 0 && sign[axis] == 1)
{
programTrackCoordinates[axis] = 0.0;
}
}
}
programTrackFile.close();
......@@ -496,7 +533,7 @@ TEST_F(PFPProgramTrackTest, SeparateMovementTest)
std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl;
std::vector<double> programTrackCoordinates = startingCoordinates;
bool immediate = true;
bool immediate = false;
while(!terminate)
{
......@@ -536,12 +573,6 @@ TEST_F(PFPProgramTrackTest, SeparateMovementTest)
{
if(moveAxis(programTrackCoordinates, axis_to_move, sign))
{
sign *= -1;
idle = true;
}
else if(programTrackCoordinates[axis_to_move] == 0.0 && sign == 1)
{
axis_to_move == 2 ? axis_to_move = 0 : axis_to_move++;
idle = true;
}
......@@ -568,8 +599,6 @@ TEST_F(PFPProgramTrackTest, RapidTrajectoryTest)
ofstream programTrackFile;
programTrackFile.open(directory + "/trajectory.txt", ios::out);
unsigned int axis_to_move = 1;
int sign = -1;
unsigned int idle_count = 0;
bool idle = false;
......@@ -577,6 +606,9 @@ TEST_F(PFPProgramTrackTest, RapidTrajectoryTest)
while(!terminate)
{
unsigned int axis_to_move = 1;
int sign = -1;
std::vector<double> programTrackCoordinates = startingCoordinates;
programTrackCoordinates[axis_to_move] = MAX_RANGES[axis_to_move];
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment