#136 servo360 - Add coupling feature for a given pair of servos

Added servo360_couple function.

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

@@ -39,6 +39,7 @@ servo360_unitsEncoderToAngle.c
 servo360_setAngleCtrlSpeedMax.c
 servo360_getAngleCtrlSpeedMax.c
 z_dev_console.c
+servo360_couple.c
 >compiler=C
 >memtype=cmm main ram compact
 >optimize=-Os

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

@@ -100,58 +100,47 @@ void servo360_mainLoop()
       }        
     }
 
-
-
-
-
-
-
-
     #ifdef couple_servos
-
-    input(14);
-
-    if(fb[0].speedTarget > 0) fb[0].stepDir = 1;
-    else if(fb[0].speedTarget < 0) fb[0].stepDir = -1;
-    else  fb[0].stepDir = 0;
-
-    fb[0].lag = fb[0].stepDir * fb[0].angleError;
-
-    if(fb[1].speedTarget > 0) fb[1].stepDir = 1;
-    else if(fb[1].speedTarget < 0) fb[1].stepDir = -1;
-    else  fb[1].stepDir = 0;
 
-    fb[1].lag = fb[1].stepDir * fb[1].angleError;
-
-    if(fb[1].lag > fb[0].lag)
-    {
-      int compensate = fb[1].lag - fb[0].lag;
-      compensate *= 2;
-      if(compensate > 500) compensate = 500;           
-
-      if(fb[0].speedOut > 000) fb[0].speedOut -= compensate;
-      if(fb[0].speedOut < 0000) fb[0].speedOut += compensate;
-    }
-    else if(fb[0].lag > fb[1].lag)
+    for(int p = 0; p < servo360_DEVS_MAX; p++)
     {
-      int compensate = fb[0].lag - fb[1].lag;
-      compensate *= 2;           
-      if(compensate > 500) compensate = 500;           
-
-      if(fb[1].speedOut > 000) fb[1].speedOut -= compensate;
-      if(fb[1].speedOut < 000) fb[1].speedOut += compensate;
-    }
+      if((fb[p].couple != -1) && (fb[p].feedback))
+      {
+        if(fb[p].speedTarget > 0) fb[p].stepDir = 1;
+        else if(fb[p].speedTarget < 0) fb[p].stepDir = -1;
+        else  fb[p].stepDir = 0;
 
-    #endif
-
-
-
-
-
-
-
+        fb[p].lag = fb[p].stepDir * fb[p].angleError;
+
+        if(fb[fb[p].couple].speedTarget > 0) fb[fb[p].couple].stepDir = 1;
+        else if(fb[fb[p].couple].speedTarget < 0) fb[fb[p].couple].stepDir = -1;
+        else  fb[fb[p].couple].stepDir = 0;
+
+        fb[fb[p].couple].lag = fb[fb[p].couple].stepDir * fb[fb[p].couple].angleError;
+
+        if(fb[fb[p].couple].lag > fb[p].lag)
+        {
+          int compensate = fb[fb[p].couple].lag - fb[p].lag;
+          compensate *= 2;
+          if(compensate > 500) compensate = 500;           
+
+          if(fb[p].speedOut > 0) fb[p].speedOut -= compensate;
+          if(fb[p].speedOut < 0) fb[p].speedOut += compensate;
+        }
+        else if(fb[p].lag > fb[fb[p].couple].lag)
+        {
+          int compensate = fb[p].lag - fb[fb[p].couple].lag;
+          compensate *= 2;           
+          if(compensate > 500) compensate = 500;           
+
+          if(fb[fb[p].couple].speedOut > 0) fb[fb[p].couple].speedOut -= compensate;
+          if(fb[fb[p].couple].speedOut < 0) fb[fb[p].couple].speedOut += compensate;
+        }
+      }        
+    }
 
-
+    #endif
+
     lockclr(lock360);
 
     int target[2];

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

@@ -122,7 +122,7 @@ int servo360_set(int pinControl, int time);
 
 int servo360_setAngleCtrlSpeedMax(int pin, int speed);
 int servo360_getAngleCtrlSpeedMax(int pin);
-
+int servo360_couple(int pinA, int pinB);
 
 // int servo360_enable(int pin, int state);
 
@@ -196,6 +196,7 @@ typedef volatile struct servo360_s {
   volatile int angleMax;
   volatile int angleMin;
   volatile int unitsRev;
+  volatile int couple;
 
   // admin
   volatile int csop;

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

@@ -87,6 +87,8 @@ int servo360_connect(int pinControl, int pinFeedback)
   fb[p].angleMin = -S360_A_MAX;
 
   fb[p].opMax = MAX_SPEED;
+
+  fb[p].couple = -1;
 
   fb[p].feedback = 1;
 

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

@@ -0,0 +1,64 @@
+/*
+  @file servo360_couple.c
+
+  @author Parallax Inc
+
+  @copyright
+  Copyright (C) Parallax Inc. 2017. All Rights MIT Licensed.  See end of file.
+ 
+  @brief 
+*/
+
+
+#include "simpletools.h"  
+#include "servo360.h"
+
+
+int servo360_couple(int pinA, int pinB)
+{
+  if(!servoCog) servo360_run();
+
+  int pA = servo360_findServoIndex(pinA);
+  if(pA == -1)return -1;
+  int pB = servo360_findServoIndex(pinB);
+  if(pB == -1)return -1;
+
+  if(pA < pB)
+  {
+    fb[pA].couple = pB;
+    return pB;
+  }
+  else if(pB < pA)
+  {
+    fb[pB].couple = pA;
+    return pA;
+  }
+  else
+  {
+    return -2;
+  }              
+}
+
+
+/**
+ * TERMS OF USE: MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+

Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/abdrive360.c

@@ -61,6 +61,8 @@ void drive_init(void)
   servo360_setMaxSpeed(abd360_pinCtrlLeft, abd360_speedLimit);
   servo360_setMaxSpeed(abd360_pinCtrlRight, abd360_speedLimit);
 
+  servo360_couple(abd360_pinCtrlLeft, abd360_pinCtrlRight);
+
   abd360_initialized = 1;
 }