Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

FSU Speed Limit Handling #542

Merged
merged 13 commits into from
Dec 20, 2023
Merged

FSU Speed Limit Handling #542

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
544c41d
Added basic FSU Speed Limit Handling
EricMarcil Apr 24, 2023
305136f
Added multiple control group support
EricMarcil Apr 24, 2023
6d69201
Updated prevPulsePosData for all control groups
EricMarcil Apr 24, 2023
5c7870e
Added MotoRosYRC1u_,out
EricMarcil Apr 25, 2023
196b5c0
Delete MotoRosYRC1u_1910.out
EricMarcil Apr 25, 2023
9e3babc
Change to support DX100
EricMarcil Jun 14, 2023
0654dfa
Reset Speed Limit related variables when robot is not under ROS motio…
EricMarcil Sep 27, 2023
0adc258
Reset of hasUnprocessedData
EricMarcil Oct 6, 2023
9c6ff7d
Added comments
EricMarcil Oct 17, 2023
961183a
Update MotionServer.c
EricMarcil Oct 20, 2023
860fa61
Fix last commit error
EricMarcil Oct 23, 2023
3166e21
Correction following Ted's review
EricMarcil Dec 20, 2023
d4e4073
Removed binary output file
EricMarcil Dec 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,3 +59,7 @@ motoman_driver/MotoPlus/_buildLog/MotoROSYRC1000u.log
/motoman_gp8_support/out/build/x64-Debug (default)
/motoman_gp88_support/out/build/x64-Debug (default)
/motoman_gp7_support/out/build/x64-Debug (default)
*.vsidx
motoman_driver/MotoPlus/.vs/MpRosAllControllers/FileContentIndex/read.lock
*.db-shm
*.db-wal
305 changes: 246 additions & 59 deletions motoman_driver/MotoPlus/MotionServer.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@

#include "MotoROS.h"

#ifdef DEBUG
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I recommend removing this condition. The DEBUG preprocessor was to protect against costly printf commands. The udp-broadcast method is way more efficient. I think it's safe to allow these w/o also enabling the printf.

#include "debug.h"
#endif

//-----------------------
// Function Declarations
//-----------------------
Expand Down Expand Up @@ -1668,7 +1672,37 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
LONG q_time;
int axis;
//BOOL bNoData = TRUE; // for testing
MP_CTRL_GRP_SEND_DATA ctrlGrpData;
MP_PULSE_POS_RSP_DATA prevPulsePosData[MAX_CONTROLLABLE_GROUPS];
MP_PULSE_POS_RSP_DATA pulsePosData;

