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
-------------------------------------------------------
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