Merge pull request #88 from epics-modules/fix-esp300

Fix problems with devESP300 and drvESP300
epics-modules · Nov 27, 2017 · 5245991 · 5245991
2 parents 220d18f + 91b2e33
commit 5245991
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 4 deletions.
diff --git a/motorApp/NewportSrc/devESP300.cc b/motorApp/NewportSrc/devESP300.cc
@@ -240,7 +240,7 @@ static RTN_STATUS ESP300_build_trans(motor_cmnd command, double *parms, struct m
 	    sprintf(buff, "%.2dVA%f;", axis, cntrl_units);
 	    break;
 	case SET_ACCEL:
-	    sprintf(buff, "%.2dAC%f;", axis, cntrl_units);
+	    sprintf(buff, "%.2dAC%f;%.2dAG%f;", axis, cntrl_units, axis, cntrl_units);
 	    break;
 	case GO:
 	    /* 

diff --git a/motorApp/NewportSrc/drvESP300.cc b/motorApp/NewportSrc/drvESP300.cc
@@ -674,18 +674,40 @@ static int motor_init()
             for (motor_index = 0; motor_index < total_axis; motor_index++)
             {
                 struct mess_info *motor_info = &brdptr->motor_info[motor_index];
+                int feedback;
+                double fullStep;
+                int microStep;
 
                 /* Get controller's EGU for the user (see README). */
                 sprintf(buff, "%.2dSN?", motor_index + 1);
                 send_mess(card_index, buff, 0);
                 recv_mess(card_index, buff, 1);
 
                 /* Set axis resolution. */
-                sprintf(buff, "%.2dSU?", motor_index + 1);
+                /* Read the feedback status */
+                sprintf(buff, "%.2dZB?", motor_index + 1);
                 send_mess(card_index, buff, 0);
                 recv_mess(card_index, buff, 1);
-                cntrl->drive_resolution[motor_index] = atof(&buff[0]);
-
+                feedback = strtol(buff,0,16);
+                /* If stepper closed loop positioning is enabled (bit 9=1) and encoder feedback is disabled (bit 8=0) 
+                 * then use the full-step resolution (FR) and microstepping (QS) to determine drive_resolution.
+                 * If not then use SU (encoder resolution) for drive_resolution. */
+                if ((feedback & 0x300) == 0x200) {
+                    sprintf(buff, "%.2dFR?", motor_index + 1);
+                    send_mess(card_index, buff, 0);
+                    recv_mess(card_index, buff, 1);
+                    fullStep = atof(buff);
+                    sprintf(buff, "%.2dQS?", motor_index + 1);
+                    send_mess(card_index, buff, 0);
+                    recv_mess(card_index, buff, 1);
+                    microStep = strtol(buff, 0, 10);
+                    cntrl->drive_resolution[motor_index] = fullStep / microStep;
+                } else {
+                    sprintf(buff, "%.2dSU?", motor_index + 1);
+                    send_mess(card_index, buff, 0);
+                    recv_mess(card_index, buff, 1);
+                    cntrl->drive_resolution[motor_index] = atof(&buff[0]);
+                }
                 motor_info->status.All = 0;
                 motor_info->no_motion_count = 0;
                 motor_info->encoder_position = 0;