Merge branch 'feature/reverse-run-master2'

src/emc/motion/command.c

@@ -1218,6 +1218,20 @@ void emcmotCommandHandler(void *arg, long period)
 	    emcmotStatus->paused = 1;
 	    break;
 
+	case EMCMOT_REVERSE:
+	    /* run motion in reverse*/
+	    /* only allowed during a pause */
+	    rtapi_print_msg(RTAPI_MSG_DBG, "REVERSE");
+        tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_REVERSE);
+	    break;
+
+	case EMCMOT_FORWARD:
+	    /* run motion in reverse*/
+	    /* only allowed during a pause */
+	    rtapi_print_msg(RTAPI_MSG_DBG, "FORWARD");
+        tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_FORWARD);
+	    break;
+
 	case EMCMOT_RESUME:
 	    /* resume paused motion */
 	    /* can happen at any time */
@@ -1670,7 +1684,7 @@ void emcmotCommandHandler(void *arg, long period)
 	    /* if (emcmotStatus->spindle.orient) { */
 	    /* 	reportError(_("cant turn on spindle during orient in progress")); */
 	    /* 	emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; */
-	    /* 	tpAbort(&emcmotDebug->tp); */
+	    /* 	tpAbort(&emcmotDebug->coord_tp); */
 	    /* 	SET_MOTION_ERROR_FLAG(1); */
 	    /* } else {...} */
 	    emcmotStatus->spindle_status[spindle_num].speed = emcmotCommand->vel;
@@ -1727,7 +1741,7 @@ void emcmotCommandHandler(void *arg, long period)
 		// mah:FIXME unsure wether this is ok or an error
 		/* reportError(_("orient already in progress")); */
 		/* emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; */
-		/* tpAbort(&emcmotDebug->tp); */
+		/* tpAbort(&emcmotDebug->coord_tp); */
 		/* SET_MOTION_ERROR_FLAG(1); */
 	    }
 	    emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_IN_PROGRESS;

src/emc/motion/control.c

@@ -239,7 +239,7 @@ void emcmotController(void *arg, long period)
 static void process_inputs(void)
 {
     int joint_num, spindle_num;
-    double abs_ferror, tmp, scale;
+    double abs_ferror, scale;
     joint_hal_t *joint_data;
     emcmot_joint_t *joint;
     unsigned char enables;
@@ -271,14 +271,36 @@ static void process_inputs(void)
         }
     }
     if ( enables & AF_ENABLED ) {
-	/* read and clamp (0.0 to 1.0) adaptive feed HAL pin */
-	tmp = *emcmot_hal_data->adaptive_feed;
-	if ( tmp > 1.0 ) {
-	    tmp = 1.0;
-	} else if ( tmp < 0.0 ) {
-	    tmp = 0.0;
-	}
-	scale *= tmp;
+        /* read and clamp adaptive feed HAL pin */
+        double adaptive_feed_in = *emcmot_hal_data->adaptive_feed;
+
+        // Clip range to +/- 1.0
+        if ( adaptive_feed_in > 1.0 ) {
+            adaptive_feed_in = 1.0;
+        } else if (adaptive_feed_in < -1.0) {
+            adaptive_feed_in = -1.0;
+        }
+
+        // Actual scale factor is always positive by default
+        double adaptive_feed_out = fabs(adaptive_feed_in);
+
+        // Case 1: positive to negative direction change
+        if ( adaptive_feed_in < 0.0 && emcmotDebug->coord_tp.reverse_run == TC_DIR_FORWARD) {
+            // User commands feed in reverse direction, but we're not running in reverse yet
+            if (tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_REVERSE) != TP_ERR_OK) {
+                // Need to decelerate to a stop first
+                adaptive_feed_out = 0.0;
+            }
+        } else if (adaptive_feed_in > 0.0 && emcmotDebug->coord_tp.reverse_run == TC_DIR_REVERSE ) {
+            // User commands feed in forward direction, but we're running in reverse
+            if (tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_FORWARD) != TP_ERR_OK) {
+                // Need to decelerate to a stop first
+                adaptive_feed_out = 0.0;
+            }
+        }
+        //Otherwise, if direction and sign match, we're ok
+
+        scale *= adaptive_feed_out;
     }
     if ( enables & FH_ENABLED ) {
 	/* read feed hold HAL pin */
@@ -1802,6 +1824,7 @@ static void output_to_hal(void)
     }
 
     *(emcmot_hal_data->program_line) = emcmotStatus->id;
