joy.mxr

1 # -*- coding: utf-8 -*- 2 """ 3 Created on Thu Nov 3 17:05:26 2016 4 5 @author: birds 6 """ 7 DEBUG = [] 8 from time import time as now 9 from ckbot.dynamixel import MX64Module 10 from numpy import nan,exp,angle,abs,clip,pi,isnan,sign 11 from plans import Plan 12 from loggit import progress 13 14

15 -class ServoWrapperMX(object):

16 - def __init__(self, servo, logger=None, **kw):

17 assert isinstance(servo, MX64Module), "only works with MX" 18 self.servo = servo 19 self.aScl = 36000.0 # servo units per rotation 20 self.rpmScl = 1 / 64.0 21 self.posOfs = 0 22 self.ori = 1 23 self.desAng = 0 24 self.desRPM = 00 25 # Use this rpm in dead zone 26 self.deadRPM = 10 27 self.deadZone = 0.10 28 self.punch = 2.0 29 self.Kp = 10 30 self.logger = logger 31 self._clearV() 32 self._v = nan 33 self.__dict__.update(kw) 34 self._ensure_motor = self._set_motor 35 self._ensure_servo = self._set_servo

36

37 - def _ang2srv(self,ang):

38 """ 39 Convert cycle angle (range 0..1) to servo position 40 41 Angle 0 is mapped to .posOfs, growing in direction .ori 42 """ 43 return int((((float(ang)-self.posOfs)*self.ori) % 1.0) * self.aScl)

44

45 - def _srv2ang(self,srv):

46 """ 47 Convert cycle angle (range 0..1) to servo position 48 49 Angle 0 is mapped to .posOfs, growing in direction .ori 50 """ 51 return float(srv)/self.aScl/self.ori + self.posOfs

52

53 - def doCtrl(self):

54 """execute an interaction of the controller update loop""" 55 a = exp(1j * self.get_ang()*2*pi) 56 a0 = exp(1j * self.desAng*2*pi) 57 lead = angle(a / a0) 58 if abs(lead)>0.7*pi: 59 pFB = 0 60 pCH = 0 61 rpm = self.deadRPM 62 if "F" in DEBUG: 63 progress("FB lead %5.2g out of range" % (lead)) 64 # outside of capture range; ignore 65 else: 66 pCH = (self.punch * sign(lead)) if abs(lead)>self.deadZone else 0 67 pFB = clip(pCH + self.Kp * lead, -45, 45) 68 rpm = self.desRPM - pFB 69 if abs(rpm)<0.1: 70 rpm = 0 71 if "F" in DEBUG: 72 progress("FB desRPM %g p %g c %g" % (self.desRPM, pFB, pCH)) 73 if self.logger: 74 self.logger.write( "ctrl", nid=self.servo.node_id, 75 desRPM=str(self.desRPM), pFB=str(pFB), pCH=str(pCH) ) 76 # Push into the motor 77 self._set_rpm(rpm)

78

79 - def _set_servo(self):

80 """(private) set module in servo mode 81 82 also configures the _ensure_* callbacks so that _ensure_* only 83 sends command to motor once 84 """ 85 self.servo.set_mode(0) 86 if "h" in DEBUG: 87 progress("%s.set_mode('Servo')" % (self.servo.name)) 88 if self.servo.get_mode() == 0: 89 self._ensure_servo = lambda: None 90 self._ensure_motor = self._set_motor

91

92 - def _set_motor(self):

93 """(private) set module in motor mode 94 95 also configures the _ensure_* callbacks so that _ensure_* only 96 sends command to motor once 97 """ 98 self.servo.set_mode(1) 99 if "h" in DEBUG: 100 progress("%s.set_mode('Motor')" % (self.servo.name)) 101 if self.servo.get_mode() == 1: 102 self._ensure_motor = lambda: None 103 self._ensure_servo = self._set_servo

104

105 - def set_ang(self, ang):

106 self.desAng = float(ang) %1.0 107 if "s" in DEBUG: 108 progress("%s.set_angle(%g)" % (self.servo.name, ang)) 109 if self.logger: 110 self.logger.write( "set_ang", nid=self.servo.node_id, ang=str(ang) )

111

112 - def get_ang(self):

113 ang = self._srv2ang(self.servo.get_pos()) 114 if "g" in DEBUG: 115 progress("%s.get_angle --> %g" % (self.servo.name, ang)) 116 if self.logger: 117 self.logger.write( "get_ang", nid=self.servo.node_id, ang=str(ang) ) 118 self._updateV(ang) 119 return ang

120

121 - def get_v(self):

122 """ 123 Get a velocity estimate 124 """ 125 return self._v

126

127 - def _clearV(self):

128 """Clear state of velocity estimator""" 129 self._lastA = None 130 self._lastT = None

131

132 - def _updateV(self, ang):

133 """ 134 Push angle measurement into velocity estimator 135 """ 136 t = now() 137 if self._lastA is not None: 138 da = ang - self._lastA 139 dt = t - self._lastT 140 vNew = da / dt * 60.0 # angle is in rotations, dt seconds 141 # If no previous estimate --> use current estimate 142 if isnan(self._v): 143 self._v = vNew 144 else: # Use first order lowpass to smooth velocity update 145 a = 0.2 146 self._v = self._v * (1 - a) + vNew * a 147 self._lastA = ang 148 self._lastT = t

149

150 - def _set_rpm(self, rpm):

151 """Push an RPM setting to the motor""" 152 self._ensure_motor() 153 tq = clip(self.rpmScl * self.ori * rpm, -0.999, 0.999) 154 if "r" in DEBUG: 155 progress("%s.set_torque(%g) <-- rpm %g" % (self.servo.name, tq, rpm)) 156 self.servo.set_torque(tq)

157 158 if __name__=="__main__": 159 import ckbot.logical as L 160 from time import sleep 161 T0 = now()

162 - def progress(msg,sameLine=False):

163 print "%5.3g " % (now()-T0),msg, 164 if not sameLine: 165 print

166 c = L.Cluster(count=1) 167 m = c.values()[0] 168 w = ServoWrapperMX(m,ori=-1,posOfs=0.5) 169 try: 170 from pylab import linspace 171 a = linspace(-2,2,103) 172 b = [ w._srv2ang(w._ang2srv(ai)) for ai in a ] 173 assert abs((a-b+.2)%1.0-.2).max() < 0.01 174 except ImportError: 175 print "WARNING: No pylab for error estimate test" 176 DEBUG[:] = ['h','F','r','s'] 177 last = now() 178 while 1: 179 w.doCtrl() 180 if now()-last>1: 181 goal = 0.3 * (long(now()) % 2) 182 w.set_ang( goal ) 183 last = now() 184 progress("goal %g" % goal) 185 sleep(0.3) 186

Source Code for Module joy.mxr