def Prepare_To_Act(self):
self.motors = {}
for jointName in pyrosim.jointNamesToIndices:
if jointName == 'Torso_Leg_B':
self.motors[jointName] = MOTOR(jointName,
frequency=c.FREQUENCY / 2)
else:
self.motors[jointName] = MOTOR(jointName)
theta = imu.pitch()
pitch_dot = imu.get_gy()
pitch = alpha*(pitch + pitch_dot*dt) + (1-alpha)*theta
return (pitch, pitch_dot)
'''
Main program loop
'''
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
from motor import MOTOR
motor = MOTOR()
def forward(speed): # forwards
print('forward')
motor.B_back(speed)
motor.A_back(speed)
def forwardslow(speed): # slow forwards
print('forward slowly')
motor.B_back(2*speed/3)
motor.A_back(2*speed/3)
def back(speed): # backwards
print('back')
motor.A_forward(2*speed/3)
motor.B_forward(2*speed/3)
def left(speed): # left
print('left')
motor.B_back(speed)
motor.A_forward(speed / 4)
def leftslow(speed): # slow left
print('left slowly')
motor.B_back(2 * speed / 3)
as sum_energy, equivalent to 1 sec worth of signal.
4. Find the ratio c = instantenous energy/(sum_energy/50)
5. Wait for elapsed time > (beat_period - some margin)
since last detected beat
6. Check c value and if higher than BEAT_THRESHOLD,
detect as a beat and make segway perform next dance move
'''
import pyb
from pyb import Pin, Timer, ADC, DAC, LED, UART
import sys
from array import array # need this for memory allocation to buffers
from oled_938 import OLED_938 # Use OLED display driver
from motor import MOTOR # USE motor file to drive motor
Segway = MOTOR() # Import motor for dance movement
# The following two lines are needed by micropython
# ... must include if you use interrupt in your program
import micropython
micropython.alloc_emergency_exception_buf(100)
# Create timer interrupt - one every 1/8000 sec or 125 usec
pyb.disable_irq() # disable interrupt while configuring timer
# 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, 'Button pressed. Running.')
oled.display()
# IMU connected to X9 and X10
imu = MPU6050(1, False)
# Pitch angle calculation using complementary filter
def pitch_estimate(pitch, dt, alpha):
theta = imu.pitch()
pitch_dot = imu.get_gy()
pitch = alpha * (pitch + pitch_dot * dt) + (1 - alpha) * theta
return (pitch, pitch_dot)
motor = MOTOR()
'''
Main program loop
'''
pitch = 0 # initialise pitch angle to 0 to start
alpha = 0.965 # alpha value in complementary filter
error_current = 0
error_last = 0
# K_p = 5.0
K_d = 0.23 # 0.23 - .29
# K_i = 0.1
pitch_target = -1.5 # lean towards top of board
offset = 5
oled.draw_text(0, 40, 'Kd = {:5.2f}'.format(K_d))
theta = imu.pitch()
pitch_dot = imu.get_gy()
pitch = alpha * (pitch + pitch_dot * dt) + (1 - alpha) * theta
return (pitch, pitch_dot)
'''
Main program loop
'''
pitch = 0 # initialise pitch angle to 0 to start
alpha = 0.95 # alpha value in complementary filter
calibration = -1.2 # 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
try:
tic = pyb.micros()
while True:
b_LED.toggle()
dt = pyb.micros() - tic
if dt > 5000: # sampling time is 5 msec or 50Hz
pitch, pitch_dot = pitch_estimate(pitch, dt * 0.000001, alpha)
tic = pyb.micros()
u = pidc.get_pwm(pitch, pitch_dot)
def Prepare_To_Act(self):
self.motors = {}
for jointName in pyrosim.jointNamesToIndices:
self.motors[jointName] = MOTOR(jointName)
import pyb
from pyb import Pin, Timer, ADC, DAC, LED, ExtInt
import time
from array import array # need this for memory allocation to buffers
from oled_938 import OLED_938 # Use OLED display driver
from mpu6050 import MPU6050
from motor import MOTOR
import mic
import micropython
micropython.alloc_emergency_exception_buf(100)
var = False
var2 = False
pyb.disable_irq() # disable interrupt while configuring timer
Segway = MOTOR()
# Create timer interrupt - one every 1/8000 sec or 125 usec
# Use OLED to say what segway is doing
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, 0, 'Group 20')
# ----------------------------------------------------
# Using BLUEFRUIT board to control speed of motor
import pyb
from pyb import Pin, Timer, ADC, UART
from oled_938 import OLED_938
from motor import MOTOR
print('BlueFruit Test')
Segway = MOTOR()
#initialise UART communication
uart = UART(6)
uart.init(9600, bits=8, parity=None, stop=2)
# I2C connected to Y9, Y10 (I2C bus 2) and Y11 is reset low active
try:
i2c_2 = pyb.I2C(2, pyb.I2C.MASTER)
dev_list = i2c_2.scan()
oled = OLED_938(pinout={
'sda': 'Y10',
'scl': 'Y9',
'res': 'Y8'
},
height=64,
external_vcc=False,
i2c_devid=dev_list[0])
oled.poweron()
oled.init_display()
oled_ok = True
import pyb
from pyb import Pin, Timer, ADC, DAC, LED
from array import array
from oled_938 import OLED_938
#These lines are required for using interrupt
import micropython
micropython.alloc_emergency_exception_buf(100)
#####Dance Moves#####
from motor import MOTOR
motor_right = MOTOR()
motor_left = MOTOR()
def implement_move(current_dance_move):
if current_dance_move == 1:
motor_right.A_forward(50)
motor_left.B_forward(50)
elif current_dance_move == 2:
motor_right.A_back(50)
motor_left.B_back(50)
elif current_dance_move == 3: