sg threshold calibration routine 2

diff --git a/src/main.c b/src/main.c
index c406b1d83b5418db4ad5485610a529f2fd2232b4..e62b4ab34bf9c7ca90335fba5a2358636e55d88f 100644 (file)
--- a/src/main.c
+++ b/src/main.c
@@ -144,7 +144,7 @@ static void _init() {
 
 
 int main() {
-  wdt_enable(WDTO_250MS);
+  //wdt_enable(WDTO_250MS);
 
   _init();
 

diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c
index 9d17e969ca8c32875f953a6d5976f508e27abdc4..729b967aae05d3e7ce08cf70bdb30fe950b46352 100644 (file)
--- a/src/plan/calibrate.c
+++ b/src/plan/calibrate.c
@@ -43,7 +43,7 @@
 
 
 #define CAL_THRESHOLDS 32
-#define CAL_VELOCITIES 16
+#define CAL_VELOCITIES 256
 #define CAL_MIN_THRESH -10
 #define CAL_MAX_THRESH 63
 #define CAL_WAIT_TIME 3 // ms
@@ -64,14 +64,12 @@ typedef struct {
   int state;
   int motor;
   int axis;
-  int tstep;
   int vstep;
 
   float current_velocity;
 
-  uint16_t stallguard[CAL_VELOCITIES][CAL_THRESHOLDS];
+  uint16_t stallguard[CAL_VELOCITIES];
   uint16_t velocities[CAL_VELOCITIES];
-  int8_t thresholds[CAL_THRESHOLDS];
 } calibrate_t;
 
 static calibrate_t cal = {};
@@ -96,47 +94,27 @@ static stat_t _exec_calibrate(mpBuf_t *bf) {
 
       memset(cal.stallguard, 0, sizeof(cal.stallguard));
 
-      tmc2660_set_stallguard_threshold(cal.motor, CAL_MAX_THRESH);
+      tmc2660_set_stallguard_threshold(cal.motor, 8);
       cal.wait = rtc_get_time() + CAL_WAIT_TIME;
 
-      printf("%d", cal.motor);
-      for (int i = 0; i < CAL_THRESHOLDS; i++)
-        printf(",%d", cal.thresholds[i]);
-      putchar('\n');
-
       break;
     }
 
     case CAL_ACCEL:
-      cal.current_velocity += maxDeltaV;
-
-      // Check if we are at the target velocity
-      if (cal.velocities[cal.vstep] <= cal.current_velocity) {
-        cal.current_velocity = cal.velocities[cal.vstep];
-        cal.tstep = 0;
+      if (cal.velocities[cal.vstep] == cal.current_velocity)
         cal.state = CAL_MEASURE;
 
-        tmc2660_set_stallguard_threshold(cal.motor, cal.thresholds[0]);
-        cal.wait = rtc_get_time() + CAL_WAIT_TIME;
+      else {
+        cal.current_velocity += maxDeltaV;
+
+        if (cal.velocities[cal.vstep] <= cal.current_velocity)
+          cal.current_velocity = cal.velocities[cal.vstep];
       }
       break;
 
     case CAL_MEASURE:
-      if (++cal.tstep == CAL_THRESHOLDS) {
-        if (++cal.vstep == CAL_VELOCITIES) cal.state = CAL_DECEL;
-        else {
-          cal.state = CAL_ACCEL;
-
-          printf("%d", cal.velocities[cal.vstep - 1]);
-          for (int i = 0; i < CAL_THRESHOLDS; i++)
-            printf(",%d", cal.stallguard[cal.vstep - 1][i]);
-          putchar('\n');
-        }
-
-      } else {
-        tmc2660_set_stallguard_threshold(cal.motor, cal.thresholds[cal.tstep]);
-        cal.wait = rtc_get_time() + CAL_WAIT_TIME;
-      }
+      if (++cal.vstep == CAL_VELOCITIES) cal.state = CAL_DECEL;
+      else cal.state = CAL_ACCEL;
       break;
 
     case CAL_DECEL:
@@ -145,11 +123,17 @@ static stat_t _exec_calibrate(mpBuf_t *bf) {
 
       if (!cal.current_velocity) {
         // Print results
+        putchar('\n');
+        for (int i = 0; i < CAL_VELOCITIES; i++)
+          printf("%d,", cal.velocities[i]);
+        putchar('\n');
+        for (int i = 0; i < CAL_VELOCITIES; i++)
+          printf("%d,", cal.stallguard[i]);
+        putchar('\n');
 
         mp_free_run_buffer(); // Release buffer
         cal.busy = false;
 
-        putchar('\n');
         return STAT_OK;
       }
       break;
@@ -177,9 +161,8 @@ bool calibrate_busy() {return cal.busy;}
 
 
 void calibrate_set_stallguard(int motor, uint16_t sg) {
-  if (cal.vstep < CAL_VELOCITIES && cal.tstep < CAL_THRESHOLDS &&
-      cal.motor == motor)
-    cal.stallguard[cal.vstep][cal.tstep] = sg;
+  if (cal.vstep < CAL_VELOCITIES && cal.motor == motor)
+    cal.stallguard[cal.vstep] = sg;
 }
 
 
@@ -196,10 +179,6 @@ uint8_t command_calibrate(int argc, char *argv[]) {
   memset(&cal, 0, sizeof(cal));
   cal.busy = true;
 
-  for (int i = 0; i < CAL_THRESHOLDS; i++)
-    cal.thresholds[i] = CAL_MIN_THRESH + i *
-      (CAL_MAX_THRESH - CAL_MIN_THRESH) / (CAL_THRESHOLDS - 1);
-
   bf->bf_func = _exec_calibrate; // register callback
   mp_commit_write_buffer(MOVE_TYPE_COMMAND);