servo360 - try Increasing start pulse duration #147

Defaults for y = mx + b calculations to determine transfer fuction from
a requested speed that has converted to 4069ths of a rotation per second:

  #define S360_VM 180
  #define S360_VM_CCW S360_VM
  #define S360_VM_CW S360_VM
  #define S360_VB_CCW 200
  #define S360_VB_CW -200

Function for adjusting start pulse duration:

int servo360_setTransferFunction(int pin, int constant, int value);

Constants for adjusting the CCW and CW m and b in the transfer function's y = mx + b:

#define S360_SETTING_VB_CCW 9
#define S360_SETTING_VB_CW 10
#define S360_SETTING_VM_CCW 11
#define S360_SETTING_VM_CW 12

360 Dev/ActivityBot360/ActivityBot360 Modified/drive_goto variations (2).c

@@ -0,0 +1,47 @@
+/* 
+  drive_goto variations.c
+
+  This code demonstrates the problem in ActivityBots with servos that both have 
+  the similar zero speed deadband offsets near the allowable limits. Example 
+  instead of 1480 to 1520, both servos hare 1490 to 1530 us zero speed deadbands.
+
+  The monitor (with SimpleIDE Terminal) functions have no effect unless you build 
+  the libservo360 project with this un-commented:
+    #define _servo360_monitor_
+  Make sure to re-comment it when done because it takes a signfiicant program memory.
+  
+  For best results, use the Parallax 
+*/
+
+#include "simpletools.h"
+#include "abdrive360.h"
+
+int main()                    
+{
+  drive_speed(0, 0);
+  pause(1000);
+  servo360_monitorRun();
+  pause(1000);
+
+  drive_goto(192, 192);
+  pause(1000);
+  drive_goto(-28, 28);
+  pause(1000);
+  drive_goto(28 * 2, -28 * 2);
+  pause(1000);
+  drive_goto(-28, 28);
+  pause(1000);
+  drive_goto(-192, -192);
+  pause(1000);
+
+  drive_goto(128, 192);
+  pause(1000);
+  drive_goto(-128, -192);
+  pause(1000);
+  drive_goto(192, 128);
+  pause(1000);
+  drive_goto(-192, -128);
+  pause(1000);
+
+  servo360_monitorEnd();
+}

360 Dev/ActivityBot360/ActivityBot360 Modified/drive_goto variations (2).side

@@ -0,0 +1,9 @@
+drive_goto variations (2).c
+>compiler=C
+>memtype=cmm main ram compact
+>optimize=-Os
+>-m32bit-doubles
+>-fno-exceptions
+>defs::-std=c99
+>-lm
+>BOARD::ACTIVITYBOARD

360 Dev/ActivityBot360/ActivityBot360 Modified/drive_goto variations (3).c

@@ -0,0 +1,52 @@
+/* 
+  drive_goto variations.c
+
+  This code demonstrates the problem in ActivityBots with servos that both have 
+  the similar zero speed deadband offsets near the allowable limits. Example 
+  instead of 1480 to 1520, both servos hare 1490 to 1530 us zero speed deadbands.
+
+  The monitor (with SimpleIDE Terminal) functions have no effect unless you build 
+  the libservo360 project with this un-commented:
+
+    #define _servo360_monitor_
+
+  For best results, use the Parallax WX ESP8266 WiFi Module - DIP for programming
+  and terminal display.  
+  https://www.parallax.com/product/32420d
+
+  Make sure to re-comment it when done because it takes a signfiicant program memory.
+  
+*/
+
+#include "simpletools.h"
+#include "abdrive360.h"
+
+int main()                    
+{
+  drive_speed(0, 0);
+  pause(1000);
+  servo360_monitorRun();
+  pause(1000);
+
+  drive_goto(192, 192);
+  pause(1000);
+  drive_goto(-28, 28);
+  pause(1000);
+  drive_goto(28 * 2, -28 * 2);
+  pause(1000);
+  drive_goto(-28, 28);
+  pause(1000);
+  drive_goto(-192, -192);
+  pause(1000);
+
+  drive_goto(128, 192);
+  pause(1000);
+  drive_goto(-128, -192);
+  pause(1000);
+  drive_goto(192, 128);
+  pause(1000);
+  drive_goto(-192, -128);
+  pause(1000);
+
+  servo360_monitorEnd();
+}

