>From 27b02cdf7b5cd7eb69adc178c171b4b95257318e Mon Sep 17 00:00:00 2001
From: Frank Tkalcevic <frankt@cncrouter.(none)>
Date: Tue, 1 Nov 2011 10:54:09 +1100
Subject: [PATCH 3/3] Limit the velocities in the SetTeleopVector command to the values defined in [AXIS_n]MAX_VELOCITY

---
 src/emc/task/taskintf.cc |   52 ++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 52 insertions(+), 0 deletions(-)

diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc
index 157206f..269c86a 100644
--- a/src/emc/task/taskintf.cc
+++ b/src/emc/task/taskintf.cc
@@ -691,10 +691,62 @@ int emcTrajSetMode(int mode)
     }
 }
 
+static double getPoseByAxis( int axis, EmcPose *pos )
+{
+    switch ( axis )
+    {
+        case 0: return pos->tran.x;
+        case 1: return pos->tran.y;
+        case 2: return pos->tran.z;
+        case 3: return pos->a;
+        case 4: return pos->b;
+        case 5: return pos->c;
+        case 6: return pos->u;
+        case 7: return pos->v;
+        case 8: return pos->w;
+        default: 
+	    printf("axis number out of range %d in getPoseByAxis()\n", axis);
+            return 0;
+    }
+}
+
+static void setPoseByAxis( int axis, EmcPose *pos, double value )
+{
+    switch ( axis )
+    {
+        case 0: pos->tran.x = value; break;
+        case 1: pos->tran.y = value; break;
+        case 2: pos->tran.z = value; break;
+        case 3: pos->a = value; break;
+        case 4: pos->b = value; break;
+        case 5: pos->c = value; break;
+        case 6: pos->u = value; break;
+        case 7: pos->v = value; break;
+        case 8: pos->w = value; break;
+        default: 
+	    printf("axis number out of range %d in setPoseByAxis()\n", axis);
+            break;
+    }
+}
+
 int emcTrajSetTeleopVector(EmcPose vel)
 {
+    int axis;
+
     emcmotCommand.command = EMCMOT_SET_TELEOP_VECTOR;
     emcmotCommand.pos = vel;
+
+    for ( axis = 0; axis < EMCMOT_MAX_JOINTS; axis++ ) {
+
+        double max_vel = AXIS_MAX_VELOCITY[axis];
+        double axis_vel = getPoseByAxis( axis, &emcmotCommand.pos );
+
+        if ( axis_vel > max_vel ) {
+            setPoseByAxis( axis, &emcmotCommand.pos, max_vel );
+        } else if ( axis_vel < -max_vel ) {
+            setPoseByAxis( axis, &emcmotCommand.pos,  -max_vel );
+        }
+    }
     return usrmotWriteEmcmotCommand(&emcmotCommand);
 }
 
-- 
1.7.0.4

