pitch = 0 # initialise pitch angle to 0 to start
alpha = 0.95 # alpha value in complementary filter
calibration = -3.6 # calibrate for centre of mass (negative: lean towards top of board)
motor_offset = 5 # remove motor deadzone
motor = MOTOR() # init motor object
pidc = PIDC(Kp=K_p, Kd=K_d, Ki=K_i, theta_0=calibration) # init PID controller object
pidc.target_reset() # set target point for self-balance as normal to ground
# Create microphone object
# define ports for microphone, LEDs and trigger out (X5)
SAMP_FREQ = 8000
N = 160
mic = MICROPHONE(Timer(7,freq=SAMP_FREQ),ADC('Y11'),N)
# Define constants for main program loop - shown in UPPERCASE
M = 50 # number of instantaneous energy epochs to sum
BEAT_THRESHOLD = 1.9 # threshold for c to indicate a beat
# initialise variables for main program loop
e_ptr = 0 # pointer to energy buffer
e_buf = array('L', 0 for i in range(M)) # reserve storage for energy buffer
sum_energy = 0 # total energy in last 50 epochs
pyb.delay(100)
tic2 = pyb.millis() # mark time now in msec (interrupt timer)
# read the characters into a list, intialise stuff
movelist = readlist('g24_choreo2.txt')
counter = 0
oled.display()
oled.clear()
# CONTROL PANNEL #
set_point = 0.51
current_pitch = 0
kp = 5.3
ki = 0.45
kd = 0.3
offset = 1
mic = ADC(Pin('Y11'))
sample_timer = pyb.Timer(7, freq=8000)
# define ports for microphone, LEDs and trigger out (X5)
micro = MICROPHONE(sample_timer, mic, 160)
b_LED = LED(4)
N = 160 # size of sample buffer s_buf[]
s_buf = array('H', 0 for i in range(N)) # reserve buffer memory
ptr = 0 # sample buffer index pointer
buffer_full = False # semaphore - ISR communicate with main program
def energy(buf): # Compute energy of signal in buffer
sum = 0
for i in range(len(buf)):
s = buf[i] - MIC_OFFSET # adjust sample to remove dc offset
sum = sum + s * s # accumulate sum of energy
return sum
micropython.alloc_emergency_exception_buf(100)
# -------- PERIPHERAL SETUP -------- #
# OLED Display
# 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()
oled.draw_text(0,20, 'Milestone 4b: Loading...')
oled.display()
# -- Microphone, 8000 Hz sample rate
N = 160
mic = MICROPHONE(Timer(7,freq=8000),ADC('Y11'),N)
# -- Blue LED
b_LED = LED(4)
# -- IMU
imu = MPU6050(1, False)
# -- WHEELS
A1 = Pin('X3', Pin.OUT_PP) # Control direction of motor A
A2 = Pin('X4', Pin.OUT_PP)
PWMA = Pin('X1') # Control speed of motor A
B2 = Pin('X7', Pin.OUT_PP) # Control direction of motor B
B1 = Pin('X8', Pin.OUT_PP)
PWMB = Pin('X2') # Control speed of motor B
oled = OLED_938(pinout={
'sda': 'Y10',
'scl': 'Y9',
'res': 'Y8'
},
height=64,
external_vcc=False,
i2c_devid=61)
oled.poweron()
oled.init_display()
oled.draw_text(0, 20, 'Milestone 3: Loading')
oled.display()
# Microphone, 125 microsecond samples
N = 160
mic = MICROPHONE(Timer(7, freq=8000), ADC('Y11'), N)
# Blue LED
b_LED = LED(4)
# ----- FLASH SCRIPT ----- #
def flash():
b_LED.on()
pyb.delay(20)
b_LED.off()
# ----- MAINLOOP CONSTANTS ----- #
M = 50 # number of instantaneous energy epochs to sum
BEAT_THRESHOLD = 1.6 # threshold for c to indicate a beat