def __init__(self, data_queue: Queue):
super().__init__()
self.receive_queue = data_queue
self.running = True
self.logger = logging.getLogger('ImuReader')
self.logger.debug('__init__')
self.imu = mpu9250.IMU(enable_dmp=True,
dmp_sample_rate=10,
enable_magnetometer=True,
enable_fusion=True)
# Place to Store Data bufferSize = 1024 # Yes, lots of space. # Set Motors Pins motor_x = 3 motor_y = 4 # Initial Motor Duty Cycles duty_x = 0 duty_y = 0 sample_rate = 100 imu = mpu9250.IMU(enable_dmp=True, dmp_sample_rate=sample_rate) # Set RCPY State to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) base_srvo = servo.Servo(base_channel) shoulder_srvo = servo.Servo(shoulder_channel) elbow_srvo = servo.Servo(elbow_channel) pitch_srvo = servo.Servo(pitch_channel) roll_srvo = servo.Servo(roll_channel) grabber_srvo = servo.Servo(grabber_channel) base_clck = clock.Clock(base_srvo, period) shoulder_clck = clock.Clock(shoulder_srvo, period) elbow_clck = clock.Clock(elbow_srvo, period)
import rcpy.clock as clock
import time
import math
def mySet(serv, duty, lastDuty):
timeToWait = 0.2 * abs(duty - lastDuty)
serv.set(duty)
serv.run()
#time.sleep(timeToWait)
COMPASS_FIX = -90
COMPASS_ERR = 5
imu = mpu9250.IMU(enable_dmp=True, dmp_sample_rate=4, enable_magnetometer=True)
serv = servo.Servo(1)
servo.enable()
clck = clock.Clock(serv, 0.01)
clck.start()
duty = 0
lastDuty = duty
mySet(serv, duty, duty)
dataArr = []
prev_heading = 0
change = 0
servo_move = 0.2
servo_min = -1.5