+    *(emcmot_hal_data->tp_reverse) = emcmotStatus->reverse_run;
     *(emcmot_hal_data->motion_type) = emcmotStatus->motionType;
     *(emcmot_hal_data->distance_to_go) = emcmotStatus->distance_to_go;
     if(GET_MOTION_COORD_FLAG()) {
@@ -2023,6 +2046,8 @@ static void update_status(void)
     emcmotStatus->depth = tpQueueDepth(&emcmotDebug->coord_tp);
     emcmotStatus->activeDepth = tpActiveDepth(&emcmotDebug->coord_tp);
     emcmotStatus->id = tpGetExecId(&emcmotDebug->coord_tp);
+    //KLUDGE add an API call for this
+    emcmotStatus->reverse_run = emcmotDebug->coord_tp.reverse_run;
     emcmotStatus->motionType = tpGetMotionType(&emcmotDebug->coord_tp);
     emcmotStatus->queueFull = tcqFull(&emcmotDebug->coord_tp.queue);
 

src/emc/motion/mot_priv.h

@@ -146,6 +146,7 @@ typedef struct {
     hal_bit_t *feed_hold;	/* RPI: set TRUE to stop motion maskable with g53 P1*/
     hal_bit_t *feed_inhibit;	/* RPI: set TRUE to stop motion (non maskable)*/
     hal_bit_t *homing_inhibit;	/* RPI: set TRUE to inhibit homing*/
+    hal_bit_t *tp_reverse;	/* Set true if trajectory planner is running in reverse*/
     hal_bit_t *motion_enabled;	/* RPI: motion enable for all joints */
     hal_bit_t *in_position;	/* RPI: all joints are in position */
     hal_bit_t *coord_mode;	/* RPA: TRUE if coord, FALSE if free */

src/emc/motion/motion.c

@@ -327,6 +327,8 @@ static int init_hal_io(void)
     if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) != 0) goto error;
     if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->homing_inhibit), mot_comp_id, "motion.homing-inhibit")) != 0) goto error;
     if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) != 0) goto error;
+        if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->tp_reverse), mot_comp_id, "motion.tp-reverse")) < 0) goto error;
+    *(emcmot_hal_data->tp_reverse) = 0;
 
     /* export motion-synched digital output pins */
     /* export motion digital input pins */

src/emc/motion/motion.h