LONG newPulseInc[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; // Pulse increments that we just retrieved from the incQueue
LONG toProcessPulses[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; // Total pulses that still need to be sent to the command
LONG processedPulses[MP_GRP_AXES_NUM]; // Amount of pulses from the last command that were actually processed (accepted)
LONG prevMaxSpeed[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM];
LONG prevMaxSpeedRemain[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM];
LONG maxSpeed[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM];
LONG maxSpeedRemain[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM];
BOOL skipReadingQ[MAX_CONTROLLABLE_GROUPS];
BOOL queueRead[MAX_CONTROLLABLE_GROUPS];
BOOL isMissingPulse;
BOOL hasUnprocessedData;

ctrlGrpData.sCtrlGrp = 0;

memset(newPulseInc, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
memset(toProcessPulses, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
memset(processedPulses, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
memset(prevMaxSpeed, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
memset(prevMaxSpeedRemain, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
memset(maxSpeed, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
memset(maxSpeedRemain, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
memset(skipReadingQ, 0x00, sizeof(BOOL) * MAX_CONTROLLABLE_GROUPS);
memset(queueRead, 0x00, sizeof(BOOL) * MAX_CONTROLLABLE_GROUPS);

isMissingPulse = FALSE;
hasUnprocessedData = FALSE;

printf("IncMoveTask Started\r\n");

memset(&moveData, 0x00, sizeof(moveData));
Expand All @@ -1677,92 +1711,108 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
{
moveData.ctrl_grp |= (0x01 << i);
moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(controller->ctrlGroups[i]);
mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]);
}

FOREVER
{
mpClkAnnounce(MP_INTERPOLATION_CLK);

if (Ros_Controller_IsMotionReady(controller)
&& Ros_MotionServer_HasDataInQueue(controller)
&& (Ros_MotionServer_HasDataInQueue(controller) || hasUnprocessedData)
&& !controller->bStopMotion)
{
//bNoData = FALSE; // for testing

for(i=0; i<controller->numGroup; i++)
for (i = 0; i < controller->numGroup; i++)
{
q = &controller->ctrlGroups[i]->inc_q;
queueRead[i] = FALSE;

// Lock the q before manipulating it
if(mpSemTake(q->q_lock, Q_LOCK_TIMEOUT) == OK)
if (skipReadingQ[i])
{
skipReadingQ[i] = FALSE;
memset(&moveData.grp_pos_info[i].pos, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
}
else
{
if(q->cnt > 0)
q = &controller->ctrlGroups[i]->inc_q;

// Lock the q before manipulating it
if (mpSemTake(q->q_lock, Q_LOCK_TIMEOUT) == OK)
{
time = q->data[q->idx].time;
q_time = controller->ctrlGroups[i]->q_time;
moveData.grp_pos_info[i].pos_tag.data[2] = q->data[q->idx].tool;
moveData.grp_pos_info[i].pos_tag.data[3] = q->data[q->idx].frame;
moveData.grp_pos_info[i].pos_tag.data[4] = q->data[q->idx].user;

memcpy(&moveData.grp_pos_info[i].pos, &q->data[q->idx].inc, sizeof(LONG) * MP_GRP_AXES_NUM);

// increment index in the queue and decrease the count
q->idx = Q_OFFSET_IDX( q->idx, 1, Q_SIZE );
q->cnt--;

// Check if complet interpolation period covered
while(q->cnt > 0)
if (q->cnt > 0)
{
if( (q_time <= q->data[q->idx].time)
&& (q->data[q->idx].time - q_time <= controller->interpolPeriod) )
{
// next incMove is part of same interpolation period

// check that information is in the same format
if( (moveData.grp_pos_info[i].pos_tag.data[2] != q->data[q->idx].tool)
|| (moveData.grp_pos_info[i].pos_tag.data[3] != q->data[q->idx].frame)
|| (moveData.grp_pos_info[i].pos_tag.data[4] != q->data[q->idx].user) )
time = q->data[q->idx].time;
q_time = controller->ctrlGroups[i]->q_time;
moveData.grp_pos_info[i].pos_tag.data[2] = q->data[q->idx].tool;
moveData.grp_pos_info[i].pos_tag.data[3] = q->data[q->idx].frame;
moveData.grp_pos_info[i].pos_tag.data[4] = q->data[q->idx].user;

memcpy(&moveData.grp_pos_info[i].pos, &q->data[q->idx].inc, sizeof(LONG) * MP_GRP_AXES_NUM);
queueRead[i] = TRUE;
#ifdef DEBUG
Debug_BroadcastMsg("New Inc from Buffer: %d, %d, %d, %d, %d, %d\r\n",
moveData.grp_pos_info[i].pos[0], moveData.grp_pos_info[i].pos[1], moveData.grp_pos_info[i].pos[2],
moveData.grp_pos_info[i].pos[3], moveData.grp_pos_info[i].pos[4], moveData.grp_pos_info[i].pos[5]);
#endif
// increment index in the queue and decrease the count
q->idx = Q_OFFSET_IDX(q->idx, 1, Q_SIZE);
q->cnt--;

// Check if complet interpolation period covered
while (q->cnt > 0)
{
if ((q_time <= q->data[q->idx].time)
&& (q->data[q->idx].time - q_time <= controller->interpolPeriod))
{
// next incMove is part of same interpolation period

// check that information is in the same format
if ((moveData.grp_pos_info[i].pos_tag.data[2] != q->data[q->idx].tool)
|| (moveData.grp_pos_info[i].pos_tag.data[3] != q->data[q->idx].frame)
|| (moveData.grp_pos_info[i].pos_tag.data[4] != q->data[q->idx].user))
{
// Different format can't combine information
break;
}

// add next incMove to current incMove
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
moveData.grp_pos_info[i].pos[axis] += q->data[q->idx].inc[axis];
time = q->data[q->idx].time;

// increment index in the queue and decrease the count
q->idx = Q_OFFSET_IDX(q->idx, 1, Q_SIZE);
q->cnt--;
}
else
{
// Different format can't combine information
// interpolation period complet
break;
}

// add next incMove to current incMove
for(axis=0; axis<MP_GRP_AXES_NUM; axis++)
moveData.grp_pos_info[i].pos[axis] += q->data[q->idx].inc[axis];
time = q->data[q->idx].time;

// increment index in the queue and decrease the count
q->idx = Q_OFFSET_IDX( q->idx, 1, Q_SIZE );
q->cnt--;
}
else
{
// interpolation period complet
break;
}

controller->ctrlGroups[i]->q_time = time;
}

controller->ctrlGroups[i]->q_time = time;
else
{
moveData.grp_pos_info[i].pos_tag.data[2] = 0;
moveData.grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE;
moveData.grp_pos_info[i].pos_tag.data[4] = 0;
memset(&moveData.grp_pos_info[i].pos, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
}

// Unlock the q
mpSemGive(q->q_lock);
}
else
{
moveData.grp_pos_info[i].pos_tag.data[2] = 0;
moveData.grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE;
moveData.grp_pos_info[i].pos_tag.data[4] = 0;
printf("ERROR: Can't get data from queue. Queue is locked up.\r\n");
memset(&moveData.grp_pos_info[i].pos, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
continue;
}

// Unlock the q
mpSemGive(q->q_lock);
}
else
{
printf("ERROR: Can't get data from queue. Queue is locked up.\r\n");
memset(&moveData.grp_pos_info[i].pos, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
continue;
}
}
}

#if DX100
// first robot
Expand Down Expand Up @@ -1796,6 +1846,133 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
}
}
#else
// --- FSU Speed Limit Check ---
hasUnprocessedData = FALSE;

for (i = 0; i < controller->numGroup; i++)
{
memcpy(newPulseInc[i], moveData.grp_pos_info[i].pos, sizeof(LONG)* MP_GRP_AXES_NUM);

// record the speed associate with the next amount of pulses
if (queueRead[i])
{
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
{
maxSpeed[i][axis] = abs(newPulseInc[i][axis]);
maxSpeedRemain[i][axis] = abs(newPulseInc[i][axis]);
}
}

// Check if pulses are missing from last increment
mpGetPulsePos(&ctrlGrpData, &pulsePosData);
isMissingPulse = FALSE;
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
{
// Check how many pulses we processed from last increment
processedPulses[axis] = pulsePosData.lPos[axis] - prevPulsePosData[i].lPos[axis];
prevPulsePosData[i].lPos[axis] = pulsePosData.lPos[axis];

// Remove those pulses from the amount to process.
// If everything was processed, then there should by 0 pulses left. Otherwise FSU Speed limit prevented processing
toProcessPulses[i][axis] -= processedPulses[axis];
if (toProcessPulses[i][axis] != 0)
isMissingPulse = TRUE;

// Add the new pulses to be processed for this iteration
toProcessPulses[i][axis] += newPulseInc[i][axis];

if(toProcessPulses[i][axis] != 0)
hasUnprocessedData = TRUE;
}

if (isMissingPulse)
{
LONG max_inc;

// Prevent going faster than original requested speed once speed limit turns off
// Check if the speed (inc) of previous interation should be considered by checking
// if the unprocessed pulses from that speed setting still remains.
// If all the pulses of previous increment were processed, then transfer the current
// speed and process the next increment from the increment queue.
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
{
// Check if has pulses to process
if (toProcessPulses[i][axis] == 0)
prevMaxSpeedRemain[i][axis] = 0;
else
prevMaxSpeedRemain[i][axis] = abs(prevMaxSpeedRemain[i][axis]) - abs(processedPulses[axis]);
}

// Check if still have data to process from previous iteration
skipReadingQ[i] = FALSE;
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
{
if (prevMaxSpeedRemain[i][axis] > 0)
skipReadingQ[i] = TRUE;
}

if (!skipReadingQ[i]) {
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++) {
// Transfer the current speed as the new prevSpeed
prevMaxSpeed[i][axis] = maxSpeed[i][axis];
prevMaxSpeedRemain[i][axis] += maxSpeedRemain[i][axis];
}
}

// Set the number of pulse that can be sent without exceeding speed
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
{
// Check if has pulses to process
if (toProcessPulses[i][axis] == 0)
continue;

// Maximum inc that should be send ()
if (prevMaxSpeed[i][axis] > 0)
// if previous speed is defined use it
max_inc = prevMaxSpeed[i][axis];
else
{
if (maxSpeed[i][axis] > 0)
// else fallback on current speed if defined
max_inc = maxSpeed[i][axis];
else if (newPulseInc[i][axis] != 0)
// use the current speed if none zero.
max_inc = abs(newPulseInc[i][axis]);
else
// otherwise use the axis max speed
max_inc = controller->ctrlGroups[i]->maxInc.maxIncrement[axis];
#ifdef DEBUG
Debug_BroadcastMsg("Warning undefined speed: Axis%d Defaulting Max Inc: %d (prevSpeed: %d curSpeed %d)\r\n",
axis, max_inc, prevMaxSpeed[i][axis], maxSpeed[i][axis]);
#endif
}

// Set new increment and recalculate unsent pulses
if (abs(toProcessPulses[i][axis]) <= max_inc)
{
// Pulses to send is small than max, so send everything
moveData.grp_pos_info[i].pos[axis] = toProcessPulses[i][axis];
}
else {
// Pulses to send is too high, so send the amount matching the maximum speed
if (toProcessPulses[i][axis] >= 0)
moveData.grp_pos_info[i].pos[axis] = max_inc;
else
moveData.grp_pos_info[i].pos[axis] = -max_inc;
}
}
}
else
{
// No PFL Speed Limit detected
for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
{
prevMaxSpeed[i][axis] = abs(moveData.grp_pos_info[i].pos[axis]);
prevMaxSpeedRemain[i][axis] = abs(moveData.grp_pos_info[i].pos[axis]);
}
}
}

ret = mpExRcsIncrementMove(&moveData);
if(ret != 0)
{
Expand Down Expand Up @@ -1846,6 +2023,16 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
Ros_MotionServer_StopMotion(controller);
}
}

#ifdef DEBUG
i = 0;
for(axis=0; axis<6; axis++)

Debug_BroadcastMsg("Axis %d: New: %d ToProcess: %d Sent: %d MaxSpeed: %d for remaining %d Skip: %d\r\n",
axis, newPulseInc[i][axis], toProcessPulses[i][axis],
moveData.grp_pos_info[i].pos[axis], prevMaxSpeed[i][axis], prevMaxSpeedRemain[i][axis], skipReadingQ[i]);
#endif

#endif

}
Expand Down
2 changes: 2 additions & 0 deletions motoman_driver/MotoPlus/MpRosAllControllers.vcxproj
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,7 @@
<ItemGroup>
<ClCompile Include="Controller.c" />
<ClCompile Include="CtrlGroup.c" />
<ClCompile Include="debug.c" />
<ClCompile Include="IoServer.c" />
<ClCompile Include="MotionServer.c" />
<ClCompile Include="mpMain.c" />
Expand All @@ -188,6 +189,7 @@
<ItemGroup>
<ClInclude Include="Controller.h" />
<ClInclude Include="CtrlGroup.h" />
<ClInclude Include="debug.h" />
<ClInclude Include="IoServer.h" />
<ClInclude Include="MotionServer.h" />
<ClInclude Include="MotoROS.h" />
Expand Down
Loading