360 Dev/ActivityBot360/ActivityBot360 Modified/drive_goto variations (3).side

@@ -0,0 +1,9 @@
+drive_goto variations (3).c
+>compiler=C
+>memtype=cmm main ram compact
+>optimize=-Os
+>-m32bit-doubles
+>-fno-exceptions
+>defs::-std=c99
+>-lm
+>BOARD::ACTIVITYBOARD

360 Dev/ActivityBot360/ActivityBot360 Modified/drive_goto variations (4).c

@@ -0,0 +1,59 @@
+/* 
+  drive_goto variations.c
+
+  This code demonstrates the problem in ActivityBots with servos that both have 
+  the similar zero speed deadband offsets near the allowable limits. Example 
+  instead of 1480 to 1520, both servos hare 1490 to 1530 us zero speed deadbands.
+
+  The monitor (with SimpleIDE Terminal) functions have no effect unless you build 
+  the libservo360 project with this un-commented:
+
+    #define _servo360_monitor_
+
+  For best results, use the Parallax WX ESP8266 WiFi Module - DIP for programming
+  and terminal display.  
+  https://www.parallax.com/product/32420d
+
+  Make sure to re-comment it when done because it takes a signfiicant program memory.
+  
+*/
+
+#include "simpletools.h"
+#include "abdrive360.h"
+
+int main()                    
+{
+  drive_speed(0, 0);
+  pause(1000);
+  servo360_monitorRun();
+  pause(1000);
+
+
+  servo360_setTransferFunction(12, S360_SETTING_VB_CCW, 250);
+  servo360_setTransferFunction(12, S360_SETTING_VB_CW, -75);
+  servo360_setTransferFunction(13, S360_SETTING_VB_CCW, 250);
+  servo360_setTransferFunction(13, S360_SETTING_VB_CW, -75);
+
+
+  drive_goto(192, 192);
+  pause(1000);
+  drive_goto(-28, 28);
+  pause(1000);
+  drive_goto(28 * 2, -28 * 2);
+  pause(1000);
+  drive_goto(-28, 28);
+  pause(1000);
+  drive_goto(-192, -192);
+  pause(1000);
+
+  drive_goto(128, 192);
+  pause(1000);
+  drive_goto(-128, -192);
+  pause(1000);
+  drive_goto(192, 128);
+  pause(1000);
+  drive_goto(-192, -128);
+  pause(1000);
+
+  servo360_monitorEnd();
+}

360 Dev/ActivityBot360/ActivityBot360 Modified/drive_goto variations (4).side

@@ -0,0 +1,9 @@
+drive_goto variations (4).c
+>compiler=C
+>memtype=cmm main ram compact
+>optimize=-Os
+>-m32bit-doubles
+>-fno-exceptions
+>defs::-std=c99
+>-lm
+>BOARD::ACTIVITYBOARD

360 Dev/Servo360/Servo360 Modified/Juke bot servo tests.txt

