308 lines
12 KiB
Python
308 lines
12 KiB
Python
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
|