@@ -107,6 +107,8 @@ extern "C" {
 	EMCMOT_DISABLE,		/* disable servos for active joints */
 
 	EMCMOT_PAUSE,		/* pause motion */
+	EMCMOT_REVERSE,		/* run reverse motion */
+	EMCMOT_FORWARD,		/* run reverse motion */
 	EMCMOT_RESUME,		/* resume motion */
 	EMCMOT_STEP,		/* resume motion until id encountered */
 	EMCMOT_FREE,		/* set mode to free (joint) motion */
@@ -710,7 +712,7 @@ Suggestion: Split this in to an Error and a Status flag register..
 	int overrideLimitMask;	/* non-zero means one or more limits ignored */
 				/* 1 << (joint-num*2) = ignore neg limit */
 				/* 2 << (joint-num*2) = ignore pos limit */
-
+    int reverse_run;
 
 	/* static status-- only changes upon input commands, e.g., config */
 	double vel;		/* scalar max vel */

src/emc/nml_intf/emc.hh

@@ -167,6 +167,8 @@ struct PM_CARTESIAN;
 #define EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE         ((NMLTYPE) 517)
 #define EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE          ((NMLTYPE) 518)
 #define EMC_TASK_PLAN_OPTIONAL_STOP_TYPE             ((NMLTYPE) 519)
+#define EMC_TASK_PLAN_REVERSE_TYPE                   ((NMLTYPE) 520)
+#define EMC_TASK_PLAN_FORWARD_TYPE                   ((NMLTYPE) 521)
 
 #define EMC_TASK_STAT_TYPE                           ((NMLTYPE) 599)
 
@@ -411,6 +413,8 @@ extern int emcTrajEnable();
 extern int emcTrajDisable();
 extern int emcTrajAbort();
 extern int emcTrajPause();
+extern int emcTrajReverse();
+extern int emcTrajForward();
 extern int emcTrajStep();
 extern int emcTrajResume();
 extern int emcTrajDelay(double delay);

src/emc/nml_intf/emc_nml.hh

@@ -1346,6 +1346,23 @@ class EMC_TASK_PLAN_PAUSE:public EMC_TASK_CMD_MSG {
     void update(CMS * cms);
 };
 
+class EMC_TASK_PLAN_REVERSE:public EMC_TASK_CMD_MSG {
+  public:
+    EMC_TASK_PLAN_REVERSE():EMC_TASK_CMD_MSG(EMC_TASK_PLAN_REVERSE_TYPE,
+					   sizeof(EMC_TASK_PLAN_REVERSE)) {
+    };
+
+};
+
+class EMC_TASK_PLAN_FORWARD:public EMC_TASK_CMD_MSG {
+  public:
+    EMC_TASK_PLAN_FORWARD():EMC_TASK_CMD_MSG(EMC_TASK_PLAN_FORWARD_TYPE,
+					   sizeof(EMC_TASK_PLAN_FORWARD)) {
+    };
+
+};
+
+
 class EMC_TASK_PLAN_STEP:public EMC_TASK_CMD_MSG {
   public:
     EMC_TASK_PLAN_STEP():EMC_TASK_CMD_MSG(EMC_TASK_PLAN_STEP_TYPE,

src/emc/task/emctaskmain.cc

@@ -931,6 +931,8 @@ static int emcTaskPlan(void)
 	    case EMC_TASK_PLAN_OPEN_TYPE:
 	    case EMC_TASK_PLAN_CLOSE_TYPE:
 	    case EMC_TASK_PLAN_PAUSE_TYPE:
+		case EMC_TASK_PLAN_REVERSE_TYPE:
+		case EMC_TASK_PLAN_FORWARD_TYPE:
 	    case EMC_TASK_PLAN_RESUME_TYPE:
 	    case EMC_TASK_PLAN_INIT_TYPE:
 	    case EMC_TASK_PLAN_SYNCH_TYPE:
@@ -1012,7 +1014,7 @@ static int emcTaskPlan(void)
 		case EMC_TRAJ_SET_MAX_VELOCITY_TYPE:
 		case EMC_TRAJ_SET_SPINDLE_SCALE_TYPE:
 		case EMC_TRAJ_SET_FO_ENABLE_TYPE:
-	        case EMC_TRAJ_SET_FH_ENABLE_TYPE:
+        case EMC_TRAJ_SET_FH_ENABLE_TYPE:
 		case EMC_TRAJ_SET_SO_ENABLE_TYPE:
 		case EMC_SPINDLE_SPEED_TYPE:
 		case EMC_SPINDLE_ORIENT_TYPE:
@@ -1039,6 +1041,8 @@ static int emcTaskPlan(void)
 		case EMC_TASK_PLAN_RUN_TYPE:
 		case EMC_TASK_PLAN_EXECUTE_TYPE:
 		case EMC_TASK_PLAN_PAUSE_TYPE:
+		case EMC_TASK_PLAN_REVERSE_TYPE:
+		case EMC_TASK_PLAN_FORWARD_TYPE:
 		case EMC_TASK_PLAN_RESUME_TYPE:
 		case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
 		case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
@@ -1121,6 +1125,8 @@ static int emcTaskPlan(void)
 		case EMC_SPINDLE_DECREASE_TYPE:
 		case EMC_SPINDLE_CONSTANT_TYPE:
 		case EMC_TASK_PLAN_PAUSE_TYPE:
+		case EMC_TASK_PLAN_REVERSE_TYPE:
+		case EMC_TASK_PLAN_FORWARD_TYPE:
 		case EMC_TASK_PLAN_RESUME_TYPE:
 		case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
 		case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
@@ -1208,6 +1214,8 @@ static int emcTaskPlan(void)
 		case EMC_TASK_ABORT_TYPE:
 		case EMC_TASK_PLAN_EXECUTE_TYPE:
 		case EMC_TASK_PLAN_PAUSE_TYPE:
+		case EMC_TASK_PLAN_REVERSE_TYPE:
+		case EMC_TASK_PLAN_FORWARD_TYPE:
 		case EMC_TASK_PLAN_RESUME_TYPE:
 		case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
 		case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
@@ -1276,6 +1284,8 @@ static int emcTaskPlan(void)
 		case EMC_SPINDLE_CONSTANT_TYPE:
 		case EMC_TASK_PLAN_EXECUTE_TYPE:
 		case EMC_TASK_PLAN_PAUSE_TYPE:
+		case EMC_TASK_PLAN_REVERSE_TYPE:
+		case EMC_TASK_PLAN_FORWARD_TYPE:
 		case EMC_TASK_PLAN_RESUME_TYPE:
 		case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
 		case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
@@ -1370,6 +1380,8 @@ static int emcTaskPlan(void)
 	    case EMC_TASK_PLAN_OPEN_TYPE:
 	    case EMC_TASK_PLAN_CLOSE_TYPE:
 	    case EMC_TASK_PLAN_PAUSE_TYPE:
+		case EMC_TASK_PLAN_REVERSE_TYPE:
+		case EMC_TASK_PLAN_FORWARD_TYPE:
 	    case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
 	    case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
 	    case EMC_TASK_PLAN_RESUME_TYPE:
@@ -2290,6 +2302,16 @@ static int emcTaskIssueCommand(NMLmsg * cmd)
 	retval = 0;
 	break;
 
+    case EMC_TASK_PLAN_REVERSE_TYPE:
+	emcTrajReverse();
+	retval = 0;
+	break;
+
+    case EMC_TASK_PLAN_FORWARD_TYPE:
+	emcTrajForward();
+	retval = 0;
+	break;
+
     case EMC_TASK_PLAN_RESUME_TYPE:
 	emcTrajResume();
 	emcStatus->task.interpState =

src/emc/task/taskintf.cc

@@ -1305,6 +1305,20 @@ int emcTrajPause()
     return usrmotWriteEmcmotCommand(&emcmotCommand);
 }
 
+int emcTrajReverse()
+{
+    emcmotCommand.command = EMCMOT_REVERSE;
+
+    return usrmotWriteEmcmotCommand(&emcmotCommand);
+}
+
+int emcTrajForward()
+{
+    emcmotCommand.command = EMCMOT_FORWARD;
+
+    return usrmotWriteEmcmotCommand(&emcmotCommand);
+}
+
 int emcTrajStep()
 {
     emcmotCommand.command = EMCMOT_STEP;

src/emc/tp/blendmath.c

@@ -121,6 +121,21 @@ double saturate(double x, double max) {
     }
 }
 
+/**
+ * Saturate a value x to be within max and min.
+ */
+double bisaturate(double x, double max, double min) {
+    if ( x > max ) {
+        return max;
+    }
+    else if ( x < min ) {
+        return min;
+    }
+    else {
+        return x;
+    }
+}
+
 
 /**
  * Apply bounds to a value x.

src/emc/tp/blendmath.h

@@ -132,6 +132,8 @@ int clip_max(double * const x, double max);
 
 double saturate(double x, double max);
 
+double bisaturate(double x, double max, double min);
+
 int sat_inplace(double * const x, double max);
 
 int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end);

src/emc/tp/spherical_arc.c

@@ -23,12 +23,11 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
         PmCartesian const * const center)
 {
 #ifdef ARC_PEDANTIC
-    if (!P0 || !P1 || !center) {
+    if (!P0 || !P1 || !center)
         return TP_ERR_MISSING_INPUT;
 
-    if (!arc) {
+    if (!arc)
         return TP_ERR_MISSING_OUTPUT;
-    }
 #endif
 
     // Store the start, end, and center

src/emc/tp/tc.c

@@ -262,6 +262,27 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou
     return 0;
 }
 
+
+
+/**
+ * Calculate the distance left in the trajectory segment in the indicated
+ * direction.
+ */
+double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction)
+{
+    double distance = tcGetTarget(tc, direction) - tc->progress;
+    if (direction == TC_DIR_REVERSE) {
+        distance *=-1.0;
+    }
+    return distance;
+}
+
+double tcGetTarget(TC_STRUCT const * const tc, int direction)
+{
+    return (direction == TC_DIR_REVERSE) ? 0.0 : tc->target;
+}
+
+
 /*! tcGetPos() function
  *
  * \brief This function calculates the machine position along the motion's path.

src/emc/tp/tc.h

@@ -22,8 +22,8 @@
 #include "tc_types.h"
 #include "tp_types.h"
 
-double tcGetMaxTargetVel(TC_STRUCT const * const tc,
-        double max_scale);
+//double tcGetMaxTargetVel(TC_STRUCT const * const tc,
+//        double max_scale);
 int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out);
 int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out);
 int tcGetPos(TC_STRUCT const * const tc,  EmcPose * const out);
@@ -33,6 +33,9 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou
 int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
 int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
 
+double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction);
+double tcGetTarget(TC_STRUCT const * const tc, int direction);
+
 int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc,
         TC_STRUCT const * const tc, PmCartesian * const point);
 

src/emc/tp/tc_types.h

@@ -40,6 +40,11 @@ typedef enum {
     TC_SYNC_POSITION
 } tc_spindle_sync_t;
 
+typedef enum {
+    TC_DIR_FORWARD = 0,
+    TC_DIR_REVERSE
+} tc_direction_t;
+
 #define TC_GET_PROGRESS 0
 #define TC_GET_STARTPOINT 1
 #define TC_GET_ENDPOINT 2