servo360 - try adding delay in calculated distance during acceleration …

…#149 (step 1) Added as commented code (that does work). Needs either tuning or extra conditions so that it doesn't introduce errors when slowing down to a stop.
parallaxinc · Oct 16, 2017 · 68fea9e · 68fea9e
1 parent 721d9ca
commit 68fea9e
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 5 deletions.
diff --git a/Learn/Simple Libraries/Motor/libservo360/cmm/libservo360.a b/Learn/Simple Libraries/Motor/libservo360/cmm/libservo360.a
diff --git a/Learn/Simple Libraries/Motor/libservo360/servo360.c b/Learn/Simple Libraries/Motor/libservo360/servo360.c
@@ -487,11 +487,48 @@ int servo360_pidV(int p)
   int opMax = _fs[p].speedLimit;
 
   _fs[p].speedMeasured = (_fs[p].angle - _fs[p].angleP) * 50; 
+
   if(abs(_fs[p].angleError) < S360_UNITS_ENCODER/2)
   {
+
+
+
+    //
     _fs[p].angleDeltaCalc = _fs[p].speedTarget / S360_CS_HZ;
     _fs[p].angleCalc += _fs[p].angleDeltaCalc;
-  }    
+    //
+
+
+
+    /*
+    // This could remedy the overshoot problem, but it seems to reduce
+    // drive_goto accuracy.
+    if(_fs[p].speedTarget == _fs[p].speedTargetP)
+    {
+      _fs[p].offset--;
+      if(_fs[p].offset < 0) _fs[p].offset = 0;
+    }
+    else
+    {
+      _fs[p].offset++;
+      if(_fs[p].offset > FB360_OFFSET_MAX) _fs[p].offset = FB360_OFFSET_MAX;
+    }
+
+    int idx = _fb360c.pulseCount % FB360_V_ARRAY;
+    _fs[p].vT[idx] = _fs[p].speedTarget;
+
+    idx -= _fs[p].offset;
+    if(idx < 0) idx += FB360_V_ARRAY;
+    
+    _fs[p].speedTargetTemp = _fs[p].vT[_fs[p].offset];    
+
+    _fs[p].angleDeltaCalc = _fs[p].speedTargetTemp / S360_CS_HZ;
+    _fs[p].angleCalc += _fs[p].angleDeltaCalc;
+    */ 
+
+
+
+  }
 
   _fs[p].angleError = _fs[p].angleCalc - _fs[p].angle;
 

diff --git a/Learn/Simple Libraries/Motor/libservo360/servo360.h b/Learn/Simple Libraries/Motor/libservo360/servo360.h
@@ -289,7 +289,7 @@ typedef volatile struct servo360_s
   volatile int speedOut;
   volatile int pw;
 
-  // speed control system
+  // velocity control system
   volatile int opPidV;
   volatile int approachFlag;
   volatile int ticksDiff;
@@ -314,6 +314,18 @@ typedef volatile struct servo360_s
   volatile int stepDir;
   volatile int lag;
   volatile int accelerating;
+  volatile int speedTargetTemp;
+
+  /*
+  // This could remedy the overshoot problem, but it seems to reduce
+  // drive_goto accuracy.
+  
+  #define FB360_OFFSET_MAX 5
+  #define FB360_V_ARRAY 8
+
+  volatile int vT[FB360_V_ARRAY];
+  volatile int offset;
+  */
 
   // position control system
   volatile int Kp;

diff --git a/Learn/Simple Libraries/Motor/libservo360/z_dev_console.c b/Learn/Simple Libraries/Motor/libservo360/z_dev_console.c
@@ -255,8 +255,8 @@ void console()
                        fbt[p].pV, fbt[p].iV, fbt[p].dV,
                        fbt[p].drive, fbt[p].opV, fbt[p].opPidV); 
           */
-          dprint(term, "spR: %d, spT: %d, spM: %d\r",
-                       fbt[p].speedReq, fbt[p].speedTarget, fbt[p].speedMeasured);
+          dprint(term, "spR: %d, spT: %d, spTT: %d, spM: %d\r",
+                       fbt[p].speedReq, fbt[p].speedTarget, _fs[p].speedTargetTemp, fbt[p].speedMeasured);
           dprint(term, "aC: %d, aM: %d, aE: %d\r",
                        fbt[p].angleCalc, fbt[p].angle, fbt[p].angleError); 
           dprint(term, "pV: %d, iV: %d, dV: %d\r",

diff --git a/Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/cmm/libabdrive360.a b/Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/cmm/libabdrive360.a
diff --git a/version.txt b/version.txt
@@ -1 +1 @@
-v1.4.15
+v1.4.16