def __init__(self, i2c=None):
if i2c is None:
self.i2c = machine.I2C(sda=machine.Pin(21),
scl=machine.Pin(22),
freq=400000)
else:
self.i2c = i2c
self.imu = MPU6050(self.i2c)
# self.set_motor(0, 0)
self.imu.setGyroOffsets(-2.71, -0.01, -0.04)
self.loop_interval = time.ticks_us()
self.dt = time.ticks_us()
self.angleX = 0
self.angleX_offset = 0
self.last_angle = 0.0
self.last_wheel = 0.0
self.in_speed0 = 0
self.in_speed1 = 0
self.left = 0
self.right = 0
self.K1 = 40
self.K2 = 40
self.K3 = 6.5
self.K4 = 5.5
self.K5 = 0
self.enc_filter = 0.90
def __init__(self, i2c=None):
if i2c is None:
self.i2c = i2c_bus.get(i2c_bus.M_BUS)
else:
self.i2c = i2c
self.imu = MPU6050(self.i2c)
if self.i2c.is_ready(M5GO_WHEEL_ADDR) or self.i2c.is_ready(
M5GO_WHEEL_ADDR):
pass
else:
raise ImportError("Bala Motor not connect")
self.id = self.imu.whoami
# self.set_motor(0, 0)
self.imu.setGyroOffsets(-2.71, -0.01, -0.04)
self.loop_interval = time.ticks_us()
self.dt = time.ticks_us()
self.angleX = 0
self.angleX_offset = 0
self.last_angle = 0.0
self.last_wheel = 0.0
self.in_speed0 = 0
self.in_speed1 = 0
self.left = 0
self.right = 0
self.K1 = 40
self.K2 = 40
self.K3 = 6.5
self.K4 = 5.5
self.K5 = 0
self.enc_filter = 0.90
def __init__(self):
self.chip = MPU6050()
self.interval = 0.1
self.last_angular_speed = 0
self.angle = 0
self.round = 450.0 # change with the scale value and threshold value of gyro in MPU6050
# for communication
self.serverIP = '192.168.1.241' # IP of the server raspberryPi
self.serverPort = 8082
def initalize_robot():
oled = init_oled()
A1, A2, B1, B2, motorA, motorB = init_motors()
# Define LEDs
b_LED = LED(4)
b_LED.toggle()
# IMU connected to X9 and X10
imu = MPU6050(1, False) # Use I2C port 1 on Pyboard
return imu, oled, A1, A2, B1, B2, motorA, motorB
def gyro_start(obj):
global _pos
_pos = [0, 0]
try:
from mpu6050 import MPU6050
obj['motion'] = MPU6050(i2c, accel_sf=10)
except:
from mpu9250 import MPU9250
obj['motion'] = MPU9250(i2c)
obj['buf'] = [[0, 0] for i in range(0, 6)]
tft.rect(65, 65, 60, 60, tft.WHITE, tft.WHITE) # old pic dot clean
def gyro_start(obj):
global _pos
_pos = [0, 0]
lcd.image(0, 0, '/flash/img/3-3.jpg', type=lcd.JPG)
try:
from mpu6050 import MPU6050
obj['imu'] = MPU6050(i2c)
except:
from mpu9250 import MPU9250
obj['imu'] = MPU9250(i2c)
obj['buf'] = [[0, 0] for i in range(0, 6)]
lcd.rect(65, 65, 60, 60, lcd.WHITE, lcd.WHITE) # old pic dot clean
def main():
led = Pin(2, Pin.OUT)
i2c = I2C(scl=Pin(5), sda=Pin(4))
accelerometer = MPU6050(i2c)
accelerometer.get_values()
enabled = False
while True:
if enabled:
led.off()
else:
led.on()
utime.sleep_ms(1000)
enabled = not enabled
def mpu(req):
try:
sensor = MPU6050(0x69)
except:
rospy.logerr("error initializing mpu no %d",req.no)
rospy.logerr("sending previous values")
return mpu_valuesResponse(prev_ax[req.no],prev_ay[req.no],prev_az[req.no])
accl = sensor.get_accel_data()
rospy.loginfo(accl)
msg.mpu_no=req.no
msg.ax=prev_ax[req.no]=accl['x']
msg.ay=prev_ay[req.no]=accl['y']
msg.az=prev_az[req.no]=accl['z']
pub.publish(msg)
return mpu_valuesResponse(accl['x'],accl['y'],accl['z'])
def __init__(self, bus, gyro_address, compass_address, name, gyro_scale=MPU6050.FS_2000, accel_scale=MPU6050.AFS_16g):
self.bus = bus
self.gyro_address = gyro_address
self.name = name
self.gyro_scale = gyro_scale
self.accel_scale = accel_scale
self.gyroscope = MPU6050(bus, gyro_address, name + "-gyroscope", gyro_scale, accel_scale)
self.compass = HMC5883L(bus, compass_address, name + "-compass")
self.last_time = time.time()
self.time_diff = 0
# Take a reading as a starting point
(self.pitch, self.roll, self.gyro_scaled_x, self.gyro_scaled_y,
self.gyro_scaled_z, self.accel_scaled_x, self.accel_scaled_y,
self.accel_scaled_z) = self.gyroscope.read_all()
def __init__(self):
# Read device settings from config file
self.sensitivity_threshold = cfg.device_config['sensitivity_threshold']
self.update_frequency_delay = cfg.device_config[
'update_frequency_delay']
self.send_activation_msg = cfg.device_config['send_activation_msg']
# Initialize the mpu6050
self.mpu = MPU6050()
# Initialize Twilio API
self.sendsms = SendSMS()
if (self.send_activation_msg):
self.sendsms.send("Motion detector activated.",
self.sendsms.target_phone_number)
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.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 v1.1 --")
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)
def __init__(self,
addr=0x68,
poll_delay=0.0166,
sensor=SENSOR_MPU6050,
dlp_setting=DLP_SETTING_DISABLED):
self.sensortype = sensor
if self.sensortype == SENSOR_MPU6050:
from mpu6050 import mpu6050 as MPU6050
self.sensor = MPU6050(addr)
if (dlp_setting > 0):
self.sensor.bus.write_byte_data(self.sensor.address,
CONFIG_REGISTER, dlp_setting)
else:
from mpu9250_jmdev.registers import AK8963_ADDRESS, GFS_1000, AFS_4G, AK8963_BIT_16, AK8963_MODE_C100HZ
from mpu9250_jmdev.mpu_9250 import MPU9250
self.sensor = MPU9250(
address_ak=AK8963_ADDRESS,
address_mpu_master=addr, # In 0x68 Address
address_mpu_slave=None,
bus=1,
gfs=GFS_1000,
afs=AFS_4G,
mfs=AK8963_BIT_16,
mode=AK8963_MODE_C100HZ)
if (dlp_setting > 0):
self.sensor.writeSlave(CONFIG_REGISTER, dlp_setting)
self.sensor.calibrateMPU6500()
self.sensor.configure()
self.accel = {'x': 0., 'y': 0., 'z': 0.}
self.gyro = {'x': 0., 'y': 0., 'z': 0.}
self.mag = {'x': 0., 'y': 0., 'z': 0.}
self.temp = 0.
self.poll_delay = poll_delay
self.on = True
def __init__(self, config):
Thread.__init__(self)
self.angle = 0
self.K = 0.98
self.logDataSetBuffer = []
self.Kp = config.config.as_float('Kp')
self.Ki = config.config.as_float('Ki')
self.Kd = config.config.as_float('Kd')
self.set_point = config.config.as_float('set_point')
self.disable_motors = config.config.as_bool('disable_motors')
self.integral_error = 0
self.last_error = 0
self.motor_left = Motor("L", port_motor_left_pwm,
port_motor_left_backward,
port_motor_left_forward)
self.motor_right = Motor("R", port_motor_right_pwm,
port_motor_right_backward,
port_motor_right_forward)
self.accelerometer = MPU6050()
self.last_time = time()
self.logger = logging.getLogger(__name__)
self.setDaemon(True)
self.latest_sensor = 0
self.logger.info(
'Initialized ControlThread with the following settings')
self.logger.info('disable_motors={}'.format(self.disable_motors))
self.logger.info('set_point={:1.2f}'.format(self.set_point))
self.logger.info('Kp={:1.1f}'.format(self.Kp))
self.logger.info('Ki={:1.1f}'.format(self.Ki))
self.logger.info('Kd={:1.1f}'.format(self.Kd))
#init only one time
if not 'tft' in dir():
tft = m5stack.Display()
import machine, time
if not 'i2c' in dir():
i2c = machine.I2C(0, sda=21, scl=22)
MOTION_ID = const(104)
if MOTION_ID in i2c.scan():
print('motion sensor detected on i2cbus')
# load motion sensor logic,
# two different devices share the same ID, try and retry
try:
from mpu6050 import MPU6050
imu = MPU6050(i2c, accel_sf=10)
print("Gyro+Accelerometer/Compass MPU id: " + hex(imu.whoami))
except:
from mpu9250 import MPU9250
imu = MPU9250(i2c)
print("Gyro+Accelerometer/Compass {} id: {}".format(
imu.__class__.__name__, hex(imu.whoami)))
else:
print('No motion sensor detected')
#SENSITIVITY = 1
while 1:
x, y, z = motion.acceleration
x1 = min(abs(int(x * 10)), 100)
y1 = min(abs(int(y * 10)), 100)
z1 = min(abs(int(z * 10)), 100)
vbat = ADC(35)
vbat.atten(ADC.ATTN_11DB)
print("battery is at {} Volts".format(vbat.read() / 4095 * 3.9 * 2))
timer = Timer(0)
try:
i2c = I2C(-1, Pin(22), Pin(21), freq=100000)
except TypeError:
i2c = I2C(scl=Pin(22), sda=Pin(21),
freq=100000) #old version of micropython
limbs = limb.get_cat_limbs(i2c)
cat = Cat(limbs,
timer=timer,
mpu=MPU6050(i2c),
dac=dac,
touch_pin=t,
v_bat=vbat,
dist_sensor=sensor)
cat.meow()
cat.mpu.get_values()
cat.stand(height=7, t=2)
cat.wag(6, freq=2)
cat.yes(3)
cat.meow()
#calibration
x, y, z = 0, 0, 0
n = 10
## Robot Test 3 ## Directional Control ## import zbIrButtonMap as Buttons import ZeroBorg import time from mpu6050 import MPU6050 import smbus address = 0x68 bus = smbus.SMBus(1) ##bus = 1 sensor = MPU6050(bus, address, "MPU6050") sensor.read_raw_data() from pid import PID ZB = ZeroBorg.ZeroBorg() ZB.Init() ZB.SetCommsFailsafe(False) ZB.ResetEpo() pid = PID(0.075, 0.0, 0.0) maxPower = 0.75
import os
from pygame import mixer
from mpu6050 import MPU6050
# Initialise the mixer, load the track to play.
mixer.init()
sound = mixer.Sound('rick-roll.wav')
sound.set_volume(1)
channel = mixer.Channel(1)
channel.play(sound, -1)
channel.pause()
print "Channel initialised"
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68 # This is the address value read via the i2cdetect command
accelerometer = MPU6050(bus, address)
while True:
xRotation = accelerometer.get_x_rotation()
print "x rotation: ", xRotation
if (xRotation > 45):
channel.unpause()
time.sleep(4.2)
channel.pause()
# Poll the sensor every 0.1 seconds
time.sleep(0.1)
# 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
def read_imu(dt):
global g_pitch
alpha = 0.7 # larger = longer time constant
pitch = imu.pitch()
roll = imu.roll()
gy_dot = imu.get_gy()
gx_dot = imu.get_gx()
g_pitch = alpha * (g_pitch + gy_dot * dt * 0.001) + (1 - alpha) * pitch
#show graphics
oled.clear()
oled.line(96, 26, pitch, 24, 1)
oled.line(32, 26, g_pitch, 24, 1)
oled.draw_text(0, 0, "Raw | PITCH |")
from mpu6050 import MPU6050
import time
mpu = MPU6050(14, 13) #attach the IIC pin(sclpin,sdapin)
mpu.MPU_Init() #initialize the MPU6050
G = 9.8
time.sleep_ms(1000) #waiting for MPU6050 to work steadily
try:
while True:
accel = mpu.MPU_Get_Accelerometer() #gain the values of Acceleration
gyro = mpu.MPU_Get_Gyroscope() #gain the values of Gyroscope
print("\na/g:\t")
print(accel[0], "\t", accel[1], "\t", accel[2], "\t", gyro[0], "\t",
gyro[1], "\t", gyro[2])
print("a/g:\t")
print(accel[0] / 16384, "g", accel[1] / 16384, "g", accel[2] / 16384,
"g", gyro[0] / 131, "d/s", gyro[1] / 131, "d/s", gyro[2] / 131,
"d/s")
time.sleep_ms(1000)
except:
pass
def provide_driver_mpu6050(self):
return MPU6050(I2C(-1, scl=Pin(26, Pin.IN), sda=Pin(25, Pin.OUT)))
#init i2c bus
i2c = I2C(sda=Pin(21), scl=Pin(22), freq=100000)
#distance sensor
from vl53l0x import VL53L0X
dist = VL53L0X(i2c)
dist.start() #better performance (faster response, allows higher frequency)
# works up to ~1300 mm
# returns something >8000 if above.
for _ in range(100):
print(dist.read())
time.sleep(.1)
dist.stop() # saves power
# mpu
from mpu6050 import MPU6050
accel = MPU6050(i2c)
#print some values:
accel.val_test()
#read once:
accel_reading = accel.get_values()
#servo
#simple servo controll with PWM
#assuming servo is plugged in GPIO26 (last of the Vbat connectors)
from machine import PWM
servo = PWM(Pin(26), freq=50)
servo.duty(8) # 8% of 50 Hz = 16 milliseconds ~ about center
time.sleep(1)
for i in range(5):
np.set(0, 0xFF0000) #red
np.show()
import sh1106
import utime
from driver import SPI
from driver import GPIO
from mpu6050 import MPU6050
mpu6050Dev = MPU6050()
mpu6050Dev.open("mpu6050")
mpu6050Dev.init()
print("mpu6050 init finished")
spi0 = SPI()
spi0.open("oled_spi")
gpio_dc = GPIO()
gpio_dc.open("oled_dc")
gpio_res = GPIO()
gpio_res.open("oled_res")
display = sh1106.SH1106_SPI(width=132, height=64, spi=spi0, dc = gpio_dc, res = gpio_res)
# display.init_display()
ac = []
while(True):
ac = mpu6050Dev.get_Accelerometer()
#print("mpu6050 acc is: " , ac[0], ac[1], ac[2])
display.fill(0)
#display.fill_circle(50, 30, 20, 0xAF)
#display.draw_circle(90, 30, 20, 2, 0xAF)
display.draw_circle(66, 32, 10, 1, 1)
display.fill_circle(int(66 - ac[0] / 250), int(32 + ac[1] / 500), 8, 1)
# main.py -- put your code here! from mpu6050 import MPU6050 from pid import PID from rc import RC, _map, wrap_180 from esc import ESC # MPU mpu = MPU6050() mpu.dmpInitialize() mpu.setDMPEnabled(True) packetSize = mpu.dmpGetFIFOPacketSize() # PID rr_pid = PID(p=0.7, i=1, imax=50) pr_pid = PID(p=0.7, i=1, imax=50) yr_pid = PID(p=2.7, i=1, imax=50) rs_pid = PID(p=4.5) ps_pid = PID(p=4.5) ys_pid = PID(p=10) # RC rc_thr = RC(0) # throttle rc_rol = RC(1) # roll rc_pit = RC(2) # pitch rc_yaw = RC(3) # yaw # ESC esc_0 = ESC(0) esc_1 = ESC(1) esc_2 = ESC(2) esc_3 = ESC(3)
def setUp(self):
self.testAddress = 0x68
self.testBus = Mock()
self.testBus.read_byte_data.return_value = 0x00
self.accelerometer = MPU6050(self.testBus, self.testAddress)
oled.display()
while trigger(): pass
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)
#init only one time
if not 'tft' in dir():
tft = m5stack.Display()
import machine, time
if not 'i2c' in dir():
i2c = machine.I2C(0, sda=21, scl=22)
MOTION_ID = const(104)
if MOTION_ID in i2c.scan():
print('motion sensor detected on i2cbus')
# load motion sensor logic,
# two different devices share the same ID, try and retry
try:
from mpu6050 import MPU6050
motion = MPU6050(i2c, accel_sf=10)
print("Gyro+Accelerometer/Compass MPU id: " + hex(motion.whoami))
except:
from mpu9250 import MPU9250
motion = MPU9250(i2c)
print("Gyro+Accelerometer/Compass {} id: {}".format(
motion.__class__.__name__, hex(motion.whoami)))
else:
print('No motion sensor detected')
#shock detector
SENSITIVITY = 11
blu_pwm.duty(0)
grn_pwm.duty(0)
red_pwm.duty(0)
from m5stack import * from m5ui import * from mpu6050 import MPU6050 from mag3110 import MAG3110 import i2c_bus clear_bg(0x111111) itwoc = i2c_bus.get(i2c_bus.M_BUS) mag = MAG3110(itwoc) imu = MPU6050(itwoc) btnA = M5Button(name='ButtonA', text='ButtonA', visibility=False) btnB = M5Button(name='ButtonB', text='ButtonB', visibility=False) btnC = M5Button(name='ButtonC', text='ButtonC', visibility=False) labelx = M5TextBox(40, 30, "Text", lcd.FONT_Default, 0xFFFFFF) labely = M5TextBox(40, 50, "Text", lcd.FONT_Default, 0xFFFFFF) labelz = M5TextBox(40, 70, "Text", lcd.FONT_Default, 0xFFFFFF) labelax = M5TextBox(40, 90, "Text", lcd.FONT_Default, 0xFFFFFF) labelay = M5TextBox(40, 110, "Text", lcd.FONT_Default, 0xFFFFFF) labelaz = M5TextBox(40, 130, "Text", lcd.FONT_Default, 0xFFFFFF) labelgx = M5TextBox(40, 150, "Text", lcd.FONT_Default, 0xFFFFFF) labelgy = M5TextBox(40, 170, "Text", lcd.FONT_Default, 0xFFFFFF) labelgz = M5TextBox(40, 190, "Text", lcd.FONT_Default, 0xFFFFFF) labelmg = M5TextBox(40, 210, "Text", lcd.FONT_Default, 0xFFFFFF) lpitch = M5TextBox(5, 30, "pitch", lcd.FONT_Default, 0xFFFFFF) lroll = M5TextBox(5, 50, "roll", lcd.FONT_Default, 0xFFFFFF) lyaw = M5TextBox(5, 70, "yaw", lcd.FONT_Default, 0xFFFFFF) lax = M5TextBox(5, 90, "ax", lcd.FONT_Default, 0xFFFFFF) lay = M5TextBox(5, 110, "ay", lcd.FONT_Default, 0xFFFFFF)
import pyb
import graphics
from ssd1306 import SSD1306
lcd = SSD1306(pinout={
'sda': 'X10',
'scl': 'X9'
},
height=64,
external_vcc=False)
lcd.poweron()
lcd.init_display()
from mpu6050 import MPU6050
imu = MPU6050(2, False)
# set up stepper motors
from nemastepper import Stepper
motor1 = Stepper('Y1', 'Y2', 'Y3')
motor2 = Stepper('X4', 'X5', 'X6')
from pyb import Timer
def issr(t):
global motor1, motor2
motor1.do_step()
motor2.do_step()