@@ -0,0 +1,86 @@
+These servos are close to the edge, and their
+errors compound each other on accelerate forward 
+and backward maneuvers.
+
+
+
+Press yellow button to start....
+
+Testing...
+Performing servo check...
+volume = 0.050842
+
+
+Pulse Velocity  Current     vMax       vMin       iMax       iMin  
+(us)   (RPS)      (A)       (RPS)      (RPS)       (A)        (A)  
+----- --------- --------   --------    ------   --------     ------
+1720, 2.281250, 0.125195,  2.950000,  1.950000,  0.240000,  0.045000
+1670, 1.781250, 0.102691,  2.337500,  1.337500,  0.180000,  0.036000
+1620, 1.187500, 0.068176,  1.725000,  0.725000,  0.130000,  0.026000
+1580, 0.687500, 0.046763,  1.235000,  0.235000,  0.100000,  0.020000
+1540, 0.156250, 0.028839,  0.745000,  0.050000,  0.800000,  0.003000
+1530, 0.000000, 0.015979,  0.622500,  0.000000,  0.070000,  0.003000
+1525, 0.000000, 0.012606,  0.561300,  0.000000,  0.060000,  0.003000
+1520, 0.000000, 0.009798,  0.450000,  0.000000,  0.050000,  0.003000
+1515, 0.000000, 0.016311,  0.350000,  0.000000,  0.040000,  0.003000
+1510, 0.000000, 0.011559,  0.000000,  0.000000,  0.030000,  0.003000
+1500, 0.000000, 0.014011,  0.000000,  0.000000,  0.030000,  0.003000
+1490, 0.000000, 0.028269,  0.000000,  0.000000,  0.030000,  0.003000
+1485, -0.156250, 0.030680,  0.000000,  -0.350000,  0.040000,  0.003000
+1480, -0.218750, 0.026282,  0.000000,  -0.450000,  0.050000,  0.003000
+1475, -0.281250, 0.032083,  0.000000,  -0.561300,  0.600000,  0.003000
+1470, -0.343750, 0.032547,  0.000000,  -0.622500,  0.070000,  0.003000
+1460, -0.500000, 0.043424,  -0.100000,  -0.745000,  0.080000,  0.003000
+1420, -0.937500, 0.052595,  -0.235000,  -1.235000,  0.100000,  0.020000
+1380, -1.437500, 0.076962,  -0.725000,  -1.725000,  0.130000,  0.026000
+1330, -2.000000, 0.113774,  -1.337500,  -2.338800,  0.180000,  0.036000
+1280, -2.406250, 0.136543,  -1.950000,  -2.950000,  0.240000,  0.045000
+
+
+=== PASS ===
+
+
+
+remove servo
+or yellow button re-test
+
+
+Press yellow button to start....
+
+Testing...
+Performing servo check...
+volume = 0.094299
+
+
+Pulse Velocity  Current     vMax       vMin       iMax       iMin  
+(us)   (RPS)      (A)       (RPS)      (RPS)       (A)        (A)  
+----- --------- --------   --------    ------   --------     ------
+1720, 2.375000, 0.120801,  2.950000,  1.950000,  0.240000,  0.045000
+1670, 1.812500, 0.095406,  2.337500,  1.337500,  0.180000,  0.036000
+1620, 1.218750, 0.066129,  1.725000,  0.725000,  0.130000,  0.026000
+1580, 0.718750, 0.046111,  1.235000,  0.235000,  0.100000,  0.020000
+1540, 0.218750, 0.036697,  0.745000,  0.050000,  0.800000,  0.003000
+1530, 0.000000, 0.014362,  0.622500,  0.000000,  0.070000,  0.003000
+1525, 0.000000, 0.013258,  0.561300,  0.000000,  0.060000,  0.003000
+1520, 0.000000, 0.011315,  0.450000,  0.000000,  0.050000,  0.003000
+1515, 0.000000, 0.007546,  0.350000,  0.000000,  0.040000,  0.003000
+1510, 0.000000, 0.012029,  0.000000,  0.000000,  0.030000,  0.003000
+1500, 0.000000, 0.012583,  0.000000,  0.000000,  0.030000,  0.003000
+1490, 0.000000, 0.012139,  0.000000,  0.000000,  0.030000,  0.003000
+1485, -0.156250, 0.022651,  0.000000,  -0.350000,  0.040000,  0.003000
+1480, -0.187500, 0.032769,  0.000000,  -0.450000,  0.050000,  0.003000
+1475, -0.250000, 0.031430,  0.000000,  -0.561300,  0.600000,  0.003000
+1470, -0.312500, 0.028979,  0.000000,  -0.622500,  0.070000,  0.003000
+1460, -0.500000, 0.032689,  -0.100000,  -0.745000,  0.080000,  0.003000
+1420, -0.937500, 0.050564,  -0.235000,  -1.235000,  0.100000,  0.020000
+1380, -1.375000, 0.081877,  -0.725000,  -1.725000,  0.130000,  0.026000
+1330, -2.000000, 0.105574,  -1.337500,  -2.338800,  0.180000,  0.036000
+1280, -2.406250, 0.119090,  -1.950000,  -2.950000,  0.240000,  0.045000
+
+
+=== PASS ===
+
+
+
+remove servo
+or yellow button re-test

Learn/Simple Libraries/Motor/libservo360/libservo360.side

@@ -43,6 +43,7 @@ servo360_couple.c
 servo360_setCoupleScale.c
 servo360_setAngleOffset.c
 servo360_getAngleOffset.c
+servo360_setTransferFunction.c
 >compiler=C
 >memtype=cmm main ram compact
 >optimize=-Os

