from machine import Pin, ADC, PWM import time import uasyncio as asyncio import Hardware as HW import os # import PID # # [(0.0, 3071), (0.08333334, 2634), (0.1666667, 2286), (0.25, 1847), # # (0.3333333, 1453), (0.4166667, 1110), (0.5, 809), (0.5833333, 442), # # (0.6666667, 105), (0.75, 518), (0.8333333, 4095), (0.9166667, 3599)] # # >>> m.pid.Ki # # -1 # # >>> m.pid.Kd # # 10 # # >>> m.pid.Kp # # -2 # # >>> m.pid.windup_guard # # 200 # # >>> # class MotorPot: # def __init__(self, pins, adcPin, updateRate=100, freq=40, inverted=False): # self._pins = [PWM(Pin(p, Pin.OUT), freq=freq, duty=0) for p in pins] # for p in self._pins: # p.freq(freq) # self._adc = ADC(Pin(adcPin)) # self._adc.atten(ADC.ATTN_11DB) # Enable 3.6V Ranges # self.reset() # self.updateRate = updateRate # self.inverted = inverted # self._adcAngleMap = [] # self._okTargetDiff = 0.1 # self.pid = PID.PID(P=-2, D=10, I=-1) # self.pid.setWindup(200) # self.min_duty = 100 # # self._maxSpeed = 1 # self._duty = 500 # self.calib_number = None # #loop = asyncio.get_event_loop() # #loop.create_task(self._update_async()) # def reset(self): # self._rotTarget = 0 # self.rotDirection = 0 # def getStepsToTarget(self): # # return ((self._rotTarget - self.stepnum) # # *self.rotDirection)%self.stepsPerRev # return 0 # def rotateTo(self, target=None, duration=0, direction=0, normalise=True): # """sets the rotation target and (if direction == 0) calculates if # it should rotate left or right. # If target is None moves infinitely in direction # (if direction is 0 then hold position with power).""" # if target is None: # self._rotTarget = None # self.rotDirection = direction # else: # pass # def stop(self): # for p in self._pins: # p.duty(0) # self.rotDirection = 0 # self._rotTarget = None # async def _update_async(self): # while(True): # await asyncio.sleep_ms(self.updateRate) # self._update() # def _update_block(self): # while(self.isAtTarget()): # await asyncio.sleep_ms(self.updateRate) # self._update() # def _update(self): # # duty = int(self._duty) # # if self._rotTarget is not None: # # targetDiff = self.getOrientation() - self._rotTarget # # if not self.isAtTarget(): # # self.rotDirection = 1 if targetDiff > 0 else -1 # # # TODO duty = min(abs(targetDiff)*kp,1)*duty # # else: # # self.rotDirection = 0 # # self._pins[0].duty(duty if self.rotDirection < 0 else 0) # # self._pins[1].duty(duty if self.rotDirection > 0 else 0) # self.pid.SetPoint = self._rotTarget # self._adcValue = self._adc.read() # self.pid.update(self._adcValue) # v = int(self.pid.output) # print("target", self._rotTarget, # "adcValue", self._adcValue, # "output:", v) # self._pins[0].duty(-v if v < -self.min_duty else 0) # self._pins[1].duty(v if v > self.min_duty else 0) # def getOrientation(self): # adcValueNow = self._adc.read() # for i in range(len(self._adcAngleMap)-1): # if(self._adcAngleMap[i][1] <= adcValueNow # and self._adcAngleMap[i+1][1] > adcValueNow): # r = (adcValueNow - self._adcAngleMap[i][0]) / ( # self._adcAngleMap[i+1][0] - self._adcAngleMap[i][0]) # r = self._adcAngleMap[i][1] + r * \ # (self._adcAngleMap[i+1][1] - self._adcAngleMap[i][1]) # return r # # return didn't trigger, so the pot is in the deadzone # # TODO Calc a possible orientation based on time of last duty command # return 0 # def getTarget(self): # return self._rotTarget # def isAtTarget(self): # if self._rotTarget is None: # return self.rotDirection == 0 # targetDiff = self.getOrientation() - self._rotTarget # return (abs(targetDiff) < self._okTargetDiff) # def printAdcValues(self, duty=200, sleepTime=0.01, pin=0): # start = self._adc.read() # while abs(start - self._adc.read()) <= 30: # self._pins[pin].duty(duty) # time.sleep(sleepTime) # self._pins[pin].duty(0) # print(self._adc.read()) # while abs(start - self._adc.read()) > 30: # self._pins[pin].duty(duty) # time.sleep(sleepTime) # self._pins[pin].duty(0) # print(self._adc.read()) class MotorPot: def __init__(self,pins, adcPin, stepTime=3, stepWait=130,steps_per_turn = None,adc_map = None,tolerance = 30,inverted=False): self._pins = [Pin(p,Pin.OUT) for p in pins] self._adc = ADC(Pin(adcPin)) self._adc.atten(ADC.ATTN_11DB) # Enable 3.6V Ranges self.inverted = inverted self.stepTime = stepTime self.stepWait = stepWait self.steps_per_turn = steps_per_turn self.rotation_current = 0 self.rotation_target = 0 self.tolerance = tolerance self._adc_max_value = 4096 self._adc_map = adc_map self.load_settings() self.calib_number = 1 self._calibrating = False if(self._adc_map is None): self._adc_map = [(4095, 0.8333333-1-1/12), (3599, 0.9166667-1-1/12), (3071, 0.0-1/12), (2634, 0.08333334-1/12), (2286, 0.1666667-1/12), (1847, 0.25-1/12), (1453, 0.3333333-1/12), (1110, 0.4166667-1/12), (809, 0.5-1/12), (442, 0.5833333-1/12), (105, 0.6666667-1/12)] self.save_settings() #[(4095, 0.8333333-1-1/12), (3599, 0.9166667-1-1/12), (3071, 0.0-1/12), (2634, 0.08333334-1/12), (2286, 0.1666667-1/12), (1847, 0.25-1/12), (1453, 0.3333333-1/12), (1110, 0.4166667-1/12), (809, 0.5-1/12), (442, 0.5833333-1/12), (105, 0.6666667-1/12)] #self._adc_map = [(d[0],(d[1]+0.5)%1-0.5) for d in self._adc_map] loop = asyncio.get_event_loop() loop.create_task(self._update_async()) async def _getStepsPerTurn_async(self,steps_dir = 1): startV = self._adc.read() while (startV == 0 or startV == self._adc_max_value): self._step(steps_dir) startV = self._adc.read() steps = 0 while (startV - self.tolerance - self._adc.read())%self._adc_max_value > self.tolerance: self._step(steps_dir) await asyncio.sleep_ms(self.stepWait) steps += abs(steps_dir) print("waiting for:",(startV - self.tolerance - self._adc.read())%self._adc_max_value) self.steps_per_turn = steps #[(0.0, 3071), (0.08333334, 2634), (0.1666667, 2286), (0.25, 1847), (0.3333333, 1453), (0.4166667, 1110), (0.5, 809), (0.5833333, 442), (0.6666667, 105), (0.8333333, 4095), (0.9166667, 3599)] def _getStepsPerTurn(self,steps_dir = 1): loop = asyncio.get_event_loop() loop.run_until_complete(self._getStepsPerTurn_async(steps_dir)) return self.steps_per_turn def _step(self,direc): direcBool = self.inverted != (direc>0) p = self._pins[1 if direcBool else 0] adc_before = self._adc.read() p.on() #await asyncio.sleep_ms(self.stepTime) time.sleep_ms(self.stepTime) p.off() if(self._calibrating): return oldR = self.rotation_current self.rotation_current = (self.rotation_current + direc / self.steps_per_turn)%1 self.update_rotation_from_adc() #print("rotation updated:",oldR," -> ", self.rotation_current," t",self.rotation_target) def update_rotation_from_adc(self): adc_after = self._adc.read() if not (self._adc_map[0][1] > self.rotation_current > self._adc_map[-1][1]): i = 0 for i in range(len(self._adc_map)-1): if (min(self._adc_map[i+1][0],self._adc_map[i][0]) < adc_after < max(self._adc_map[i+1][0],self._adc_map[i][0])): self.rotation_current = (self._adc_map[i+1][1]+((adc_after-self._adc_map[i][0])/(self._adc_map[i+1][0]-self._adc_map[i][0]))*(self._adc_map[i+1][1]-self._adc_map[i][1]))%1 break async def run_until_target_async(self): while(not self.isAtTarget()): self._step(1 if (self.rotation_current - self.rotation_target+0.5)%1<0.5 else -1) await asyncio.sleep_ms(self.stepWait) def run_until_target_block(self,target): self.rotation_target = target loop = asyncio.get_event_loop() loop.run_until_complete(self.run_until_target_async()) return self.steps_per_turn async def _update_async(self): while(True): await self.run_until_target_async() await asyncio.sleep_ms(self.stepWait) #self.update_rotation_from_adc() def set_current_rotation(self,curr): self._adc_map = [(a[0],(a[1]-self.rotation_current+curr+0.5)%1-0.5) for a in self._adc_map] def get_filename(self): return (str(self._pins)+str(self._adc)).replace(" ","").replace("(","").replace(")","").replace(",","").replace("]","").replace("[","") def save_settings(self): foldername = "/motor_settings" try: os.remove(foldername+"/"+self.get_filename()) except Exception as e: print(e) try: os.mkdir(foldername) except: pass with open(foldername+"/"+self.get_filename(),'w') as f: f.write(str(self._adc_map)+"\n") f.write(str(self.steps_per_turn)+"\n") def load_settings(self): foldername = "/motor_settings"+"/"+self.get_filename() try: with open(foldername,'r') as f: s=f.readline() print(foldername,":\n"," _adc_map =",s) self._adc_map = eval(s) s=f.readline() print(" steps_per_turn =",s) self.steps_per_turn = eval(s) except Exception as e: print(e) def stop(self): for p in self._pins: p.off() def calib_onButtonReleased(self, button, pushdown): HW.leds[button].off() if button == 1: if pushdown > 700: self._calibrating = False self.stop() else: self._adc_map.append( (self._adc.read(), (self.calib_number-1 % 12)/12)) self._adc_map.sort(key=lambda x: -x[0]) self.calib_number = (self.calib_number) % 12+1 print(self._adc_map) self.stop() def calib_onButtonPressed(self, button): HW.leds[button].on() self._step(button - 1) def calibrate(self): self._adc_map = [] self.calib_number = 1 self._calibrating = True for i, b in enumerate(HW.buttons): b.setCallbacks( onPushDown=lambda i=i: self.calib_onButtonPressed(i), onPushUp=lambda pushDownTime, button=i: self.calib_onButtonReleased(button, pushDownTime)) # await asyncio.sleep_ms(self.updateRate) self._rotTarget = None HW.buzzer.playSound(HW.Buzzer.BEEP*self.calib_number) HW.buzzer.awaitFinish_nonasync() currNum = self.calib_number while self._calibrating: while currNum == self.calib_number: time.sleep(0.1) currNum = self.calib_number HW.buzzer.playSound(HW.Buzzer.BEEP*self.calib_number) HW.buzzer.awaitFinish_nonasync() def rotateTo(self,target): self.rotation_target = -target%1 def getOrientation(self): return self.rotation_current def getTarget(self): return self.rotation_target def isAtTarget(self): return abs(self.rotation_current - self.rotation_target)%1 < self.tolerance/self._adc_max_value