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