def start(self):
print('warming up MPU...')
xyz = mpu9250.read()['tb']
while np.sum(xyz) == 0:
print('MPU is not ready')
time.sleep(0.10)
for n in range(3):
print('sample reading from MPU_9250', mpu9250.read()['tb'])
def move(self, action_vector):
t_0 = time.time()
t_stop = t_0 + 0.21
t_sensing = t_0 + 0.20 # was .25
t1 = t_0
max_accel = [0,0,0]
min_accel = [0,0,0]
max_gyro = [0,0,0]
min_gyro = [0,0,0]
motor.motor1.set(action_vector[0] * 0.35)
motor.motor2.set(action_vector[1] * 0.36)
count = 0
while t1 <= t_stop:
if t1 <= t_sensing:
data = mpu9250.read()
max_accel = np.maximum(max_accel, data['accel'])
min_accel = np.minimum(min_accel, data['accel'])
max_gyro = np.maximum(max_gyro, data['gyro'])
min_gyro = np.minimum(min_gyro, data['gyro'])
count += 1
t1 = time.time()
motor.motor1.set(action_vector[0] * 0.00)
motor.motor2.set(action_vector[1] * 0.00)
if max_gyro[Z] > 150.0:
return 'TL'
elif min_gyro[Z] < -150.0:
return 'TR'
elif max_accel[Y] > abs(min_accel[Y]):
return 'F'
else:
return 'R'
def test2():
N = 200
try:
# with dmp, no magnetometer
mpu9250.initialize(enable_magnetometer = False, enable_dmp = True)
conf = mpu9250.get()
assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': True, 'enable_magnetometer': False, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98}
print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Temp (C)')
for i in range(N):
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |')
.format(data['accel'],
data['gyro']),
end='')
# with dmp, with magnetometer
mpu9250.initialize(enable_magnetometer = True, enable_dmp = True)
conf = mpu9250.get()
assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': True, 'enable_magnetometer': True, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98}
print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Mag Field XYZ(uT) | Temp (C)')
for i in range(N):
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |')
.format(data['accel'],
data['gyro'],
data['mag']),
end='')
except (KeyboardInterrupt, SystemExit):
pass
finally:
mpu9250.power_off()
def train():
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_magnetometer=False)
while True:
data = mpu9250.read()
print(data['gyro'][0], data['gyro'][1], data['gyro'][2])
time.sleep(0.1)
def read(self):
#print('> read')
if self.enabled:
# Read imu and store data (blocking call)
self.data = mpu9250.read()
# call super
return super().read()
def main():
# defaults
enable_magnetometer = False
show_compass = False
show_gyro = False
show_accel = False
show_quat = False
show_tb = False
sample_rate = 100
enable_fusion = False
show_period = False
newline = '\r'
show_tb = True
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# magnetometer ?
mpu9250.initialize(enable_dmp=True,
dmp_sample_rate=sample_rate,
enable_fusion=enable_fusion,
enable_magnetometer=enable_magnetometer)
try:
# keep running
while True:
# running
if rcpy.get_state() == rcpy.RUNNING:
t0 = time.perf_counter()
data = mpu9250.read()
t1 = time.perf_counter()
dt = t1 - t0
t0 = t1
print(data)
# print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
# .format(data['tb']), end='')
# no need to sleep
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
# say bye
print("\nBye Beaglebone!")
def read(self):
try: # keep running
while True:
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
' {3:6.1f}').format(data['accel'],
data['gyro'],
data['mag'],
temp), end='')
return data
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
print("\nBye BeagleBone!")
def get_attitude(self):
return np.multiply(mpu9250.read()['tb'], 57.29578)
mpu9250.initialize(enable_dmp=True,
dmp_sample_rate=4,
enable_magnetometer=True)
while (1):
enable_magnetometer = True
enable_fusion = True
ImuArr = []
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
if rcpy.get_state() == rcpy.RUNNING:
data = mpu9250.read()
quaternation = data['quat']
q1 = data['quat'][0]
q2 = data['quat'][1]
q3 = data['quat'][2]
q4 = data['quat'][3]
ImuArr = (q1, q2, q3, q4)
sendArr = json.dumps({"b": ImuArr})
clientSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
clientSock.sendto(sendArr.encode(), (UDP_IP_ADDRESS, UDP_PORT_NO))
if (socket.error == True):
print("Something went wrong connecting to the server!")
def get(self):
self.i += 1
xyz = np.multiply(mpu9250.read()['tb'], 57.29578) # radians to degrees
return xyz, time.time()
def main():
# exit if no options
if len(sys.argv) < 2:
usage()
sys.exit(2)
# Parse command line
try:
opts, args = getopt.getopt(sys.argv[1:], "hmpcagtqfos:", ["help"])
except getopt.GetoptError as err:
# print help information and exit:
print('rcpy_test_dmp: illegal option {}'.format(sys.argv[1:]))
usage()
sys.exit(2)
# defaults
enable_magnetometer = False
show_compass = False
show_gyro = False
show_accel = False
show_quat = False
show_tb = False
sample_rate = 100
enable_fusion = False
show_period = False
for o, a in opts:
if o in ("-h", "--help"):
usage()
sys.exit()
elif o in "-s":
sample_rate = int(a)
elif o == "-m":
enable_magnetometer = True
elif o == "-c":
show_compass = True
elif o == "-a":
show_accel = True
elif o == "-g":
show_gyro = True
elif o == "-q":
show_quat = True
elif o == "-t":
show_tb = True
elif o == "-f":
enable_fusion = True
elif o == "-p":
show_period = True
else:
assert False, "Unhandled option"
if show_compass and not enable_magnetometer:
print('rcpy_test_dmp: -c can only be used with -m ')
usage()
sys.exit(2)
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# magnetometer ?
mpu9250.initialize(enable_dmp=True,
dmp_sample_rate=sample_rate,
enable_fusion=enable_fusion,
enable_magnetometer=enable_magnetometer)
# message
print("Press Ctrl-C to exit")
# header
if show_accel:
print(" Accel XYZ (m/s^2) |", end='')
if show_gyro:
print(" Gyro XYZ (deg/s) |", end='')
if show_compass:
print(" Mag Field XYZ (uT) |", end='')
print("Head(rad)|", end='')
if show_quat:
print(" Quaternion |", end='')
if show_tb:
print(" Tait Bryan (rad) |", end='')
if show_period:
print(" Ts (ms)", end='')
print()
try:
# keep running
while True:
# running
if rcpy.get_state() == rcpy.RUNNING:
t0 = time.perf_counter()
data = mpu9250.read()
t1 = time.perf_counter()
dt = t1 - t0
t0 = t1
print('\r', end='')
if show_accel:
print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'.format(
data['accel']),
end='')
if show_gyro:
print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} |'.format(
data['gyro']),
end='')
if show_compass:
print('{0[0]:7.1f} {0[1]:7.1f} {0[2]:7.1f} |'.format(
data['mag']),
end='')
print(' {:6.2f} |'.format(data['head']), end='')
if show_quat:
print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} {0[3]:6.1f} |'.
format(data['quat']),
end='')
if show_tb:
print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'.format(
data['tb']),
end='')
if show_period:
print(' {:7.2f}'.format(1000 * dt), end='')
# no need to sleep
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
# say bye
print("\nBye Beaglebone!")
def get_data(self):
return mpu9250.read()['tb']
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_magnetometer = True)
print("Press Ctrl-C to exit")
# header
print(" Accel XYZ (m/s^2) |"
" Gyro XYZ (deg/s) |", end='')
print(" Mag Field XYZ (uT) |", end='')
print(' Temp (C)')
try: # keep running
while True:
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
' {3:6.1f}').format(data['accel'],
data['gyro'],
data['mag'],
temp),
end='')
time.sleep(.5) # sleep some
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
print("\nBye BeagleBone!")
def main():
# Parse command line
try:
opts, args = getopt.getopt(sys.argv[1:], "hm", ["help"])
except getopt.GetoptError as err:
# print help information and exit:
print('rcpy_test_imu: illegal option {}'.format(sys.argv[1:]))
usage()
sys.exit(2)
# defaults
enable_magnetometer = False
for o, a in opts:
if o in ("-h", "--help"):
usage()
sys.exit()
elif o == "-m":
enable_magnetometer = True
else:
assert False, "Unhandled option"
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# no magnetometer
mpu9250.initialize(enable_magnetometer = enable_magnetometer)
# message
print("try 'python rcpy_test_imu -h' to see other options")
print("Press Ctrl-C to exit")
# header
print(" Accel XYZ (m/s^2) |"
" Gyro XYZ (deg/s) |", end='')
if enable_magnetometer:
print(" Mag Field XYZ (uT) |", end='')
print(' Temp (C)')
try:
# keep running
while True:
# running
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read()
if enable_magnetometer:
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
' {3:6.1f}').format(data['accel'],
data['gyro'],
data['mag'],
temp),
end='')
else:
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
' {2:6.1f}').format(data['accel'],
data['gyro'],
temp),
end='')
# sleep some
time.sleep(.5)
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
# say bye
print("\nBye Beaglebone!")
def test1():
N = 1
try:
# no magnetometer
mpu9250.initialize(enable_magnetometer = False)
conf = mpu9250.get()
assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': False, 'enable_magnetometer': False, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98}
print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Temp (C)')
for i in range(N):
(ax,ay,az) = mpu9250.read_accel_data()
(gx,gy,gz) = mpu9250.read_gyro_data()
temp = mpu9250.read_imu_temp()
print(('\r{:6.2f} {:6.2f} {:6.2f} |' +
'{:6.1f} {:6.1f} {:6.1f} | {:6.1f}')
.format(ax, ay, az,
gx, gy, gz, temp),
end='')
time.sleep(1)
with pytest.raises(mpu9250.error):
mpu9250.read_mag_data()
# consolidated read function
for i in range(N):
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |')
.format(data['accel'],
data['gyro']),
end='')
time.sleep(1)
# with magnetometer
mpu9250.initialize(enable_magnetometer = True)
conf = mpu9250.get()
assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': False, 'enable_magnetometer': True, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98}
print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Mag Field XYZ(uT) | Temp (C)')
for i in range(N):
(ax,ay,az) = mpu9250.read_accel_data()
(gx,gy,gz) = mpu9250.read_gyro_data()
(mx,my,mz) = mpu9250.read_mag_data()
temp = mpu9250.read_imu_temp()
print(('\r{:6.2f} {:6.2f} {:6.2f} |' +
'{:6.1f} {:6.1f} {:6.1f} |'
'{:6.1f} {:6.1f} {:6.1f} | {:6.1f}')
.format(ax, ay, az,
gx, gy, gz,
mx, my, mz, temp),
end='')
time.sleep(1)
# consolidated read function
for i in range(N):
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |')
.format(data['accel'],
data['gyro'],
data['mag']),
end='')
time.sleep(1)
except (KeyboardInterrupt, SystemExit):
pass
finally:
mpu9250.power_off()
def getAhrs(self):
print(mpu9250.read())
def getAll():
data = mpu9250.read() # this command returns a string with many parameters.
return(data)
def step(self, action):
#file = open('capture.csv','w')
hip_action = action // 16
pos_fr = hip_action & 0b0001 != 0
pos_rr = hip_action & 0b0010 != 0
pos_rl = hip_action & 0b0100 != 0
pos_fl = hip_action & 0b1000 != 0
if pos_fr:
self.move_hip(0, 0)
else:
self.move_hip(0, 1)
if pos_rr:
self.move_hip(1, 0)
else:
self.move_hip(1, 1)
if pos_rl:
self.move_hip(2, 0)
else:
self.move_hip(2, 1)
if pos_fl:
self.move_hip(3, 0)
else:
self.move_hip(3, 1)
ankle_action = action % 16
pos_fr = ankle_action & 0b0001 != 0
pos_rr = ankle_action & 0b0010 != 0
pos_rl = ankle_action & 0b0100 != 0
pos_fl = ankle_action & 0b1000 != 0
if pos_fr:
self.move_ankle(0, 0)
else:
self.move_ankle(0, 1)
if pos_rr:
self.move_ankle(1, 0)
else:
self.move_ankle(1, 1)
if pos_rl:
self.move_ankle(2, 0)
else:
self.move_ankle(2, 1)
if pos_fl:
self.move_ankle(3, 0)
else:
self.move_ankle(3, 1)
t0 = time.time() + 0.085
dx = 0.0
dy = 0.0
n = 0
diff = 0.0
max_read = np.zeros(3)
#reading_list = []
while time.time(
) < t0: # assumes each loop takes apx same amount of time
accel = mpu9250.read()['accel']
max_read = np.maximum(accel, max_read)
#reading_list.append(accel)
diff += accel[0] - accel[1]
n += 1
time.sleep(0.25)
reward = -diff
print(-diff)
reward = 0 if reward < 45 else reward
'''
for i in reading_list:
for j in i:
file.write(str(j))
file.write(',')
file.write('\n')
file.close()
'''
done = bool(reward > 45)
return action, reward, done
def turn(angle=None, mode=None, speed=30, direction=None):
turn = True
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_dmp=True)
while turn:
if rcpy.state() == rcpy.RUNNING:
imu_data = mpu9250.read() # Read Data from Sensors
imu = imu_data[
'tb'] # Get imu Value and Convert to Degrees (0 to 180 , -180 to 0)
imu_deg = (math.degrees(round(imu[2], 2))) % 360
angle = angle % 360
if mode == "point" or mode == None:
if angle == 0:
data_l = [146, 32] # Brake
data_r = [146, 32]
# Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn
elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise
data_l = [255, 0, 128 - speed] # Rotate Left
data_r = [255, 1, 128 + speed]
elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise
data_l = [255, 0, 128 + speed] # Rotate Right
data_r = [255, 1, 128 - speed]
if (imu_deg - 2) <= angle and angle <= (
imu_deg + 2): # Once Angle is within Range Stop Motors
data_l = [146, 32] # Brake
data_r = [146, 32]
elif mode == "swing":
if direction == "forward":
if angle == 0:
data_l = [146, 32] # Brake
data_r = [146, 32]
# Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn
elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise
data_l = [255, 0, 128] # Rotate Left
data_r = [255, 1, 128 + speed]
elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise
data_l = [255, 0, 128 + speed] # Rotate Right
data_r = [255, 1, 128]
if (imu_deg - 2) <= angle and angle <= (
imu_deg + 2): # Once Angle is within Range Stop Motors
data_l = [146, 32] # Brake
data_r = [146, 32]
elif direction == "backward":
if angle == 0:
data_l = [146, 32] # Brake
data_r = [146, 32]
# Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn
elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise
data_l = [255, 0, 128 - speed] # Rotate Left
data_r = [255, 1, 128]
elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise
data_l = [255, 0, 128] # Rotate Right
data_r = [255, 1, 128 - speed]
if (imu_deg - 2) <= angle and angle <= (
imu_deg + 2): # Once Angle is within Range Stop Motors
data_l = [146, 32] # Brake
data_r = [146, 32]
else:
continue
ser_motor.write(data_l) # Send Data to Motor Controllers
ser_motor.write(data_r)
data_l = [146, 32] # Brake
data_r = [146, 32]
ser_motor.write(data_l) # Send Data to Motor Controllers
ser_motor.write(data_r)
turn = False
print("Done!")
pass
def main():
# exit if no options
if len(sys.argv) < 2:
usage()
sys.exit(2)
# Parse command line
try:
opts, args = getopt.getopt(sys.argv[1:], "hmpcagtqfos:", ["help"])
except getopt.GetoptError as err:
# print help information and exit:
print('rcpy_test_dmp: illegal option {}'.format(sys.argv[1:]))
usage()
sys.exit(2)
# defaults
enable_magnetometer = False
show_compass = False
show_gyro = False
show_accel = False
show_quat = False
show_tb = False
sample_rate = 100
enable_fusion = False
show_period = False
for o, a in opts:
if o in ("-h", "--help"):
usage()
sys.exit()
elif o in "-s":
sample_rate = int(a)
elif o == "-m":
enable_magnetometer = True
elif o == "-c":
show_compass = True
elif o == "-a":
show_accel = True
elif o == "-g":
show_gyro = True
elif o == "-q":
show_quat = True
elif o == "-t":
show_tb = True
elif o == "-f":
enable_fusion = True
elif o == "-p":
show_period = True
else:
assert False, "Unhandled option"
if show_compass and not enable_magnetometer:
print('rcpy_test_dmp: -c can only be used with -m ')
usage()
sys.exit(2)
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# magnetometer ?
mpu9250.initialize(enable_dmp = True,
dmp_sample_rate = sample_rate,
enable_fusion = enable_fusion,
enable_magnetometer = enable_magnetometer)
# message
print("Press Ctrl-C to exit")
# header
if show_accel:
print(" Accel XYZ (m/s^2) |", end='')
if show_gyro:
print(" Gyro XYZ (deg/s) |", end='')
if show_compass:
print(" Mag Field XYZ (uT) |", end='')
print("Head(rad)|", end='')
if show_quat:
print(" Quaternion |", end='')
if show_tb:
print(" Tait Bryan (rad) |", end='')
if show_period:
print(" Ts (ms)", end='')
print()
try:
# keep running
while True:
# running
if rcpy.get_state() == rcpy.RUNNING:
t0 = time.perf_counter()
data = mpu9250.read()
t1 = time.perf_counter()
dt = t1 - t0
t0 = t1
print('\r', end='')
if show_accel:
print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
.format(data['accel']), end='')
if show_gyro:
print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} |'
.format(data['gyro']), end='')
if show_compass:
print('{0[0]:7.1f} {0[1]:7.1f} {0[2]:7.1f} |'
.format(data['mag']), end='')
print(' {:6.2f} |'
.format(data['head']), end='')
if show_quat:
print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} {0[3]:6.1f} |'
.format(data['quat']), end='')
if show_tb:
print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
.format(data['tb']), end='')
if show_period:
print(' {:7.2f}'.format(1000*dt), end='')
# no need to sleep
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
# say bye
print("\nBye Beaglebone!")
