diff --git a/tasdrive.c b/tasdrive.c
index d4d6b96b..2f64fffa 100644
--- a/tasdrive.c
+++ b/tasdrive.c
@@ -39,9 +39,9 @@
 static long TASSetValue(void *pData, SConnection * pCon, float value)
 {
   ptasMot self = (ptasMot) pData;
-  assert(self);
   int qcodes[] = {3,4,5,8}; /* qh,qk,ql, en */
   int i;
+  assert(self);
 
   if (self->code > 5 && self->math->tasMode == ELASTIC) {
     SCWrite(pCon, "ERROR: cannot drive this motor in elastic mode",
@@ -328,9 +328,9 @@ static int startMotors(ptasMot self, tasAngles angles,
     self->math->busy[A2] = 1;
   }
 
-  if (self->math->motors[MCV] != NULL) {
+  if (self->math->motors[MCV] != NULL && self->math->autofocus) {
     curve = maCalcVerticalCurvature(self->math->machine.monochromator,
-                                    angles.monochromator_two_theta);
+                                    angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
     status = startTASMotor(self->math->motors[MCV], pCon, self->math->motname[MCV],
                            curve, silent,stopFixed);
     if (status != OKOK) {
@@ -341,9 +341,9 @@ static int startMotors(ptasMot self, tasAngles angles,
     }
   }
 
-  if (self->math->motors[MCH] != NULL) {
+  if (self->math->motors[MCH] != NULL && self->math->autofocus) {
     curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
-                                      angles.monochromator_two_theta);
+                                      angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
     status = startTASMotor(self->math->motors[MCH], pCon, self->math->motname[MCH],
                            curve, silent,stopFixed);
      if (status != OKOK) {
@@ -374,9 +374,9 @@ static int startMotors(ptasMot self, tasAngles angles,
       self->math->busy[A6] = 1;
     }
 
-    if (self->math->motors[ACV] != NULL) {
+    if (self->math->motors[ACV] != NULL && self->math->autofocus) {
       curve = maCalcVerticalCurvature(self->math->machine.analyzer,
-                                      angles.analyzer_two_theta);
+                                      angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
       status = startTASMotor(self->math->motors[ACV], pCon, self->math->motname[ACV],
                              curve, silent,stopFixed);
       if (status != OKOK) {
@@ -386,9 +386,9 @@ static int startMotors(ptasMot self, tasAngles angles,
         self->math->busy[ACV] = 1;
       }
     }
-    if (self->math->motors[ACH] != NULL) {
+    if (self->math->motors[ACH] != NULL && self->math->autofocus) {
       curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
-                                        angles.analyzer_two_theta);
+                                        angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
       status = startTASMotor(self->math->motors[ACH], pCon, self->math->motname[ACH],
                              curve, silent,stopFixed);
       if (status != OKOK) {
@@ -666,9 +666,9 @@ static int startQMMotors(ptasMot self, tasAngles angles,
     self->math->busy[A2] = 1;
   }
 
-  if (self->math->motors[MCV] != NULL) {
+  if (self->math->motors[MCV] != NULL && self->math->autofocus) {
     curve = maCalcVerticalCurvature(self->math->machine.monochromator,
-                                    angles.monochromator_two_theta);
+                                    angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
     status = startTASMotor(self->math->motors[MCV], pCon, self->math->motname[MCV],
                            curve, silent,stopFixed);
     if (status != OKOK) {
@@ -678,9 +678,9 @@ static int startQMMotors(ptasMot self, tasAngles angles,
     }
   }
 
-  if (self->math->motors[MCH] != NULL) {
+  if (self->math->motors[MCH] != NULL && self->math->autofocus) {
     curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
-                                      angles.monochromator_two_theta);
+                                      angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
     status = startTASMotor(self->math->motors[MCH], pCon, self->math->motname[MCH],
                            curve, silent,stopFixed);
     if (status != OKOK) {
@@ -709,9 +709,9 @@ static int startQMMotors(ptasMot self, tasAngles angles,
     self->math->busy[A6] = 1;
   }
 
-  if (self->math->motors[ACV] != NULL) {
+  if (self->math->motors[ACV] != NULL && self->math->autofocus) {
     curve = maCalcVerticalCurvature(self->math->machine.analyzer,
-                                    angles.analyzer_two_theta);
+                                    angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
     status = startTASMotor(self->math->motors[ACV], pCon, self->math->motname[ACV],
                            curve, silent,stopFixed);
     if (status != OKOK) {
@@ -720,9 +720,9 @@ static int startQMMotors(ptasMot self, tasAngles angles,
       self->math->busy[ACV] = 1;
     }
   }
-  if (self->math->motors[ACH] != NULL) {
+  if (self->math->motors[ACH] != NULL && self->math->autofocus) {
     curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
-                                      angles.analyzer_two_theta);
+                                      angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
     status = startTASMotor(self->math->motors[ACH], pCon, self->math->motname[ACH],
                            curve, silent,stopFixed);
     if (status != OKOK) {
diff --git a/tasub.c b/tasub.c
index 32b4c7d1..45992104 100644
--- a/tasub.c
+++ b/tasub.c
@@ -173,6 +173,8 @@ static ptasUB MakeTasUB()
   pNew->actualEn = .0;
   pNew->outOfPlaneAllowed = 1;
   pNew->mustRecalculate = 1;
+  pNew->autofocus = 0;
+  pNew->focusfn = THETA_FN;
 
   return pNew;
 }
@@ -381,6 +383,10 @@ int TasUBFactory(SConnection * pCon, SicsInterp * pSics, void *pData,
             eError);
     return 0;
   }
+  if (pNew->motors[MCV] != NULL && pNew->motors[MCH] != NULL &&
+      pNew->motors[ACV] != NULL && pNew->motors[ACH] != NULL) {
+    pNew->autofocus = 1;
+  }
 
   status = AddCommand(pSics, argv[1], TasUBWrapper, KillTasUB, pNew);
   if (status != 1) {
@@ -453,6 +459,11 @@ static int setCrystalParameters(pmaCrystal crystal, SConnection * pCon,
     SCSendOK(pCon);
     SCparChange(pCon);
     return 1;
+  } else if (strcmp(argv[2], "hb3") == 0) {
+    crystal->HB3 = d;
+    SCSendOK(pCon);
+    SCparChange(pCon);
+    return 1;
   } else if (strcmp(argv[2], "vb1") == 0) {
     crystal->VB1 = d;
     SCSendOK(pCon);
@@ -493,6 +504,11 @@ static int getCrystalParameters(pmaCrystal crystal, SConnection * pCon,
              crystal->HB2);
     SCWrite(pCon, pBueffel, eValue);
     return 1;
+  } else if (strcmp(argv[2], "hb3") == 0) {
+    snprintf(pBueffel, 131, "%s.%s.hb3 = %f", argv[0], argv[1],
+             crystal->HB3);
+    SCWrite(pCon, pBueffel, eValue);
+    return 1;
   } else if (strcmp(argv[2], "vb1") == 0) {
     snprintf(pBueffel, 131, "%s.%s.vb1 = %f", argv[0], argv[1],
              crystal->VB1);
@@ -2200,9 +2216,44 @@ int TasUBWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
       SCWrite(pCon, pBueffel, eValue);
       return 1;
     }
- } else {
+  } else if (strcmp(argv[1], "autofocus") == 0) {
+    if (argc > 2) {
+      status = Tcl_GetInt(InterpGetTcl(pSics), argv[2], &(self->autofocus));
+      if (status != TCL_OK) {
+        SCWrite(pCon, "ERROR: failed to convert argument to number",
+            eError);
+        return 0;
+      }
+    } else {
+      snprintf(pBueffel, 131, "%s.autofocus = %d", argv[0], self->autofocus);
+      SCWrite(pCon, pBueffel, eValue);
+      return 1;
+    }
+  } else if (strcmp(argv[1], "focusfn") == 0) {
+    if (argc > 2) {
+      strtolower(argv[2]);
+      if (strcmp(argv[2], "theta") == 0) {
+        self->focusfn = THETA_FN;
+      } else if (strcmp(argv[2], "energy") == 0) {
+        self->focusfn = ENERGY_FN;
+      } else {
+        SCWrite(pCon, "ERROR: Arguments should be theta or energy", eError);
+      }
+    } else {
+      switch (self->focusfn) {
+        case ENERGY_FN:
+          snprintf(pBueffel, 131, "%s.focusfn = %s", argv[0], "energy");
+          break;
+        case THETA_FN:
+          snprintf(pBueffel, 131, "%s.focusfn = %s", argv[0], "theta");
+          break;
+      }
+      SCWrite(pCon, pBueffel, eValue);
+      return 1;
+    }
+  } else {
     snprintf(pBueffel, 131, "ERROR: subcommand %s to %s not defined",
-             argv[1], argv[0]);
+        argv[1], argv[0]);
     SCWrite(pCon, pBueffel, eError);
     return 0;
   }
diff --git a/tasub.h b/tasub.h
index 0d4ba3e5..bd226423 100644
--- a/tasub.h
+++ b/tasub.h
@@ -24,6 +24,8 @@
                 tasQEPosition current;
                 int tasMode;
                 int outOfPlaneAllowed;
+                int autofocus;
+                int focusfn;
                 double targetEn, actualEn;
                 int mustRecalculate;
                 int mustDrive; 
@@ -34,7 +36,7 @@
                 tasReflection r1, r2;
                 int ubValid;
                 int silent;
-		int stopFixed; /* flag to stop multiple fixed messages in scans*/
+                int stopFixed; /* flag to stop multiple fixed messages in scans*/
                 char *updater;
 }tasUB, *ptasUB;
 
diff --git a/tasublib.c b/tasublib.c
index 4b38bfee..04dec63e 100644
--- a/tasublib.c
+++ b/tasublib.c
@@ -40,13 +40,21 @@ double KtoEnergy(double k)
 }
 
 /*-------------------------------------------------------------------*/
-static double calcCurvature(double B1, double B2, double theta, int ori)
+static double calcCurvature(double B1, double B2, double B3, double theta, double energy, int ori, int focusfn)
 {
   assert(ori == VERT || ori == HOR);
-  if (ori == VERT) {
-    return B1 + B2 / Sind(ABS(theta));
+  if (focusfn == ENERGY_FN) {
+    if (ori == VERT) {
+      return B1 + B2 * energy;
+    } else {
+      return B1 + B2 * pow(B3, energy);
+    }
   } else {
-    return B1 + B2 * Sind(ABS(theta));
+    if (ori == VERT) {
+      return B1 + B2 / Sind(ABS(theta));
+    } else {
+      return B1 + B2 * Sind(ABS(theta));
+    }
   }
 }
 
@@ -66,15 +74,15 @@ int maCalcTwoTheta(maCrystal data, double k, double *two_theta)
 }
 
 /*--------------------------------------------------------------------*/
-double maCalcVerticalCurvature(maCrystal data, double two_theta)
+double maCalcVerticalCurvature(maCrystal data, double two_theta, double energy, int focusfn)
 {
-  return calcCurvature(data.VB1, data.VB2, two_theta / 2., VERT);
+  return calcCurvature(data.VB1, data.VB2, 1, two_theta / 2., energy, VERT, focusfn);
 }
 
 /*-------------------------------------------------------------------*/
-double maCalcHorizontalCurvature(maCrystal data, double two_theta)
+double maCalcHorizontalCurvature(maCrystal data, double two_theta, double energy, int focusfn)
 {
-  return calcCurvature(data.HB1, data.HB2, two_theta / 2., HOR);
+  return calcCurvature(data.HB1, data.HB2, data.HB3, two_theta / 2., energy, HOR, focusfn);
 }
 
 /*--------------------------------------------------------------------*/
diff --git a/tasublib.h b/tasublib.h
index 15ef992f..53a1b0d8 100644
--- a/tasublib.h
+++ b/tasublib.h
@@ -22,6 +22,9 @@
 #define KICONST 1
 #define KFCONST 2
 #define ELASTIC 3
+/*========================== defines for focusfn ====================*/
+#define ENERGY_FN 1
+#define THETA_FN  2
 /*
  * in elastic mode A5, A5 will be disregarded and ki = kf at all times
  */
@@ -42,8 +45,8 @@
 typedef struct {
   double dd;                    /* lattice spacing */
   int ss;                       /* scattering sense */
-  double HB1, HB2;              /* horizontal curvature parameters */
-  double VB1, VB2;              /* vertical curvature parameters   */
+  double HB1, HB2, HB3;         /* horizontal curvature parameters */
+  double VB1, VB2;         /* vertical curvature parameters   */
 } maCrystal, *pmaCrystal;
 /**
  * the machine parameters of a triple axis spectrometer
@@ -109,14 +112,14 @@ int maCalcTwoTheta(maCrystal data, double k, double *two_theta);
  * @param two_theta The tow theta value for which to calculate the curvature.
  * @return A new value for the curvature.
  */
-double maCalcVerticalCurvature(maCrystal data, double two_theta);
+double maCalcVerticalCurvature(maCrystal data, double two_theta, double energy, int focusfn);
 /**
  * calculate the value for the horizontal curvature
  * @param data The input crystal parameters
  * @param two_theta The tow theta value for which to calculate the curvature.
  * @return A new value for the curvature.
  */
-double maCalcHorizontalCurvature(maCrystal data, double two_theta);
+double maCalcHorizontalCurvature(maCrystal data, double two_theta, double energy, int focusfn);
 /**
  * calculate the value of the K vector from the angle
  * @param data The crystals constants