From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Mon, 17 Jul 2017 01:33:06 +0000 (-0700)
Subject: Much improved jogging
X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=9b69c76f8881180b8db0b7a31e8b5960bdfbe797;p=bbctrl-firmware

Much improved jogging
---

diff --git a/avr/src/plan/jog.c b/avr/src/plan/jog.c
index 4dd8be4..631047a 100644
--- a/avr/src/plan/jog.c
+++ b/avr/src/plan/jog.c
@@ -51,6 +51,7 @@ typedef struct {
 
   int sign;
   float velocity;
+  float accel;
   float next;
   float initial;
   float target;
@@ -104,30 +105,26 @@ static float _compute_axis_velocity(int axis) {
 
   if (fp_EQ(V, Vt)) return Vt;
 
-  if (a->changed) {
-    // Compute axis max jerk
-    float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
+  // Compute axis max jerk
+  float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
 
-    // Compute length to velocity given max jerk
-    float length = mp_get_target_length(V, Vt, jerk * JOG_JERK_MULT);
+  // Compute target accel
+  float accel = sqrt(jerk * fabs(Vt - V)) * (Vt < V ? -1 : 1);
 
-    // Compute move time
-    float move_time = 2 * length / (V + Vt);
+  // Compute max delta accel
+  float deltaAccel = jerk * SEGMENT_TIME;
 
-    if (move_time <= SEGMENT_TIME) return Vt;
+  // Update accel
+  if (a->accel < accel) {
+    if (accel < a->accel + deltaAccel) a->accel = accel;
+    else a->accel += deltaAccel;
 
-    a->initial = V;
-    a->t = a->delta = SEGMENT_TIME / move_time;
+  } else if (accel < a->accel) {
+    if (a->accel - deltaAccel < accel) a->accel = accel;
+    else a->accel -= deltaAccel;
   }
 
-  if (a->t <= 0) return V;
-  if (1 <= a->t) return Vt;
-
-  // Compute quintic Bezier curve
-  V = velocity_curve(a->initial, Vt, a->t);
-  a->t += a->delta;
-
-  return V;
+  return V + a->accel * SEGMENT_TIME;
 }
 
 
@@ -144,7 +141,7 @@ static float _axis_velocity_at_limits(int axis) {
 
     float position = mp_runtime_get_axis_position(axis);
     float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
-    float decelDist = mp_get_target_length(V, 0, jerk * JOG_JERK_MULT);
+    float decelDist = mp_get_target_length(V, 0, jerk);
 
     if (position < min + decelDist) {
     }
diff --git a/avr/src/plan/velocity_curve.c b/avr/src/plan/velocity_curve.c
index 75c17fb..7ef1009 100644
--- a/avr/src/plan/velocity_curve.c
+++ b/avr/src/plan/velocity_curve.c
@@ -64,6 +64,8 @@
 /// takes about 60uS or about 1,920 clocks.  The code was compiled with avr-gcc
 /// v4.9.2 with -O3.
 float velocity_curve(float Vi, float Vt, float t) {
+  // If the change is small enough just do a linear velocity transition.
+  // TODO revisit this.
   if (fabs(Vt - Vi) < 200) return Vi + (Vt - Vi) * t;
 
   const float t2 = t * t;