Learn/Simple Libraries/Motor/libservo360/servo360.c

@@ -258,7 +258,7 @@ void servo360_outputSelector(int p)
   if(_fs[p].csop == S360_POSITION)
   {
     int output = servo360_pidA(p);
-    _fs[p].pw = servo360_upsToPulseFromTransferFunction(output);
+    _fs[p].pw = servo360_upsToPulseFromTransferFunction(output, p);
     _fs[p].speedOut = _fs[p].pw - 15000;
   }
   else if(_fs[p].csop == S360_SPEED)
@@ -552,17 +552,20 @@ int servo360_upsToPulseFromTransferFunction(int unitsPerSec)
 }
 */
 
-int servo360_upsToPulseFromTransferFunction(int unitsPerSec)
+int servo360_upsToPulseFromTransferFunction(int unitsPerSec, int p)
 {
-  int y, x = unitsPerSec, m = S360_VM, b;
+  int y, m, x, b;
+  x = unitsPerSec; 
 
   if(x > 0)
   {
-    b = S360_VB_POS;
+    m = _fs[p].vmCcw;
+    b = _fs[p].vbCcw;
   }    
   else if(x < 0)
   {
-    b = S360_VB_NEG;
+    m = _fs[p].vmCw;
+    b = _fs[p].vbCw;
   }
   else
   {
@@ -619,7 +622,7 @@ void servo360_speedControl(int p)
         //speedUpdateFlag = 0;
       }
     }      
-    _fs[p].pw = servo360_upsToPulseFromTransferFunction(_fs[p].speedTarget);
+    _fs[p].pw = servo360_upsToPulseFromTransferFunction(_fs[p].speedTarget, p);
     _fs[p].drive = _fs[p].pw - 15000;
     _fs[p].opPidV = _fs[p].drive + servo360_pidV(p);
   }

Learn/Simple Libraries/Motor/libservo360/servo360.h

@@ -35,9 +35,16 @@ extern "C" {
 #define S360_UNITS_FULL_CIRCLE 360
 #define S360_MAX_SPEED 2 * 4096 
 
-#define S360_VB_POS 200
-#define S360_VB_NEG -200
 #define S360_VM 180
+//#define S360_VB_POS 200
+//#define S360_VB_NEG -200
+#define S360_VM_CCW S360_VM
+#define S360_VM_CW S360_VM
+
+#define S360_VM_CCW S360_VM
+#define S360_VM_CW S360_VM
+#define S360_VB_CCW 200
+#define S360_VB_CW -200
 
 #define S360_DUTY_CYCLE_MIN 290
 #define S360_DUTY_CYCLE_MAX 9710
@@ -101,6 +108,10 @@ extern "C" {
 #define S360_SETTING_KIA 6
 #define S360_SETTING_KDA 7
 #define S360_SETTING_IA_MAX 8
+#define S360_SETTING_VB_CCW 9
+#define S360_SETTING_VB_CW 10
+#define S360_SETTING_VM_CCW 11    
+#define S360_SETTING_VM_CW 12
 
 //#define S360_A_MAX (((1 << 31) - 1)) / S360_UNITS_FULL_CIRCLE  // 524287 degrees
 #define S360_A_MAX 524287  
@@ -186,7 +197,7 @@ void servo360_setPositiveDirection(int p, int direction);
 
 int servo360_setRampStep(int p, int stepSize);
 
-int servo360_upsToPulseFromTransferFunction(int unitsPerSec);
+int servo360_upsToPulseFromTransferFunction(int unitsPerSec, int p);
 void servo360_pulseControl(int p, int speedUps);
 
 void servo360_speedControl(int p);
@@ -204,6 +215,9 @@ int servo360_setMaxSpeedEncoded(int pin, int speed);
 void servo360_monitorRun(void);
 void servo360_monitorEnd(void);
 
+int servo360_setTransferFunction(int pin, int constant, int value);
+
+
 /*
 __attribute__((constructor))
 void servo360_patch(void);
@@ -253,6 +267,11 @@ typedef volatile struct servo360_s
   volatile int coupleScale;
   volatile int enable;
 
+  volatile int vmCcw;
+  volatile int vmCw;
+  volatile int vbCcw;    
+  volatile int vbCw;
+
   // admin
   volatile int csop;
   volatile int speedReq;