def main():
# Configure the timer as a microsecond counter.
tim = Timer(config["timer"],
prescaler=(machine.freq()[0] // 1000000) - 1,
period=config["timer-period"])
print(tim)
# Configure channel for timer IC.
ch = tim.channel(1,
Timer.IC,
pin=config["pin-capture"],
polarity=Timer.FALLING)
# Slave mode disabled in order to configure
mem32[config["timer-addr"] + TIM_SMCR] = 0
# Reset on rising edge (or falling in case of inverted detection). Ref: 25.4.3 of STM32F76xxx_reference_manual.pdf
mem32[config["timer-addr"] +
TIM_SMCR] = (mem32[config["timer-addr"] + TIM_SMCR]
& 0xfffe0000) | 0x54
# Capture sensitive to rising edge. Ref: 25.4.9 of STM32F76xxx_reference_manual.pdf
mem32[config["timer-addr"] + TIM_CCER] = 0b1001
try:
capture(ch, 50)
finally:
tim.deinit()
async def isr_test(): # Test trigger from hard ISR
from pyb import Timer
s = '''
Timer holds off cb for 5 secs
cb should now run
cb callback
Done
'''
printexp(s, 6)
def cb(v):
print('cb', v)
d = Delay_ms(cb, ('callback', ))
def timer_cb(_):
d.trigger(200)
tim = Timer(1, freq=10, callback=timer_cb)
print('Timer holds off cb for 5 secs')
await asyncio.sleep(5)
tim.deinit()
print('cb should now run')
await asyncio.sleep(1)
print('Done')
class caliMagCtrl():
def __init__(self, buf=array('H', [100]), ch=1):
self.timer = Timer(6)
self.dac = DAC(ch, bits=12)
self.buf = buf
self.dac_on = 0
def reset_buf(self, buf):
self.buf = buf
def start(self, freq=10):
self.dac.write_timed(self.buf, self.timer, mode=DAC.CIRCULAR)
self.timer.init(freq=freq * len(self.buf))
self.dac_on = 1
def stop(self):
self.timer.deinit()
self.dac.write(0)
self.dac_on = 0
def toggle(self, freq=5):
if self.dac_on:
self.stop()
else:
self.start(freq)
class caliMagCtrl1():
def __init__(self, amps=[], freqs=[], bufs=[], ch=1):
# amps: 幅值序列
# bufs: 对应幅值的正弦序列
self.timer = Timer(6)
self.dac = DAC(ch, bits=12)
self.amps = amps
self.bufs = bufs
self.freqs = freqs
self.indx = range(len(amps))
self.newtask = False
def next(self):
ind = self.indx.pop(0)
self.indx.append(ind)
self.amp = self.amps[ind]
buf = self.bufs[ind]
self.freq = self.freqs[ind]
if self.amp == 0:
self.timer.deinit()
self.dac.write(0)
else:
self.dac.write_timed(buf, self.timer, mode=DAC.CIRCULAR)
self.timer.init(freq=self.freq * len(buf))
self.newtask = True
class LedController():
def __init__(self):
self.red_led = pyb.LED(1)
self.green_led = pyb.LED(2)
self.blue_led = pyb.LED(3)
self.led_timer = Timer(LED_TIMER_ID, freq=50, callback=self.leds_on)
self.red_channel = self.led_timer.channel(1,
Timer.PWM,
callback=self.red_led_off,
pulse_width_percent=0)
self.green_channel = self.led_timer.channel(
2, Timer.PWM, callback=self.green_led_off, pulse_width_percent=0)
self.blue_channel = self.led_timer.channel(3,
Timer.PWM,
callback=self.blue_led_off,
pulse_width_percent=0)
def leds_on(self, timer):
self.red_led.on()
self.green_led.on()
self.blue_led.on()
def red_led_off(self, timer):
self.red_led.off()
def green_led_off(self, timer):
self.green_led.off()
def blue_led_off(self, timer):
self.blue_led.off()
def set_pw_colors(self, i, red_channel, green_channel, blue_channel):
red_on = max(0, math.sin(i) * RBG_MAX)
green_on = max(0, math.sin(i - DEG_120) * RBG_MAX)
blue_on = max(0, math.sin(i - DEG_240) * RBG_MAX)
debug("%f %f %f" % (red_on, blue_on, green_on))
self.red_channel.pulse_width_percent(red_on)
self.green_channel.pulse_width_percent(green_on)
self.blue_channel.pulse_width_percent(blue_on)
def pulse(self, duration_ms):
startTime = pyb.millis()
while (duration_ms < 0 or pyb.elapsed_millis(startTime) < duration_ms):
self.set_pw_colors(pyb.millis() / 1000, self.red_channel,
self.green_channel, self.blue_channel)
pyb.udelay(self.led_timer.period())
self.led_timer.deinit()
self.red_led_off(None)
self.green_led_off(None)
self.blue_led_off(None)
class Motor:
_index = None
_pin = None
_timer = None
_channel = None
_rate = None
_rate_min = None
_rate_max = None
# Each range represents 50% of the complete range
_step = None
_debug = False
# ########################################################################
# ### Properties
# ########################################################################
@property
def rate(self):
return self._rate
@rate.setter
def rate(self, value):
self._rate = max(min(value, 100), 0)
pulse_value = int(self._rate_min + self._rate * self._step)
if not self._debug:
self._channel.pulse_width(pulse_value)
else:
print("<<ESC>> %s: %.3d" % (self._pin, self._rate))
# ########################################################################
# ### Constructors and destructors
# ########################################################################
def __init__(self, esc_index, rate_min=1000, rate_max=2000, debug=False):
if esc_index >= 0 & esc_index <= 3:
self._debug = debug
self._rate_min = rate_min
self._rate_max = rate_max
self._step = (rate_max - rate_min) / 100
self._index = esc_index
self._pin = ESC_PIN[esc_index]
self._timer = Timer(ESC_TIMER[esc_index], prescaler=83, period=19999)
self._channel = self._timer.channel(ESC_CHANNEL[esc_index], Timer.PWM, pin=Pin(self._pin))
self.rate = 0
else:
raise Exception("Invalid ESC index")
def __del__(self):
self.rate(self._rate_min)
self._timer.deinit()
class ESC:
freq_min = 950
freq_max = 1950
def __init__(self, index):
self.timer = Timer(esc_pins_timers[index], prescaler=83, period=19999)
self.channel = self.timer.channel(esc_pins_channels[index],
Timer.PWM,
pin=Pin(esc_pins[index]))
self.trim = esc_trim[index]
def move(self, freq):
freq = min(self.freq_max, max(self.freq_min, freq + self.trim))
self.channel.pulse_width(int(freq))
def __del__(self):
self.timer.deinit()
class PidTimer(Pid):
'''
Implements the PID-stabilization algorithm as a Timer callback
IMPORTANT!!! It seems it is not allowed to allocate memory within the timer callback.
Therefore, this class won't work.
'''
def __init__(self, timerId, length, readInputDelegate, setOutputDelegate,
pidName):
'''
Constructor
@param timerId: Timer to be used
@param length: Number of items to stabilize
@param readInputDelegate: Method to gather current values.
Must return an array with the same number of items to stabilize
@param setOutputDelegate: Delegate's parameter is an array with the values to react,
one for each item to stabilize.
@param pidName: (optional) Name to identify the PID-thread among other ones.
'''
super().__init__(length, readInputDelegate, setOutputDelegate, pidName)
self._timer = Timer(timerId)
def _doCalculate(self, timer):
print("_doCalculate")
self._calculate()
def init(self, freq, initRunning=True):
'''
Initializes stabilization as a coroutine.
Initializes a thread to perform calculations in background
@param freq: Frequency of the stabilization. This value is ignored if period is provided
@param initRunning: Starts stabilization immediately
'''
self._timer.init(freq=freq)
self._timer.callback(lambda t: self._doCalculate(t))
def stop(self):
'''
Stops the coroutine
'''
self._timer.callback(None)
self._timer.deinit()
class ESC:
freq_min = 950
freq_max = 1950
def __init__(self, index):
self.timer = Timer(esc_pins_timers[index], prescaler=83, period=19999)
self.channel = self.timer.channel(esc_pins_channels[index],
Timer.PWM,
pin=Pin(esc_pins[index]))
self.trim = esc_trim[index]
def move(self, freq):
freq = min(self.freq_max, max(self.freq_min, freq + self.trim))
self.channel.pulse_width(int(freq))
def __del__(self):
self.timer.deinit()
def main():
# Configure the timer as a microsecond counter.
tim = Timer(config["timer"],
prescaler=(machine.freq()[0] // 1000000) - 1,
period=config["timer-period"])
print(tim)
# Configure timer for led blinking
if "led-id" in config:
timLed = Timer(config["led-timer"], freq=config["led-freq"])
timLed.callback(lambda t: LED(config["led-id"]).toggle())
# Configure channel for timer IC.
ch = tim.channel(1,
Timer.IC,
pin=config["pin-capture"],
polarity=Timer.FALLING)
# Slave mode disabled in order to configure
mem32[config["timer-addr"] + TIM_SMCR] = 0
# Reset on rising edge (or falling in case of inverted detection). Ref: 25.4.3 of STM32F76xxx_reference_manual.pdf
mem32[config["timer-addr"] +
TIM_SMCR] = (mem32[config["timer-addr"] + TIM_SMCR]
& 0xfffe0000) | 0x54
# Capture sensitive to rising edge. Ref: 25.4.9 of STM32F76xxx_reference_manual.pdf
# The same values are also valid for the MCU STM32L4x6
mem32[config["timer-addr"] + TIM_CCER] = 0b1001
motor = Motor(config["motor-pwm-pin"], config["motor-timer"],
config["motor-channel"], config["motor-reverse-pin"])
try:
testMotor(ch, motor, 10, 15)
testMotor(ch, motor, 20, 15)
testMotor(ch, motor, 40, 15)
testMotor(ch, motor, 60, 15)
testMotor(ch, motor, 80, 15)
finally:
motor.cleanup()
timLed.deinit()
class LEDState(
State
): # Output is determined by ``__enter__`` and ``__exit__`` (common in embedded machines).
def __init__(self, led_num, time_on, event):
super().__init__(ident=led_num) # Use the LED num as the ident.
self.led = LED(self.ident) # The LED to use.
self.timer = Timer(timer0 + self.ident) # The timer to use.
self.timeout = time_on # Time to wait before firing event.
self.event = event # Event to fire at end of time_on.
def __enter__(self):
self.led.on()
self.timer.init(
freq=1 / self.timeout,
callback=lambda _: schedule(tl_fire_ref, self.event))
return self
def __exit__(self, exc_type, exc_value, traceback):
self.led.off()
self.timer.deinit()
return False
class FlashingRed(State): # Special fault state that should never exit.
def __init__(self):
super().__init__(ident='error')
self.timer = Timer(timer0 + 4)
self.led = LED(1)
# noinspection PyUnusedLocal
def toggle_with_arg(
not_used
): # Toggle func that accepts an arg, because ``schedule`` *needs* an arg.
self.led.toggle()
self.led_tog_ref = toggle_with_arg # Store the function reference locally to avoid allocation in interrupt.
def __enter__(self):
self.timer.init(
freq=2, callback=lambda _: schedule(self.led_tog_ref, None))
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.led.off()
self.timer.deinit()
class Pwm(object):
'''
Generates PWM signal
'''
def __init__(self, pin, timer, channel, freq):
'''
Constructor
@param pin: Board pin where the PWM signal will be generated. Look at pinout schematics of the proper board in order to know valid pins
@param timer: Timer to use for signal generation. Only the valid timer for the pin can be used. Look at pinout schematics.
@param channel: Channel to use for signal generation. Look at pinout schematics for valid channel.
@param freq: Frequency rate the signal will be generated.
'''
self._pin = pin
self._timer = Timer(timer, freq=freq)
self._channel = self._timer.channel(channel, Timer.PWM, pin=self._pin, pulse_width=0)
def setDutyPerc(self, dutyPerc):
'''
Sets the duty percentage
@param dutyPerc: Percentage of time the PWM signal will be high for each period.
'''
self._channel.pulse_width_percent(dutyPerc)
def cleanup(self):
'''
Finishes and releases resources
'''
self._channel.pulse_width(0)
self._timer.deinit()
self._pin.init(0)
class caliMagCtrl2():
def __init__(self, freqs=[], bufs=[], ch=1, **kwargs):
# freqs:待测试的频率
# bufs: 对应幅值的正弦序列等消息，每个元素包含(amp,buf,port)
# 其中port是指，使用可变电阻时对应控制的端口
# ch:测试信号输出端口选择
# kwargs: r_port,电阻选择的端口列表
self.timer = Timer(6)
self.dac = DAC(ch, bits=12)
self.freqs = freqs
self.bufs = bufs
self.r_adapt = False
if len(kwargs) > 0:
if 'r_port' in kwargs: #使用电阻自适应调整
self.r_adapt = True
(pyb.Pin(pin, pyb.Pin.OUT_PP) for pin in kwargs['r_port'])
self.freqs = freqs
self.indx = range(len(amps))
self.newtask = False
def next(self):
ind = self.indx.pop(0)
self.indx.append(ind)
self.amp = self.amps[ind]
buf = self.bufs[ind]
self.freq = self.freqs[ind]
if self.amp == 0:
self.timer.deinit()
self.dac.write(0)
else:
self.dac.write_timed(buf, self.timer, mode=DAC.CIRCULAR)
self.timer.init(freq=self.freq * len(buf))
self.newtask = True
def pulseIn(pin, st):
start = 0
end = 0
mas_filtr = []
micros = Timer(5, prescaler=83, period=0x3fffffff)
micros.counter(0)
if st:
while pin.value() == 0:
start = micros.counter()
while pin.value() == 1:
end = micros.counter()
else:
while pin.value() == 1:
start = micros.counter()
while pin.value() == 0:
end = micros.counter()
micros.deinit()
res = (end - start)
mas_filtr = [res for i in range(10)]
return filtr_mas(mas_filtr)
class Buzzer(object):
'''
Plays sounds through the buzzer module
'''
def __init__(self, pin, timerId, channelId):
'''
Constructor
@param pin: Pin where the module is attached to
@param timerId: Timer associated to the pin
@param channelId: Channel of the timer
'''
self._pin = pin
#20200407 DPM: The timer must be initialized with a frequency, otherwise it won't beep.
self._timer = Timer(timerId, freq=440)
self._channel = self._timer.channel(channelId,
Timer.PWM,
pin=self._pin,
pulse_width=0)
self._buzzing = False
def startBuzz(self, freq):
'''
Starts beeping. Use the stopBuzz method to finish.
@see Buzzer.stopBuzz
@param freq: Frequency of the sound
'''
if self._buzzing:
self._timer.freq(freq)
else:
self._timer.init(freq=freq)
self._buzzing = True
self._channel.pulse_width_percent(50.0)
def stopBuzz(self):
'''
Stops beeping.
@see Buzzer.startBuzz
'''
self._channel.pulse_width(0)
self._buzzing = False
def buzz(self, freq, millisecs):
'''
Beeps a sound during a time
@param freq: Frequency of the sound
@param millisecs: Time to play
'''
self.startBuzz(freq)
sleep_ms(millisecs)
self.stopBuzz()
def trill(self, freq, millisecs, playTime=10, muteTime=10):
'''
Makes a vibrating sound
@param freq: Frequency of the sound
@param millisecs: Duration of the sound
@param playTime: (default=10) Time the buzzer beeps, as milliseconds
@param muteTime: (default=10) Time the buzzer is muted, as milliseconds
'''
startTime = ticks_ms()
while millisecs > ticks_diff(ticks_ms(), startTime):
self.startBuzz(freq)
sleep_ms(playTime)
self.stopBuzz()
sleep_ms(muteTime)
def slide(self, freq1, freq2, millisecs):
'''
Slides sound between frequencies
@param freq1: Starting frequency
@param freq2: End frequency
@param millisecs: Duration of the effect
'''
step = (freq2 - freq1) / millisecs
freq = freq1
t = 0
while t < millisecs:
self.startBuzz(freq)
sleep_ms(1)
t += 1
freq += step
self._channel.pulse_width(0)
def cleanup(self):
'''
Removes resources
'''
self._channel.pulse_width(0)
self._buzzing = False
self._timer.deinit()
BLUE_BUTTON = Pin("E11", Pin.IN, Pin.PULL_UP)
UP_BUTTON = Pin("E1", Pin.IN, Pin.PULL_UP)
DOWN_BUTTON = Pin("D1", Pin.IN, Pin.PULL_UP)
LEFT_BUTTON = Pin("E3", Pin.IN, Pin.PULL_UP)
RIGHT_BUTTON = Pin("E2", Pin.IN, Pin.PULL_UP)
PUSH_BUTTON = Pin("D0", Pin.IN, Pin.PULL_UP)
JOY_BUTTON = Pin("E0", Pin.IN, Pin.PULL_UP)
BUZZER_PIN = Pin('A2')
BUZZER_FREQ = Timer(3, freq=240)
buzzer = BUZZER_FREQ.channel(1, Timer.PWM, pin=BUZZER_PIN)
buzzer.pulse_width_percent(0)
BUZZER_FREQ.deinit()
#Setup leds
BLUE_LED = Pin("E10", Pin.OUT)
RED_LED = Pin("E12", Pin.OUT)
YELLOW_LED = Pin("SD_DETECT", Pin.OUT)
GREEN_LED = Pin("E14", Pin.OUT)
#Setup hbt timer
hbt_state = 0
hbt_interval = 500
start = utime.ticks_ms()
next_hbt = utime.ticks_add(start, hbt_interval)
hbt_led.value(hbt_state)
print("starting")
prescaler = int(ticks / 0x3fffffff)
period = int((WORST_CASE_LATENCY * PAYLOAD * pyb.freq()[2] * 2) /
(1000 * (prescaler + 1)))
while (n < N_SAMPLES):
timer.counter(0)
timer.init(prescaler=prescaler, period=period, callback=timeout)
for i in range(0, PAYLOAD):
interface.send_request_only(TMCL_Request(MODULE_ID, 1, 2, 3, 4, 5),
host_id=HOST_ID,
module_id=MODULE_ID)
try:
for i in range(0, PAYLOAD):
interface.receive_reply(module_id=MODULE_ID, host_id=HOST_ID)
except:
pass
timer.deinit()
if (not (tout)):
counter = timer.counter()
logger.debug("Measured delta ticks: {}".format(counter))
# update values
value = (2 * PAYLOAD) / counter
if (n == 0):
min_ticks = value
max_ticks = value
avg_ticks = value
std_dev_ticks = 0
else:
min_ticks = min(min_ticks, value)
max_ticks = max(max_ticks, value)
avg_ticks_new = avg_ticks + ((value - avg_ticks) / (n + 1))
std_dev_ticks = (((n - 1) * std_dev_ticks) +
class Controller:
def __init__(self, uart, pwm_freq):
self.uart = uart
self.i_state = array('i', [0] * len(INAMES))
self.f_state = array('f', [0] * len(FNAMES))
# controller timer
self.controller_timer = Timer(1)
# motor power control
self.nstby = Pin('NSTBY', mode=Pin.OUT_PP)
self.nstby.value(0)
# motors
motor_pwm_timer = Timer(8, freq=pwm_freq)
self.motor1 = TB6612(
motor_pwm_timer.channel(3, Timer.PWM_INVERTED, pin=Pin('PWM_A')),
Pin('AIN1', mode=Pin.OUT_PP), Pin('AIN2', mode=Pin.OUT_PP))
self.motor2 = TB6612(
motor_pwm_timer.channel(1, Timer.PWM_INVERTED, pin=Pin('PWM_B')),
Pin('BIN1', mode=Pin.OUT_PP), Pin('BIN2', mode=Pin.OUT_PP))
# encoders
self.enc1 = init_encoder(4, 'ENC_A1', 'ENC_A2', Pin.AF2_TIM4)
self.enc2 = init_encoder(3, 'ENC_B1', 'ENC_B2', Pin.AF2_TIM3)
# gyro
i2c = I2C(1, freq=400_000)
imu = BNO055(i2c, crystal=True, transpose=(2, 0, 1), sign=(0, 0, 1))
imu.mode(NDOF_FMC_OFF_MODE)
sleep_ms(800)
imu.iget(QUAT_DATA)
self.imu = imu
# pid controllers
self.pid = [
PID(1, 7, 50, 0, -100, 100),
PID(1, 7, 50, 0, -100, 100),
]
def start(self, fs, controller_name="duty_control"):
self.stop()
self.control_laws = getattr(self, controller_name)
self.set_fs(fs)
self.nstby.value(1)
gc.collect()
self.controller_timer.init(freq=fs, callback=self.control)
def stop(self):
self.controller_timer.callback(None)
self.nstby.value(0)
def shutdown(self):
self.controller_timer.deinit()
self.nstby.value(0)
def set_fs(self, fs):
for pid in self.pid:
pid.Ts = 1 / fs
def set_kp(self, pid_index, kp):
self.pid[pid_index].kp = kp
def set_ki(self, pid_index, ki):
self.pid[pid_index].ki = ki
def set_kd(self, pid_index, kd):
self.pid[pid_index].kd = kd
def control(self, timer):
start_us = ticks_us()
# locals
i_state = self.i_state
f_state = self.f_state
# time index
i_state[_ISTATE_K] += 1
# encoders
enc1 = self.enc1
enc2 = self.enc2
cps1 = -c2(enc1.counter())
enc1.counter(0)
cps2 = c2(enc2.counter())
enc2.counter(0)
i_state[_ISTATE_ENC1] += cps1
i_state[_ISTATE_ENC2] += cps2
f_state[_FSTATE_CPT1] = cps1
f_state[_FSTATE_CPT2] = cps2
# gyro
imu = self.imu
imu.iget(QUAT_DATA)
f_state[_FSTATE_QUAT_W] = w = imu.w / (1 << 14)
f_state[_FSTATE_QUAT_X] = x = imu.x / (1 << 14)
f_state[_FSTATE_QUAT_Y] = y = imu.y / (1 << 14)
f_state[_FSTATE_QUAT_Z] = z = imu.z / (1 << 14)
sinp = 2 * (w * y - z * x)
if abs(sinp) >= 1:
# use 90 degrees if out of range
pitch = math.copysign(math.pi / 2, sinp)
else:
pitch = math.asin(sinp)
pitch *= RAD2DEG
f_state[_FSTATE_PITCH] = pitch
# call control code
self.control_laws()
# controller execution time
i_state[_ISTATE_DT1] = ticks_diff(ticks_us(), start_us)
# status
uart = self.uart
uart.writechar(CMD_STATE)
uart.write(i_state)
uart.write(f_state)
# total execution time
i_state[_ISTATE_DT2] = ticks_diff(ticks_us(), start_us)
def duty_control(self):
f_state = self.f_state
self.motor1.speed(f_state[_FSTATE_DUTY1])
self.motor2.speed(f_state[_FSTATE_DUTY2])
def speed_control(self):
f_state = self.f_state
pid = self.pid
cps1 = f_state[_FSTATE_CPT1]
cps2 = f_state[_FSTATE_CPT2]
# control
pid[PID_CPT1].setpoint = f_state[_FSTATE_CPT1_SP]
pid[PID_CPT2].setpoint = f_state[_FSTATE_CPT2_SP]
duty1 = pid[PID_CPT1].update(cps1)
duty2 = pid[PID_CPT2].update(cps2)
self.motor1.speed(duty1)
self.motor2.speed(duty2)
f_state[_FSTATE_DUTY1] = duty1
f_state[_FSTATE_DUTY2] = duty2
from pyb import Pin, Timer
from time import sleep_ms
tim = Timer(3, freq=440)
pwm = tim.channel(2, Timer.PWM, pin=Pin('D5')) # Y2 pour Pyboard
pwm.pulse_width_percent(50)
for f in [440, 560, 780]:
tim.freq(f)
sleep_ms(1000)
tim.deinit()
while pyb.millis() <= now + ms:
n = (n + 1) % 4
leds[n].toggle()
pyb.delay(50)
finally:
for l in leds:
l.off()
from nemastepper import Stepper
motor1 = Stepper('Y1','Y2','Y3')
from pyb import Timer
def step_cb(t):
motor1.do_step()
tim = Timer(8,freq=10000)
tim.callback(step_cb) #start interrupt routine
motor1.MAX_ACCEL = 1000
motor1.set_speed(1000)
disco(1000)
motor1.set_speed(0)
motor1.set_speed(-1000)
disco(1000)
motor1.set_speed(0)
motor1.set_off()
tim.deinit()
class sr04distance(object):
"""Driver for SR05 sonic distance sensor. """
# _MAXINCHES = const(20) #maximum range of SR04.
def __init__( self, tpin, epin, timer = 2 ) :
'''tpin = Timer pin.
epin = Echo pin.
timer = Timer #.
'''
if type(tpin) == str:
self._tpin = Pin(tpin, Pin.OUT_PP, Pin.PULL_NONE)
elif type(tpin) == Pin:
self._tpin = tpin
else:
raise Exception("trigger pin must be pin name or pyb.Pin configured for output.")
self._tpin.low()
if type(epin) == str:
self._epin = Pin(epin, Pin.IN, Pin.PULL_NONE)
elif type(epin) == Pin:
self._epin = epin
else:
raise Exception("echo pin must be pin name or pyb.Pin configured for input.")
# Create a microseconds counter.
self._micros = Timer(timer, prescaler = 83, period = 0x3fffffff)
def __del__( self ) :
self._micros.deinit()
@property
def counter( self ) :
return self._micros.counter()
@counter.setter
def counter( self, value ) :
self._micros.counter(value)
@property
def centimeters( self ) :
'''Get # of centimeters distance sensor is reporting.'''
start = 0
end = 0
self.counter = 0
#Send 10us pulse.
self._tpin.high()
udelay(10)
self._tpin.low()
while not self._epin.value():
start = self.counter
j = 0
# Wait 'till the pulse is gone.
while self._epin.value() and j < 1000:
j += 1
end = self.counter
# Calc the duration of the recieved pulse, divide the result by
# 2 (round-trip) and divide it by 29 (the speed of sound is
# 340 m/s and that is 29 us/cm).
return (end - start) / 58.0
@property
def inches( self ) :
#Get distance in inches.
return self.centimeters * 0.3937
class InputCapture(object):
'''
Captures periodic signals on a pin
'''
_addresses = {
1: TIM1,
2: TIM2,
3: TIM3,
4: TIM4,
5: TIM5,
6: TIM6,
7: TIM7,
8: TIM8,
15: TIM15,
16: TIM16,
17: TIM17
}
def __init__(self, timerId, pin):
'''
Constructor. Initializes a input-capture timer
@param timerId: Id-number of the timer
@param pin: Pin where the capture will be performed
'''
self._timerId = timerId
self._timerAddr = InputCapture._addresses[timerId]
self._pin = pin
self._timer = Timer(self._timerId,
prescaler=(freq()[0] // 1000000) - 1,
period=0xffff)
# Set the timer and channel into input mode
self._channel = self._timer.channel(1,
mode=Timer.IC,
pin=self._pin,
polarity=Timer.FALLING)
mem32[self._timerAddr + TIM_SMCR] = 0
mem32[self._timerAddr + TIM_SMCR] = (mem32[self._timerAddr + TIM_SMCR]
& 0xfffe0000) | 0x54
mem32[self._timerAddr + TIM_CCER] = 0b1001
self.reset()
def reset(self):
'''
Resets the captured value to zero
'''
# Set the timer and channel into output mode in order to reset the capture value
mem32[self._timerAddr + TIM_CCMR1] = 0
mem32[self._timerAddr + TIM_CCR1] = 0
mem32[self._timerAddr + TIM_CCMR1] = 1
def capture(self):
'''
@return: The captured value
'''
return self._channel.capture()
def stop(self):
'''
Stops the input capture
'''
self._timer.deinit()
def cleanup(self):
'''
Releases the resources
'''
self.stop()
self.reset()
class SR04Distance(object):
""" """
maxinches = 20 #maximum range of SR04.
def __init__( self, tpin, epin, timer=2 ) :
""" """
if type(tpin) == str:
self._tpin = Pin(tpin, Pin.OUT_PP, Pin.PULL_NONE)
elif type(tpin) == Pin:
self._tpin = tpin
else:
raise Exception("trigger pin must be pin name or pyb.Pin configured for output.")
self._tpin.low()
if type(epin) == str:
self._epin = Pin(epin, Pin.IN, Pin.PULL_NONE)
elif type(epin) == Pin:
self._epin = epin
else:
raise Exception("echo pin must be pin name or pyb.Pin configured for input.")
# Create a microseconds counter.
self._micros = Timer(timer, prescaler=83, period=0x3fffffff)
def __del__( self ) :
self._micros.deinit()
@property
def counter( self ) : return self._micros.counter()
@counter.setter
def counter( self, value ) : self._micros.counter(value)
@property
def centimeters( self ) :
start = 0
end = 0
self.counter = 0
#Send 10us pulse.
self._tpin.high()
udelay(10)
self._tpin.low()
while not self._epin.value():
start = self.counter
j = 0
# Wait 'till the pulse is gone.
while self._epin.value() and j < 1000:
j += 1
end = self.counter
# Calc the duration of the recieved pulse, divide the result by
# 2 (round-trip) and divide it by 29 (the speed of sound is
# 340 m/s and that is 29 us/cm).
return (end - start) / 58
@property
def inches( self ) : return self.centimeters * 0.3937
class PID:
def __init__(self, P, I, D, target_fn, target_freq):
self.Kp = P
self.Ki = I
self.Kd = D
self.target = 0 # Degrees, planner.dist_bearing(), returns flag, (dist, bear)
self.feedback = 0 # Degrees
self.error = 0 # Degrees
self.pid_out = 0
self.output = (0, 0) # (left_pwr, right_pwr)
self.prev_error = 0
self.error_sum = 0
self.base_speed = 0
self.t_fn = target_fn
self.target_flag = False # Set to true by the timer interrupt
self.t_c_top = round(
500 / target_freq) # Counter top values. Timer is set to 100 Hz
self.t_c = 0 # Target counter incremented when the timer triggers
self.timer = None # Timer objects interrupting at period specified by user
self.enable = False
self.LIMITER = 1 # Limits the output for testing
self.POWER_LIMIT = 90
self.BASE_PWR = 50
self.log_msg = None
self.i = 0
def pid(self):
# Updating the target variable
if self.target_flag is True:
# t_fn is planner.dist_bearing()
flag, (dist, bear) = self.t_fn()
self.target_flag = False
if flag is True:
self.target = bear
else:
return False
self.feedback = global_variables.fuse.heading
# Error term
self.error = self.target - self.feedback
# Always rotating in the shortest direction
if self.error > 180:
self.error = 360 - self.error
elif self.error < -180:
self.error = 360 + self.error
if (self.error > -20) and (self.error < 20):
self.error_sum += self.error
self.pid_out = self.Kp * self.error # Proportional
self.pid_out += self.Kd * self.prev_error # Derivative
self.pid_out += self.Ki * self.error_sum # Integral
self.prev_error = self.error
self.base(self.error)
# If error is negative (left turn needed to reduce error), so is pid_out. right speed increases, left speed decreases, vice versa
self.output = (round(
((self.base_speed + self.pid_out / 2) * self.LIMITER),
0), round(((self.base_speed - self.pid_out / 2) * self.LIMITER),
0))
if (abs(self.output[0]) < self.POWER_LIMIT) and (abs(self.output[1]) <
self.POWER_LIMIT):
motor.motor_abs(("l", self.output[0]), ("r", self.output[1]))
else:
cmd.send_uart("pid out of range\n", log_flag)
self.error_sum = 0
return True
def base(self, error):
abs_err = abs(error)
self.base_speed = -(self.BASE_PWR / 180) * abs_err + self.BASE_PWR
def update(self, timer):
self.t_c += 1
if self.t_c % self.t_c_top == 0:
self.target_flag = True
def start_update(self):
# 16-bit timer
self.timer = Timer(9)
self.timer.init(freq=100)
self.timer.callback(self.update)
self.enable = True
self.error_sum = 0
cmd.send_uart("PID timers started\n", log_flag)
def stop_update(self):
try:
self.timer.deinit()
except AttributeError:
cmd.send_uart("PID not initialised\n", log_flag)
self.enable = False
self.error_sum = 0
motor.motor_abs(("l", 0), ("r", 0))
cmd.send_uart("PID stopped\n", log_flag)
def start_pid(self):
# Allows time for the first IMU reading to be completed
sleep_ms(12)
cmd.send_uart("PID thread started", True)
print("PID thread started")
self.enable = False
i = 0
while True:
if i % 5 == 0:
if self.enable:
self.pid()
if i % 3 == 0:
try:
acc = global_variables.imu.accel.xyz
gyr = global_variables.imu.gyro.xyz
mag = global_variables.imu.mag.xyz
except:
cmd.send_uart("MPU reading error", log_flag)
if i % 300 == 0:
msg = {
"type": "gps",
"func": "heading",
"head": str(global_variables.fuse.heading)
}
msg = ujson.dumps(msg)
cmd.send_uart(msg, log_flag)
# print(global_variables.fuse.heading)
global_variables.fuse.update(acc, gyr, mag)
i += 1
def set_pid(self, P, I, D):
self.Kp = P
self.Ki = I
self.Kd = D
if debug_flag is True:
cmd.debug("PID set", debug_flag)
def get_pid(self):
return self.Kp, self.Ki, self.Kd
def set_pwr(self, base_pwr):
if (base_pwr >= 0) and (base_pwr <= 100):
self.BASE_PWR = base_pwr
return True
else:
return False
def get_pwr(self):
return self.BASE_PWR
def adc_process(tt):
lock = _thread.allocate_lock()
if 'data.json' in os.listdir():
state_file = 2
else:
state_file = 0
warning = 0
def warn_flag(t):
nonlocal warning
warning = 1
stoping = 0
def stop_flag(t):
nonlocal stoping
stoping = 1
state_warn = 0
state_stop = 0
while True:
v, c, t = adc.read_ad()
lock.acquire()
lcd.send_real(v, c, t)
lock.release()
data = {'voltage': v, 'current': c, 'temp': t}
if state_file == 0:
lock.acquire()
outctl.high('led2')
network.send(data)
lock.release()
outctl.low('led2')
if file_flag == 1:
state_file = 1
f = open('/sd/data.json', 'w')
elif state_file == 1:
data.update({'time': time_now})
f.write(json.dumps(data) + '\n')
f.flush()
if file_flag == 0:
f.close()
state_file = 2
elif state_file == 2:
lock.acquire()
outctl.high('led2')
f = open('/sd/data.json', 'r')
try:
while True:
if not f.readline():
break
data = f.readline()
data = eval(data.strip())
network.send(data)
#for line in f.readlines():
# data = eval(line.strip())
# network.send(data)
except:
pass
f.close()
outctl.low('led2')
os.remove('/sd/data.json')
lock.release()
state_file = 0
#报警处理
if state_warn == 0:
if float(v) > voltage_b * (1 + warnvalue / 100) or float(
c) > current_b * (1 + warnvalue / 100):
tim1 = Timer(1, freq=(1 / warntime), callback=warn_flag)
state_warn = 1
elif state_warn == 1:
if float(v) < voltage_b * (1 + warnvalue / 100) and float(
c) < current_b * (1 + warnvalue / 100):
tim1.deinit()
state_warn = 0
elif warning == 1:
outctl.high('alarm')
state_warn = 2
elif state_warn == 2:
if float(v) < voltage_b * (1 + warnvalue / 100) and float(
c) < current_b * (1 + warnvalue / 100):
tim1.deinit()
state_warn = 0
warning = 0
outctl.low('alarm')
#停机处理
if state_stop == 0:
if float(v) > voltage_b * (1 + stopvalue / 100) or float(
c) > current_b * (1 + stopvalue / 100):
tim2 = Timer(2, freq=(1 / stoptime), callback=stop_flag)
state_stop = 1
elif state_stop == 1:
if float(v) < voltage_b * (1 + stopvalue / 100) and float(
c) < current_b * (1 + stopvalue / 100):
tim2.deinit()
state_stop = 0
elif stoping == 1:
outctl.high('relay')
state_stop = 2
elif state_stop == 2:
#if float(v) < voltage_b*(1+stopvalue/100):
# tim2.deinit()
# state_stop = 0
# stoping = 0
pass #停止关机
time.sleep_ms(int(1000 / adc_freq))
class magCtrl():
def __init__(self,
bufs,
freqs,
DAC_ch,
sync,
BoardCtrl,
repeat=50,
timer_num=6):
'''
bufs: list, DAC output array
typ: list, the same length with bufs, 'tiny','big','cut' to describe which type of the buf
freqs: list, tested frequences
ch: DAC output channel, 1: 'X5' 2: 'X6'
sync: object for output synchronus signal to record client
resistset: object for adjusting resist according to the magnet strength
timer: internal timer, default 6
'''
self.bufs = bufs
self.freqs = freqs
if DAC_ch == 'X5':
self.dac = DAC(1, bits=12)
elif DAC_ch == 'X6':
self.dac = DAC(2, bits=12)
else:
raise ValueError('DAC pin should be X5 or X6')
self.sync = sync
self.boardctrl = BoardCtrl
self.timer = Timer(timer_num)
self.buf_indx = 0
self.fre_indx = 0
self.fre_len = len(self.freqs)
self.buf_len = len(self.bufs)
self.repeat = repeat
self.rcount = 0
self.end_flg = False
self.new_block = False
self.stimulus_level = 0
def next(self, t):
self.new_block = True
if not self.end_flg:
freq = self.freqs[self.fre_indx]
s, self.stimulus_level, buf = self.bufs[self.buf_indx]
self.boardctrl.update(self.stimulus_level)
print('new trial:', s, 'pT ', freq, 'Hz')
self.buf_indx += 1
if self.buf_indx == self.buf_len:
self.buf_indx = 0
self.fre_indx += 1
if self.fre_indx == self.fre_len:
self.fre_indx = 0
self.rcount += 1
if self.rcount == self.repeat:
self.end_flg = True
self.timer.deinit()
self.dac.write_timed(buf, self.timer,
mode=DAC.CIRCULAR) #启动定时器，写DAC
self.timer.init(freq=freq * len(buf))
self.sync.falling_edge() # 发送同步信号
from pyb import Timer
pwm_pin1 = Pin('PA5', Pin.OUT)
pwm_tim1 = Timer(2) #not start
ch = self.pwm_tim1.channel(1, Timer.PWM, pin=self.pwm_pin1)
ch.pulse_width_percent(50) #占空比
#停止
pwm_tim1.deinit()
class Button(object):
'''
This button can handle short and long press
'''
def __init__(self, pin, timerId=6, thresholdTime=1000, lowOnPress=False):
'''
Constructor
@param pin: Pin object where the button is
@param timerId: (default=6) Timer to determine the long press
@param thresholdTime: Waiting time to determine a long press as milliseconds
@param lowOnPress: Indicates whether the value is 0 when the button is pressed (pull-down)
The user (blue) button on the NUCLEO-L476RG board must have this parameter as True,
but for the case of the NUCLEO-F767ZI board, this parameter must be False
'''
self._pin = Pin(pin)
self._pin.init(mode=Pin.IN)
self._pin.irq(handler=self._onPinIrq)
self._lowOnPress = lowOnPress
self._thresholdTime = thresholdTime
self._pressTime = 0
self._timer = Timer(timerId)
self._shortPressHandler = None
self._longPressHandler = None
self._isTimeout = False
def _onPinIrq(self, _):
self._pin.irq(handler=None)
self._timer.callback(None)
self._timer.deinit()
#debounce signal
sleep_ms(50)
if self._pin.value() == (1 if self._lowOnPress else 0):
if not self._isTimeout:
schedule(self._shortPressHandler, self)
else:
self._isTimeout = False
self._timer.init(freq=1000 / self._thresholdTime,
callback=self._onTimeout)
self._pin.irq(handler=self._onPinIrq)
def _onTimeout(self, t):
t.deinit()
self._isTimeout = True
schedule(self._longPressHandler, self)
def setShortPressHandler(self, handler):
'''
Sets the handler for short press
'''
self._shortPressHandler = handler
return self
def setLongPressHandler(self, handler):
'''
Sets the handler for long press
'''
self._longPressHandler = handler
return self
def cleanup(self):
'''
Releases resources
Deinits the timer and removes handler for the pin's IRQ
'''
self._timer.deinit()
self._pin.irq(handler=None)
