def __init__(self, pins):
if pins['a_out'] == 'X5':
self.dac = pyb.DAC(1, bits=12)
else:
self.dac = pyb.DAC(2, bits=12)
self.adc = pyb.ADC(pins['a_in'])
self.mic = pyb.ADC(pins['mic'])
self.pot = pyb.ADC(pins['pot'])
# Virtual Instrument Parameters at start up
self.sig_freq = 1000.0 # sinewave frequency
self.dc_v = 2020 # DC voltage level
self.max_v = 4095 # maximum voltage
self.min_v = 0 # minimum voltage
self.N_samp = 256 # number of sample in one cycle to generate
self.samp_freq = 100
self.buf_size = 4096
self.N_window = 1000
self.duty_cycle = 90
self.function = "Idle"
self.buf_max = 8192
self.motor_direction = 0 # forward
self.dac.write(0)
# Initialise OLED display
self.test_dev = pyb.I2C('Y',pyb.I2C.MASTER)
if (not self.test_dev.scan()):
self.oled = None
else:
self.oled = OLED_938(pinout={'sda': 'Y10', 'scl': 'Y9', 'res': 'Y8'},
height=64,
external_vcc=False, i2c_devid=61)
self.oled.poweron()
self.oled.init_display()
self.oled.draw_text(0, 0, "-- PyBench v2.0 --")
self.oled.draw_text(30, 40, "** READY **")
self.oled.display()
# Initialise IMU - connected to I2C(1) - add handling missing IMU later
self.imu = MPU6050(1, False)
# create motor class to drive motors
self.motor = DRIVE()
while not trigger(): # Wait to tune Ki
time.sleep(0.001)
K_i = pot.read() * scale2 / 4095 # Use pot to set Ki
oled.draw_text(0, 40, 'Ki = {:5.5f}'.format(K_i)) # Display live value on oled
oled.display()
while trigger(): pass
while not trigger(): # Wait to tune Kd
time.sleep(0.001)
K_d = pot.read() * scale2 / 4095 # Use pot to set Ki
oled.draw_text(0, 50, 'Kd = {:5.5f}'.format(K_d)) # Display live value on oled
oled.display()
while trigger(): pass
imu = MPU6050(1, False)
motor = DRIVE()
# Pitch angle calculation using complementary filter
def pitch_estimation(pitch, dt, alpha):
theta = imu.pitch()
pitch_dot = imu.get_gy()
pitch = alpha*(pitch + pitch_dot*dt/1000000) + (1-alpha)*theta
# print(pitch_dot)
print("filtered = " + str(pitch))
print("imu = " + str(theta))
pyb.delay(2000)
return (pitch, pitch_dot)
#K_p = 5.41
ptr = 0 # sample buffer index pointer
buffer_full = False # semaphore - ISR communicate with main program
def flash(): # routine to flash blue LED when beat detected
b_LED.on()
pyb.delay(30)
b_LED.off()
motor = DRIVE()
def StepText(filename):
char_list = [ch for ch in open(filename).read() if ch not in ['\r', '\n']]
return char_list
def StepFunctions(steps, i, v):
step = steps[i]
if step == 'A':
print('Spin: wheels in opposite direction')
motor.right_forward(v)
motor.left_back(v)
elif step == 'B':
print('right 45')
motor.right_forward(v / 3)
buffer_full = False # semaphore - ISR communicate with main program
def flash(): # routine to flash blue LED when beat detected
b_LED.on()
pyb.delay(30)
b_LED.off()
'''Functions for reading .txt file and corresponding motor controlled dance moves'''
motor = DRIVE()
def StepText(filename):
char_list = [ch for ch in open(filename).read() if ch not in ['\r', '\n']]
return char_list
def StepFunctions(steps, i, v):
step = steps[i]
if step == 'A':
print('Spin: wheels in opposite direction')
motor.right_forward(v)
motor.left_back(v)
elif step == 'B':
from mpu6050 import MPU6050
from motor import DRIVE
import micropython
micropython.alloc_emergency_exception_buf(100)
pot = ADC(Pin('X11'))
oled = OLED_938(pinout={'sda': 'Y10', 'scl': 'Y9', 'res': 'Y8'}, height=64, external_vcc=False, i2c_devid=61)
oled.poweron()
oled.init_display()
imu = MPU6050(1, False)
motor = DRIVE()
# PID tuning
'''
trigger = pyb.Switch()
scale = 2.0
while not trigger(): # Wait to tune Kp
time.sleep(0.001)
K_p = pot.read() * scale / 4095 # Use potentiometer to set Kp
oled.draw_text(0, 40, 'Kp = {:5.2f}'.format(K_p)) # Display live value on oled
oled.display
while trigger(): pass
while not trigger(): # Wait to tune Ki
time.sleep(0.001)
K_i = pot.read() * scale / 4095 # Use pot to set Ki
oled.draw_text(0, 50, 'Ki: {:5.2f}'.format(K_i)) # Display live value on oled)
-------------------------------------------------------
Name: Drive with bluetooth control
Creator: Aida Manzano
Date: 20 March 2019
-------------------------------------------------------
This program does the following:
1. Define a number of dance moves / motor movements
2. Connect to bluetooth
3. Trigger dancemoves on in-app button press on mobile device
'''
import pyb
from pyb import Pin, Timer, UART
from motor import DRIVE
d = DRIVE()
#initialise UART communication
uart = UART(6)
uart.init(9600, bits=8, parity=None, stop=2)
mode = 0
def dancemoves(move, pwm):
if move == 'f':
d.right_forward(pwm)
d.left_forward(pwm)
#self.duration = 2
elif move == 'b':
d.right_back(pwm)
d.left_back(pwm)
#self.duration = 2
class PYBENCH(object):
def __init__(self, pins):
if pins['a_out'] == 'X5':
self.dac = pyb.DAC(1, bits=12)
else:
self.dac = pyb.DAC(2, bits=12)
self.adc = pyb.ADC(pins['a_in'])
self.mic = pyb.ADC(pins['mic'])
self.pot = pyb.ADC(pins['pot'])
# Virtual Instrument Parameters at start up
self.sig_freq = 1000.0 # sinewave frequency
self.dc_v = 2020 # DC voltage level
self.max_v = 4095 # maximum voltage
self.min_v = 0 # minimum voltage
self.N_samp = 256 # number of sample in one cycle to generate
self.samp_freq = 100
self.buf_size = 4096
self.N_window = 1000
self.duty_cycle = 90
self.function = "Idle"
self.buf_max = 8192
self.motor_direction = 0 # forward
self.dac.write(0)
# Initialise OLED display
self.test_dev = pyb.I2C('Y',pyb.I2C.MASTER)
if (not self.test_dev.scan()):
self.oled = None
else:
self.oled = OLED_938(pinout={'sda': 'Y10', 'scl': 'Y9', 'res': 'Y8'},
height=64,
external_vcc=False, i2c_devid=61)
self.oled.poweron()
self.oled.init_display()
self.oled.draw_text(0, 0, "-- PyBench v2.0 --")
self.oled.draw_text(30, 40, "** READY **")
self.oled.display()
# Initialise IMU - connected to I2C(1) - add handling missing IMU later
self.imu = MPU6050(1, False)
# create motor class to drive motors
self.motor = DRIVE()
def display_present(self):
if (not self.oled):
return False
else:
return True
def volts (self,value):
return (value*3.3/4096)
def put(self, value):
self.function = "DC"
self.dc_v = value
self.dac.write(value)
def out(self, value):
self.dc_v = value
self.dac.write(value)
def put_sine(self):
self.function = "AC"
if (self.display_present()):
s = "AC %4.2fV" % self.volts(self.min_v) + " - %4.2fV" % self.volts(self.max_v)
self.oled.draw_text (0, 10, s)
self.oled.draw_text (10, 20, "Fsig : %5.1fHz" % self.sig_freq)
self.oled.draw_text (10, 30, "Fsamp: %5.1fHz" % self.samp_freq)
self.oled.display()
pyb.delay(500)
# Create the samples in sine array
self.dac.write(0) # Quirk of Pyboard - need to write to DAC once first
sine_array = array('H', 0 for i in range(self.N_samp))
mid_v = (self.max_v + self.min_v)/2
amp_v = (self.max_v - self.min_v)/2
for i in range(self.N_samp):
sine_array[i] = int(mid_v
+ amp_v*math.sin(2*math.pi*i/self.N_samp))
# Generate the sinewave
self.dac.write_timed(sine_array,
pyb.Timer(2, freq=int(self.N_samp*self.sig_freq)), mode=DAC.CIRCULAR)
def put_triangle(self):
self.function = "TR"
if (self.display_present()):
s = "TR %4.2fV" % self.volts(self.min_v) + " - %4.2fV" % self.volts(self.max_v)
self.oled.draw_text (0, 10, s)
self.oled.draw_text (10, 20, "Fsig: %5.1fHz" % self.sig_freq)
self.oled.draw_text (10, 30, "Fsamp: %5.1fHz" % self.samp_freq)
self.oled.display()
pyb.delay(150)
# Create the samples in triangle array
self.dac.write(0) # Quirk of Pyboard - need to write to DAC once first
delta_v = (self.max_v - self.min_v)/(0.5*self.N_samp)
tri_array = array('H', 0 for i in range(self.N_samp))
for i in range(int(self.N_samp/2)):
tri_array[i] = int(self.min_v + i*delta_v)
tri_array[self.N_samp-i-1] = tri_array[i]
# Generate the triangular wave
self.dac.write_timed(tri_array,
pyb.Timer(2, freq=int(self.N_samp*self.sig_freq)), mode=DAC.CIRCULAR)
def put_square(self):
self.function = "SQ"
if (self.display_present()):
s = "SQ %4.2fV" % self.volts(self.min_v) + " - %4.2fV" % self.volts(self.max_v)
self.oled.draw_text (0, 10, s)
self.oled.draw_text (10, 20, "Fsig : %5.1fHz" % self.sig_freq)
self.oled.draw_text (10, 30, "Duty_cycle: %3d" % self.duty_cycle)
self.oled.display()
pyb.delay(150)
# Create the samples in square array
self.dac.write(0) # Quirk of Pyboard - need to write to DAC once first
square_array = array('H', 0 for i in range(self.N_samp))
h_index = int(self.N_samp*self.duty_cycle/100 + 0.5)
for i in range(h_index):
square_array[i] = int(self.max_v)
for i in range (h_index, self.N_samp):
square_array[i] = int(self.min_v)
# Generate the triangular wave
self.dac.write_timed(square_array,
pyb.Timer(2, freq=int(self.N_samp*self.sig_freq)), mode=DAC.CIRCULAR)
def get(self):
return self.adc.read()
def get_mean(self,n):
reading = 0
for i in range(n):
reading = reading + self.adc.read()
return int((reading/n)+0.5)
def get_block(self,n):
# Create sample buffer
s_buf = array('H', 0 for i in range(n))
x=self.adc.read_timed(s_buf, pyb.Timer(5, freq=self.samp_freq))
return (s_buf)
def get_mic(self,n):
# Create sample buffer
s_buf = array('H', 0 for i in range(n))
x=self.mic.read_timed(s_buf, pyb.Timer(5, freq=self.samp_freq))
self.oled.draw_text (0, 20, "Get %4d MIC samples" % n)
self.oled.display()
return (s_buf)
def write_message(self,message):
h_index = min(len(message), 18) # maximum message length is 18
self.oled.draw_text(0, 53, ">> " + message[0:h_index])
self.oled.display()
pyb.delay(200)
def set_max_v(self, value):
self.max_v = min(4095, max(0, value))
def set_min_v(self, value):
self.min_v = min(4095, max(0, value))
def set_sig_freq(self, value):
self.sig_freq = min(3000.0,max(0.1,value/10.0))
def set_samp_freq(self, value):
self.samp_freq = min(30000.0, max(1.0,value))
def set_duty_cycle(self, value):
self.duty_cycle = min(100.0, max(0, value))
def set_N_window(self, value):
self.N_window = value
def get_imu(self):
accel = self.imu.get_accel_raw()
gyro = self.imu.get_gyro_raw()
return [accel, gyro]
def get_accel(self):
accel = self.imu.get_accel_raw()
return accel
def get_gyro(self):
gyro = self.imu.get_gyro_raw()
return gyro
def set_motor_direction(self, value):
self.motor_direction = value;
def set_motor_speed(self, value):
if(self.motor_direction==0):
self.motor.left_forward(value)
self.motor.right_forward(value)
else:
self.motor.left_back(value)
self.motor.right_back(value)
# Display message
self.oled.draw_text(0,20,'Driving motor{:4d}'.format(value))
self.oled.display()
def get_motor_speed(self):
s_buf = array('H', 0 for i in range(2))
speedA = self.motor.get_speedA()
speedB = self.motor.get_speedB()
s_buf[0] = speedA
s_buf[1] = speedB
return(s_buf)
# I2C connected to Y9, Y10 (I2C bus 2) and Y11 is reset low active
oled = OLED_938(pinout={
'sda': 'Y10',
'scl': 'Y9',
'res': 'Y8'
},
height=64,
external_vcc=False,
i2c_devid=61)
oled.poweron()
oled.init_display()
# IMU connected to X9 and X10
imu = MPU6050(1, False) # Use I2C port 1 on Pyboard
motor = DRIVE()
pot = ADC(Pin('X11'))
def pitch_estimate(pitch, dt, alpha):
theta = imu.pitch()
pitch_dot = imu.get_gy()
pitch = alpha * (pitch + pitch_dot * dt * 0.001) + (1 - alpha) * theta
return (pitch, pitch_dot)
trigger = pyb.Switch()
scaleP = 10.0
scaleI = 100
scaleD = 20
"""