fix(limPID): removed PID limiter for small angles

Written by: V.S. Boon
Committed on: 19-12-2021
Version: 1.0.0

Author: VSBoon

raspberry_pi/control/control.py

@@ -88,7 +88,7 @@ def PosControl(sConfig: Union[np.ndarray, List], eConfig: Union[np.ndarray, List
     timeScaling=5)
     print(f"Total estimated time for trajectory: {round(dt*traj[:,0].size, 2)} s")
     traj, velTraj, accTraj = TrajDerivatives(traj, method, robot, dt)
-    g = np.array([0,0,-9.81*2]) #EXPERIMENTAL!
+    g = np.array([0,0,-9.81]) #EXPERIMENTAL!
     FTip = np.zeros(6) #Position control, --> assume no end-effector force.
     tauPID = np.zeros(traj[0,:].size)
     n = -1 #iterator
@@ -136,7 +136,7 @@ def PosControl(sConfig: Union[np.ndarray, List], eConfig: Union[np.ndarray, List
             #Take care of communication on an interval basis:
         if (time.perf_counter() - lastWrite >= dtComm):
             SReadAndParse(serial, localMu)
-            serial.mSpeed[:-1] = [abs(val) for val in PWM]
+            serial.mSpeed[:-1] = [abs(val) if abs(val) < 255 else 255 for val in PWM]
             serial.rotDirDes = [np.sign(PWM[i]) if 
                                 np.sign(PWM[i]) == 1 else 0 for i in 
                                 range(serial.lenData-1)]

raspberry_pi/main.py

@@ -262,36 +262,35 @@ def HoldPos(serial: SerialData, Teensy: serial.Serial, robot: Robot, PIDObj: PID
     g = np.array([0,0,-9.81])
     tauFF = FeedForward(robot, thetaDes, np.zeros(5), np.zeros(5), g, FTip)
     tauPID = PIDObj.Execute(thetaDes, thetaCurr, dtHold)
-    for i in range(thetaCurr.size):
-        if abs(thetaCurr[i] - thetaDes[i]) < eLim[i]:
-            tauPID[i] = 0
     tau = tauFF + tauPID
     tauFric = np.zeros(5)
     for i in range(tau.size):
         """Compute joint friction based on friction model of joint."""
-        if not np.isclose(tauPID[i], 0, atol=1e-05) and np.isclose(dthetaCurr[i], 0, atol=1e-01):
-            tauStat = robot.joints[i].fricPar['stat']
-            tauFric[i] = tauStat*np.sign(tau[i])
-        elif np.isclose(tauPID[i], 0, atol=1e-05) and not np.isclose(dthetaCurr[i],0,atol=1e-01):
-            tauKin = robot.joints[i].fricPar['kin']
-            tauFric[i] = tauKin*np.sign(tau[i]) 
+        # if not np.isclose(tauPID[i], 0, atol=1e-05) and np.isclose(dthetaCurr[i], 0, atol=1e-01):
+        #     tauStat = robot.joints[i].fricPar['stat']
+        #     tauFric[i] = tauStat*np.sign(tau[i])
+        # elif np.isclose(tauPID[i], 0, atol=1e-05) and not np.isclose(dthetaCurr[i],0,atol=1e-01):
+        #     tauKin = robot.joints[i].fricPar['kin']
+        #     tauFric[i] = tauKin*np.sign(tau[i]) 
+        tauStat = robot.joints[i].fricPar['stat']
+        tauFric[i] = tauStat
     tau += tauFric
     #Diff-drive properties:
-    tauJ4 = tau[3]
-    tauJ5 = tau[4]
+    tauJ4 = tau[3].copy()
+    tauJ5 = tau[4].copy()
     tau[3] = (-tauJ4*1.1 - tauJ5) #This motor struggle more than the other
     tau[4] = tauJ4 - tauJ5
-    PWM = tau*33.78
-    PWM = np.round(LimDamping(thetaCurr, PWM, robot.limList, k=20))
+    PWM = np.round(tau*33.78)
+    #PWM = np.round(LimDamping(thetaCurr, PWM, robot.limList, k=20)) #DEBUG COMMENTED
 
     if time.perf_counter() - lastComm >= dtComm:
         SReadAndParse(serial, Teensy)
-        serial.mSpeed[:-1] = [abs(val) for val in PWM]
+        serial.mSpeed[:-1] = [abs(val) if abs(val) < 255 else 255 for val in PWM]
         serial.rotDirDes = [np.sign(PWM[i]) if 
                             np.sign(PWM[i]) == 1 else 0 for i in 
                             range(serial.lenData-1)]
         for i in range(serial.lenData-1): #TODO: Add Gripper function
-            serial.dataOut[i] = f"{abs(serial.mSpeed[i])}|"+\
+            serial.dataOut[i] = f"{serial.mSpeed[i]}|"+\
                                 f"{serial.rotDirDes[i]}"
         print(f"HoldPos: {serial.dataOut}")
         #TODO: Replace last entry w/ gripper commands
@@ -307,7 +306,7 @@ def HoldPos(serial: SerialData, Teensy: serial.Serial, robot: Robot, PIDObj: PID
         screen.blit(background, (0,0))
         pygame.display.update()
         lastFrame = time.perf_counter()
-    return lastComm, lastFrame
+    return lastComm, lastFrame, tau #TAU IS TEMP!
 
 if __name__ == "__main__":
     robotSelected = False

raspberry_pi/robot_init.py

@@ -88,19 +88,17 @@
 
 """Here, we add a friction model to our joints for a second (hopefully 
 more accurate) instance of the robot. Values should be experimentally confirmed!"""
-PWMMinWormGear = 80 #Can be confirmed & adjusted!
-PWMMinSprocket = 40 #Should be confirmed & adjusted!
 eff = [0.2, 0.2, 0.6, 0.6, 0.6]
-tauStat0 = 3 #Determined practically (trial-and-error)
-tauStat1 = 2 #Determined practically (trial-and-error)
-tauStat2 = 1.35 #Experimentally determined, tweaked practically
-tauStat3 = 1.3 #Determined practically (trial-and-error)
-tauStat4 = 0.7 #Determined practically (trial-and-error)
-#tauStat0 = 5
-#tauStat1 = 5
-#tauStat2 = 2
-#tauStat3 = 2
-#tauStat4 = 2
+# tauStat0 = 3 #Determined practically (trial-and-error)
+# tauStat1 = 2 #Determined practically (trial-and-error)
+# tauStat2 = 1.35 #Experimentally determined, tweaked practically
+# tauStat3 = 1.3 #Determined practically (trial-and-error)
+# tauStat4 = 0.7 #Determined practically (trial-and-error)
+tauStat0 = 0
+tauStat1 = 1.35
+tauStat2 = 1.35
+tauStat3 = 0.2
+tauStat4 = 0.2
 
 tauStat = [tauStat0, tauStat1, tauStat2, tauStat3, tauStat4]
 tauKin = [0.2*stat for stat in tauStat]

raspberry_pi/settings.py

@@ -5,13 +5,13 @@
 settings related to main.py, kindly do so here and in robot_init.py."""
 sett = dict()
 #Time between refreshing frames for the UI [s].
-sett['dtFrame'] = 0.1
+sett['dtFrame'] = 0.05
 #Time between PID updates. Note: higher dtPID leads to more instability! [s].
 sett['dtPID'] = 0.05
 #Data communication interval with Teensy [s].
 sett['dtComm'] = 0.05 #Ensure equal to one in Teensy code [s].
 #Time between FF updates [s].
-sett['dtFF'] = 0.05 
+sett['dtFF'] = 0.05
 #Time interval between trajectory sub-configurations [s].
 sett['dtPosConfig'] = 0.05
 #Interval for updating the FF & PID while holding a position [s].
@@ -21,19 +21,19 @@
 #Maximum rotational velocity of the joints in position control [rad/s].
 sett['wMax'] = 0.04*np.pi
 #Proportional gain for position control [(N/m)/rad].
-sett['kPP'] = np.diag(np.array([14,22,28,6,6]))
+sett['kPP'] = np.diag(np.array([14,28,26,6,6]))
 #Integral gain for position control [(N/m)*s/rad].
-sett['kIP'] = np.diag(np.array([4,6,10,6,6]))
+sett['kIP'] = np.diag(np.array([4,6,12,3,3]))
 #Derivative gain for position control [(N/m)/(rad*s)].
-sett['kDP'] = np.diag(np.array([2,2,4,3,3]))
+sett['kDP'] = np.diag(np.array([2,2,4,1,1]))
 #Proportional gain for velocity control [(N/m)/(rad/s)].
 sett['kPV'] = np.diag(np.array([0,0,0,0,0]))
 #Integral gain for velocity control [(N/m)*s/(rad/s)].
 sett['kIV'] = np.diag(np.array([0,0,0,0,0]))
 #Derivative gain for velocity control [(N/m)/(rad)].
 sett['kDV'] = np.diag(np.array([0,0,0,0,0]))
 #PID object for position control.
-sett['PIDP'] = PID(sett['kPP'], sett['kIP'], sett['kDP'], ILim=np.array([7,7,7,7,7]))
+sett['PIDP'] = PID(sett['kPP'], sett['kIP'], sett['kDP'], ILim=np.array([2,2,2,2,2]))
 #PID object for velocity control
 sett['PIDV'] = PID(sett['kPV'], sett['kIV'], sett['kDV'], ILim=np.array([5,5,5,5,5]))
 #Joint speed increase for joint velocity control when pressing the