From dbdd1814dfe8f325edb427e3faf2c4e39053bf49 Mon Sep 17 00:00:00 2001
From: kpetersn <kmpeters@anl.gov>
Date: Tue, 27 Mar 2018 16:09:41 -0500
Subject: [PATCH] Enforce controller speed limits. Added an alternate jog
 implemention that doesn't work well (the ANF2 jog commands appear to require
 immediate stops, which don't play nicely with subsequent move commands).

---
 motorApp/AMCISrc/ANF2Driver.cpp | 70 ++++++++++++++++++++++++++++-----
 1 file changed, 61 insertions(+), 9 deletions(-)

diff --git a/motorApp/AMCISrc/ANF2Driver.cpp b/motorApp/AMCISrc/ANF2Driver.cpp
index e0c25c64..7e61bf10 100644
--- a/motorApp/AMCISrc/ANF2Driver.cpp
+++ b/motorApp/AMCISrc/ANF2Driver.cpp
@@ -562,18 +562,26 @@ asynStatus ANF2Axis::sendAccelAndVelocity(double acceleration, double velocity)
 {
   // static const char *functionName = "ANF2::sendAccelAndVelocity";
 
+  // ANF2 speed range is 1 to 1,000,000 steps/sec
+  if (velocity > 1000000.0) {
+    velocity = 1000000.0;
+  }
+  if (velocity < 1.0) {
+    velocity = 1.0;
+  }
+
   // Set the velocity register
   motionReg_[2] = NINT(velocity);
 
   // ANF2 acceleration range 1 to 2000 steps/ms/sec
   // Therefore need to limit range received by motor record from 1000 to 2e6 steps/sec/sec
-  if (acceleration < 1000) {
+  if (acceleration < 1000.0) {
     //printf("Acceleration is < 1000: %lf\n", acceleration);
-    acceleration = 1000;
+    acceleration = 1000.0;
   }
-  if (acceleration > 2000000) {
+  if (acceleration > 2000000.0) {
     //printf("Acceleration is > 2000: %lf\n", acceleration);
-    acceleration = 2000000;
+    acceleration = 2000000.0;
   }
   
   // Set the accel/decel register
@@ -706,20 +714,59 @@ asynStatus ANF2Axis::moveVelocity(double minVelocity, double maxVelocity, double
     "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
     functionName, minVelocity, maxVelocity, acceleration);
 
+  /*
+  // The jog command doesn't work well.  It seems to require stopping with an immediate stop,
+  // but immediate stops cause problems for normal moves.  
+  
+  // Clear the command/configuration register
+  status = pC_->writeReg32Array(axisNo_, zeroReg_, 5, DEFAULT_CONTROLLER_TIMEOUT);
+
+  epicsThreadSleep(0.05);
+
+  // Clear the motition registers
+  zeroRegisters(motionReg_);
+
+  // Note: the jog acceleration doesn't need to be corrected; the JAR field has units of egu/s/s
+
+  if (maxVelocity > 0.0) {
+    //printf(" ** positive jog called\n");
+
+    // Set cmd register
+    motionReg_[0] = 0x80 << 16;
+    
+    // Do nothing to the velocity
+  
+  } else {
+    //printf(" ** negative jog called\n");
+
+    // Set cmd register
+    motionReg_[0] = 0x100 << 16;
+
+    // ANF2 only accepts speeds > 0
+    maxVelocity = fabs(maxVelocity);
+  }
+
+  // This sets indices 2 & 3 of motionReg_
+  status = sendAccelAndVelocity(acceleration, maxVelocity);
+  
+  // Write all the registers atomically
+  status = pC_->writeReg32Array(axisNo_, motionReg_, 5, DEFAULT_CONTROLLER_TIMEOUT);
+  */
+
   velo = NINT(fabs(maxVelocity));
 
-  /* ANF2 does have a jog command, but don't use it yet */
-  /* ANG1 does not have jog command. Move 1 million steps */
+  // Simulate a jog like the ANG1 driver does. Move 1 million steps
   distance = 1000000;
   if (maxVelocity > 0.) {
-    /* This is a positive move in ANF2 coordinates */
+    // This is a positive move in ANF2 coordinates
     //printf(" ** relative move (JOG pos) called\n");
     status = move(distance, 0, minVelocity, velo, acceleration);
   } else {
-    /* This is a negative move in ANF2 coordinates */
+    // This is a negative move in ANF2 coordinates
     //printf(" ** relative move (JOG neg) called\n");
     status = move((distance * -1.0), 0, minVelocity, velo, acceleration);
   }
+  
   // Delay the first status read, give the controller some time to return moving status
   epicsThreadSleep(0.05);
   return status;
@@ -740,12 +787,17 @@ asynStatus ANF2Axis::stop(double acceleration)
   // Clear the command/configuration register
   status = pC_->writeReg32Array(axisNo_, zeroReg_, 1, DEFAULT_CONTROLLER_TIMEOUT);
 
-  //stopReg = 0x10 << 16;      Immediate stop
+  // Immediate stop works well with jogs, but breaks the first normal move
+  //stopReg = 0x10 << 16;     // Immediate stop
+  // Hold move works very well with normal moves
   stopReg = 0x4 << 16;      // Hold move
   
   //
   status = pC_->writeReg32Array(axisNo_, &stopReg, 1, DEFAULT_CONTROLLER_TIMEOUT);
 
+  // Clear the command/configuration register
+  //status = pC_->writeReg32Array(axisNo_, zeroReg_, 5, DEFAULT_CONTROLLER_TIMEOUT);
+  
   return status;
 }