def main():
rospy.init_node("imu")
port = rospy.get_param("~port", "GPG3_AD1")
sensor = InertialMeasurementUnit(bus=port)
pub_imu = rospy.Publisher("~imu", Imu, queue_size=10)
pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10)
pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10)
br = TransformBroadcaster()
msg_imu = Imu()
msg_temp = Temperature()
msg_magn = MagneticField()
hdr = Header(stamp=rospy.Time.now(), frame_id="IMU")
rate = rospy.Rate(rospy.get_param('~hz', 30))
while not rospy.is_shutdown():
q = sensor.read_quaternion() # x,y,z,w
mag = sensor.read_magnetometer() # micro Tesla (µT)
gyro = sensor.read_gyroscope() # deg/second
accel = sensor.read_accelerometer() # m/s²
temp = sensor.read_temperature() # °C
msg_imu.header = hdr
hdr.stamp = rospy.Time.now()
msg_temp.header = hdr
msg_temp.temperature = temp
pub_temp.publish(msg_temp)
msg_imu.header = hdr
msg_imu.linear_acceleration.x = accel[0]
msg_imu.linear_acceleration.y = accel[1]
msg_imu.linear_acceleration.z = accel[2]
msg_imu.angular_velocity.x = math.radians(gyro[0])
msg_imu.angular_velocity.y = math.radians(gyro[1])
msg_imu.angular_velocity.z = math.radians(gyro[2])
msg_imu.orientation.w = q[3]
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
pub_imu.publish(msg_imu)
msg_magn.header = hdr
msg_magn.magnetic_field.x = mag[0] * 1e-6
msg_magn.magnetic_field.y = mag[1] * 1e-6
msg_magn.magnetic_field.z = mag[2] * 1e-6
pub_magn.publish(msg_magn)
transform = TransformStamped(header=Header(stamp=rospy.Time.now(),
frame_id="world"),
child_frame_id="IMU")
transform.transform.rotation = msg_imu.orientation
br.sendTransformMessage(transform)
rate.sleep()
def configure_sensors(self):
bp = self.BP
self.config = {
} #create a dictionary that represents if the sensor is Configured
#set up colour sensor
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config['colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
self.config[
'colour'] = SensorStatus.DISABLED #SensorStatus.DISABLED
#set up ultrasonic
try:
bp.set_sensor_type(self.ultra, bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(1.5)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
self.config['ultra'] = SensorStatus.DISABLED
#set up thermal
try:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
self.__start_thermal_infrared_thread()
except Exception as error:
self.log("Thermal Sensor not found")
self.config['thermal'] = SensorStatus.DISABLED
#set up imu
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU sensor not found")
self.config['imu'] = SensorStatus.DISABLED
bp.set_motor_limits(self.mediummotor, 100,
600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
class MrA(gopigo3.GoPiGo3):
def __init__(self):
super().__init__()
self.imu = InertialMeasurementUnit(bus="GPG3_AD1")
self.ds = EasyDistanceSensor()
def dist(self):
num = self.ds.read()
print("distance from object: {} mm".format(num))
return num
def angle(self):
num = self.imu.read_euler()[0]
print("currently facing angle: {} deg".format(num))
return num
def __init__(self, lookback, calib_it):
self.imu = InertialMeasurementUnit()
self.imu.BNO055.get_calibration_status()
self.imu.BNO055.get_calibration()
sat = time.time()
calib = []
gyro_calib = []
for _ in range(calib_it):
calib.append(self.imu.read_linear_acceleration())
gyro_calib.append(self.imu.read_gyroscope())
eat = time.time()
self.dat = (eat - sat) / calib_it
self.calib = np.average(calib, axis=0)
print(self.calib)
self.gyro_calib = np.average(gyro_calib, axis=0)
print(self.gyro_calib)
self.lookback = lookback
self.__v = 0.0
def __init__(self):
self.imu = InertialMeasurementUnit(bus = "RPI_1") #RPI_1 GPG3_AD1
import time
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="GPG3_AD1")
print("Hello!")
while True:
gyro = imu.read_gyroscope()
accel = imu.read_accelerometer()
string_to_print = "Gyro: X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Accel: X: {:.1f} Y: {:.1f} Z: {:.1f} " .format(gyro[0], gyro[1], gyro[2],
accel[0],accel[1],accel[2])
print(string_to_print)
time.sleep(.1)
#!/usr/bin/env python
from __future__ import print_function
from __future__ import division
import math
import numpy as np
import time
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="RPI_1") #RPI_1 GPG3_AD1
x = []
y = []
z = []
corr_x = []
corr_y = []
corr_z = []
i = 0
j = 0
while j < 10:
mag = imu.read_magnetometer()
x.append(mag[0])
y.append(mag[1])
z.append(mag[2])
j += 1
del x[0]
del y[0]
del z[0]
while i < 1500:
mag = imu.read_magnetometer()
def __init__(self):
super().__init__()
self.imu = InertialMeasurementUnit(bus="GPG3_AD1")
self.ds = EasyDistanceSensor()
Make continuous readings from the sensors and begin
a take measurements function.
"""
from __future__ import print_function
from __future__ import division
import time
import pickle
import math as math
import numpy as np
from datetime import datetime, timedelta
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
# Setup Sensors
imu = InertialMeasurementUnit(bus="GPG3_AD1")
gpg = EasyGoPiGo3()
gpg.reset_encoders()
def print_reading():
mag = imu.read_magnetometer()
gyro = imu.read_gyroscope()
euler = imu.read_euler()
accel = imu.read_accelerometer()
temp = imu.read_temperature()
encoder = gpg.read_encoders()
string_to_print = "Magnetometer X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Gyroscope X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Accelerometer X: {:.1f} Y: {:.1f} Z: {:.1f} " \
def configure_sensors(self,
motorports,
sensorports={
'thermal': None,
'colour': None,
'ultra': None,
'imu': 0
}):
bp = self.BP
self.thermal_thread = None
self.rightmotor = motorports['rightmotor']
self.leftmotor = motorports['leftmotor']
self.largemotors = motorports['rightmotor'] + motorports['leftmotor']
self.mediummotor = motorports['mediummotor']
#set up thermal - thermal sensor uses a thread because else it disables motors
self.thermal = sensorports['thermal']
self.config['thermal'] = SensorStatus.DISABLED
if self.thermal:
try:
if self.thermal_thread:
self.CurrentCommand = 'exit'
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
self.__start_thermal_infrared_thread()
except Exception as error:
self.log("Thermal Sensor not found")
#configure colour sensor
self.colour = sensorports['colour']
self.config['colour'] = SensorStatus.DISABLED
if self.colour:
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config[
'colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
#set up ultrasonic
self.ultra = sensorports['ultra']
self.config['ultra'] = SensorStatus.DISABLED
if self.ultra:
try:
bp.set_sensor_type(self.ultra,
bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(2)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
#set up imu
self.imu = sensorports['imu']
self.config['imu'] = SensorStatus.DISABLED
if self.imu:
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU sensor not found")
self.config['imu'] = SensorStatus.DISABLED
bp.set_motor_limits(self.mediummotor, 100,
600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
import time
import numpy as np
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="GPG3_AD1")
gpg = EasyGoPiGo3()
a = True
told = time.time()
rotratevec = np.array([])
avec = np.array([])
tvec = np.array([])
gyroold = 0
speed = 90
goal = 360
gpg.set_speed(speed)
while a == True:
gpg.left()
t = time.time() - told
gyro = imu.read_gyroscope()
rotationy = gyro[2]
if rotationy == gyroold:
gyroold = rotationy
angle = 0
#told = time.time()
else:
#rotationy = gyro[1] #Rotation in y direction (vertical)
t = time.time() - told
rotratevec = np.append(rotratevec, rotationy)
tvec = np.append(tvec, t)
angle = np.trapz(rotratevec, tvec)
class sensor:
def __init__(self, lookback, calib_it):
self.imu = InertialMeasurementUnit()
self.imu.BNO055.get_calibration_status()
self.imu.BNO055.get_calibration()
sat = time.time()
calib = []
gyro_calib = []
for _ in range(calib_it):
calib.append(self.imu.read_linear_acceleration())
gyro_calib.append(self.imu.read_gyroscope())
eat = time.time()
self.dat = (eat - sat) / calib_it
self.calib = np.average(calib, axis=0)
print(self.calib)
self.gyro_calib = np.average(gyro_calib, axis=0)
print(self.gyro_calib)
self.lookback = lookback
self.__v = 0.0
def loop(self):
acc = []
gyro = []
lookback = self.lookback
c = 0
st = time.time()
while True:
try:
et = time.time()
dt = et - st
st = time.time()
ax, ay, az = self.imu.read_linear_acceleration()
gx, gy, gz = self.imu.read_gyroscope()
if c > lookback:
acc[:-1] = acc[1:]
acc[-1] = [ax, ay, az]
gyro[:-1] = gyro[1:]
gyro[-1] = [gx, gy, gz]
avacc = np.average(acc, axis=0)
avgy = np.average(gyro, axis=0)
self.__v = 0.99 * (self.__v + avacc[0] * dt)
if np.abs(avgy[2] - self.gyro_calib[2]) > 0.1:
self.__v += 0.001 * avacc[1] / (avgy[2] - self.gyro_calib[2])
# x = 1/2* avacc[0] * (dt*dt) + self.pos[0] + self.__v[0]
# y = 1/2* avacc[1] * (dt*dt) + self.pos[1] + self.__v[1]
# z = 1/2* avacc[2] * (dt*dt) + self.pos[2] + self.__v[2]
# self.pos = [x, y, z]
# print(pos)
else:
acc.append([ax, ay, az])
gyro.append([gx, gy, gz])
c += 1
except Exception as e:
print(e)
def get_vel(self):
return self.__v
def configure_sensors(self, motorports, sensorports):
bp = self.BP
self.thread_running = False #end thread if its still running
self.rightmotor = motorports['rightmotor']
self.leftmotor = motorports['leftmotor']
self.largemotors = motorports['rightmotor'] + motorports['leftmotor']
self.mediummotor = motorports['mediummotor']
#set up thermal - thermal sensor uses a thread because else it disables motors
self.config['thermal'] = SensorStatus.DISABLED; self.thermal = None
if 'thermal' in sensorports:
self.thermal = sensorports['thermal']
if self.thermal != None:
try:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
except Exception as error:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.NONE)
if self.config['thermal'] == SensorStatus.ENABLED:
self.get_thermal_sensor() #do one read
if self.config['thermal'] < SensorStatus.DISABLED:
print("STARTING THERMAL THREAD")
self.__start_thermal_infrared_thread() #thread is started
else:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.NONE)
#setup colour sensor
self.config['colour'] = SensorStatus.DISABLED; self.colour = None
if 'colour' in sensorports:
self.colour = sensorports['colour']
if self.colour:
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config['colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
#set up ultrasonic
self.config['ultra'] = SensorStatus.DISABLED; self.ultra = None
if 'ultra' in sensorports:
self.ultra = sensorports['ultra']
if self.ultra:
try:
bp.set_sensor_type(self.ultra, bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(2)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
#set up imu
self.config['imu'] = SensorStatus.DISABLED; self.imu = None
if 'imu' in sensorports:
self.imu = sensorports['imu']
if self.imu:
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.config['imu'] = SensorStatus.DISABLED
self.log("IMU sensor not found")
bp.set_motor_limits(self.mediummotor, 100, 600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
from drive.control import drive_home, return_to_point
from drive.routes import drive_inst_1, drive_inst_2
### imports for plotting
import math as math
import matplotlib.pyplot as plt
import pandas as pd
import numpy as np
# Setup Manual Inputs (HARD CODES)
test_drive_instr = drive_inst_1
attempt_return = False
saving_data = False
draw_path = True
# Setup Sensors
imu = InertialMeasurementUnit(bus="GPG3_AD1")
gpg = EasyGoPiGo3()
gpg.reset_encoders()
atexit.register(gpg.stop)
drive_process = multiprocessing.Process(name='drive', target=test_drive_instr)
def get_position(right_prev, left_prev, euler_x_prev, theta_prev):
euler = imu.read_euler()
euler_x = euler[0]
encoder = gpg.read_encoders()
left_enc = encoder[0]
right_enc = encoder[1]
Euler z:
Thermometer: Temperature in Celcius
Left Encoder: Odometer of left wheel
Right Encoder: Odometer of right wheel
"""
from __future__ import print_function
from __future__ import division
import time
import pickle
from datetime import datetime, timedelta
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
# Setup Sensors
imu = InertialMeasurementUnit(bus = "GPG3_AD1")
gpg = EasyGoPiGo3()
gpg.reset_encoders()
def print_reading():
try:
mag = imu.read_magnetometer()
gyro = imu.read_gyroscope()
euler = imu.read_euler()
accel = imu.read_accelerometer()
temp = imu.read_temperature()
encoder = gpg.read_encoders()
quat = imu.read_quaternion()
lin_accel = imu.read_linear_acceleration()
now = datetime.now()
import pickle
from datetime import datetime, timedelta
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
from di_sensors.distance_sensor import DistanceSensor
import numpy as np
import math as math
# establish communication with the DistanceSensor
ds = DistanceSensor()
# set the sensor in fast-polling-mode
ds.start_continuous()
# Setup Sensors
imu = InertialMeasurementUnit(bus = "GPG3_AD1")
gpg = EasyGoPiGo3()
gpg.reset_encoders()
def print_reading():
mag = imu.read_magnetometer()
gyro = imu.read_gyroscope()
euler = imu.read_euler()
accel = imu.read_accelerometer()
temp = imu.read_temperature()
encoder = gpg.read_encoders()
string_to_print = "Magnetometer X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Gyroscope X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Accelerometer X: {:.1f} Y: {:.1f} Z: {:.1f} " \
class BrickPiInterface():
#Initialise timelimit and logging
def __init__(self, timelimit=20, logger=logging.getLogger()):
self.logger = logger
self.CurrentCommand = "loading"
self.Configured = False #is the robot yet Configured?
self.BP = None
self.BP = brickpi3.BrickPi3() # Create an instance of the BrickPi3
self.config = {
} #create a dictionary that represents if the sensor is Configured
self.timelimit = timelimit #fail safe timelimit - motors turn off after timelimit
self.imu_status = 0
self.Calibrated = False
self.CurrentCommand = "loaded" #when the device is ready for a new instruction it
return
#------------------- Initialise Ports ---------------------------#
# motorports = {'rightmotor':bp.PORT_B, 'leftmotor':bp.PORT_C, 'mediummotor':bp.PORT_D }
# sensorports = { 'thermal':bp.PORT_2,'colour':bp.PORT_1,'ultra':bp.PORT_4,'imu':1 }
# if some ports do not exist, set as disabled
# this will take 3-4 seconds to initialise
def configure_sensors(self,
motorports,
sensorports={
'thermal': None,
'colour': None,
'ultra': None,
'imu': 0
}):
bp = self.BP
self.thermal_thread = None
self.rightmotor = motorports['rightmotor']
self.leftmotor = motorports['leftmotor']
self.largemotors = motorports['rightmotor'] + motorports['leftmotor']
self.mediummotor = motorports['mediummotor']
#set up thermal - thermal sensor uses a thread because else it disables motors
self.thermal = sensorports['thermal']
self.config['thermal'] = SensorStatus.DISABLED
if self.thermal:
try:
if self.thermal_thread:
self.CurrentCommand = 'exit'
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
self.__start_thermal_infrared_thread()
except Exception as error:
self.log("Thermal Sensor not found")
#configure colour sensor
self.colour = sensorports['colour']
self.config['colour'] = SensorStatus.DISABLED
if self.colour:
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config[
'colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
#set up ultrasonic
self.ultra = sensorports['ultra']
self.config['ultra'] = SensorStatus.DISABLED
if self.ultra:
try:
bp.set_sensor_type(self.ultra,
bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(2)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
#set up imu
self.imu = sensorports['imu']
self.config['imu'] = SensorStatus.DISABLED
if self.imu:
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU sensor not found")
self.config['imu'] = SensorStatus.DISABLED
bp.set_motor_limits(self.mediummotor, 100,
600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
#-- Start Infrared I2c Thread ---------#
def __start_thermal_infrared_thread(self):
self.thermal_thread = threading.Thread(
target=self.__update_thermal_sensor_thread, args=(1, ))
self.thermal_thread.daemon = True
self.thermal_thread.start()
return
#changes the logger
def set_log(self, logger):
self.logger = logger
return
#-----------SENSOR COMMAND----------------#
#get the current voltage - need to work out how to determine battery life
def get_battery(self):
return self.BP.get_voltage_battery()
#self.log out a complete output from the IMU sensor
def calibrate_imu(self, timelimit=20):
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return
self.stop_all() #stop everything while calibrating...
self.CurrentCommand = "calibrate_imu"
self.log("Move around the robot to calibrate the Compass Sensor...")
self.imu_status = 0
elapsed = 0
start = time.time()
timelimit = start + timelimit #maximum of 20 seconds to calibrate compass sensor
while self.imu_status != 3 and time.time() < timelimit:
newtime = time.time()
newelapsed = int(newtime - start)
if newelapsed > elapsed:
elapsed = newelapsed
self.log("Calibrating IMU. Status: " + str(self.imu_status) +
" Time: " + str(elapsed))
ifMutexAcquire(USEMUTEX)
try:
self.imu_status = self.imu.BNO055.get_calibration_status()[3]
self.config['imu'] = SensorStatus.ENABLED
time.sleep(0.01)
except Exception as error:
self.log("IMU Calibration Error: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
if self.imu_status == 3:
self.log("IMU Compass Sensor has been calibrated")
self.Calibrated = True
return True
else:
self.log("Calibration unsuccessful")
return
return
#hopefull this is an emergency reconfigure of the IMU Sensor
def reconfig_IMU(self):
ifMutexAcquire(USEMUTEX)
try:
self.imu.BNO055.i2c_bus.reconfig_bus()
time.sleep(0.1) #restabalise the sensor
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU RECONFIG HAS FAILED" + str(error))
self.config['imu'] = SensorStatus.DISABLED
finally:
ifMutexRelease(USEMUTEX)
return
#returns the compass value from the IMU sensor - note if the IMU is placed near a motor it can be affected -SEEMS TO RETURN A VALUE BETWEEN -180 and 180.
def get_compass_IMU(self):
heading = SensorStatus.NOREADING
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return heading
ifMutexAcquire(USEMUTEX)
try:
(x, y, z) = self.imu.read_magnetometer()
time.sleep(0.01)
self.config['imu'] = SensorStatus.ENABLED
heading = int(math.atan2(x, y) *
(180 / math.pi)) + MAGNETIC_DECLINATION
#make it 0 - 360 degrees
if heading < 0:
heading += 360
elif heading > 360:
heading -= 360
except Exception as error:
self.log("IMU: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return heading
#returns the absolute orientation value using euler rotations, I think this is calilbrated from the compass sensor and therefore requires calibration
def get_orientation_IMU(self):
readings = (SensorStatus.NOREADING, SensorStatus.NOREADING,
SensorStatus.NOREADING)
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return readings
ifMutexAcquire(USEMUTEX)
try:
readings = self.imu.read_euler()
time.sleep(0.01)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU Orientation: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return readings
#returns the acceleration from the IMU sensor - could be useful for detecting collisions or an involuntary stop
def get_linear_acceleration_IMU(self):
readings = (SensorStatus.NOREADING, SensorStatus.NOREADING,
SensorStatus.NOREADING)
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return readings
ifMutexAcquire(USEMUTEX)
try:
#readings = self.imu.read_accelerometer()
readings = self.imu.read_linear_acceleration()
#readings = tuple([int(i*100) for i in readings])
time.sleep(0.01)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU Acceleration: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return readings
#get the gyro sensor angle/seconds acceleration from IMU sensor
def get_gyro_sensor_IMU(self):
gyro_readings = (SensorStatus.NOREADING, SensorStatus.NOREADING,
SensorStatus.NOREADING)
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return gyro_readings
ifMutexAcquire(USEMUTEX)
try:
gyro_readings = self.imu.read_gyroscope() #degrees/s
time.sleep(0.01)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU GYRO: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return gyro_readings
#gets the temperature using the IMU sensor
def get_temperature_IMU(self):
temp = SensorStatus.NOREADING
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return temp
ifMutexAcquire(USEMUTEX)
try:
temp = self.imu.read_temperature()
time.sleep(0.01)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU Temp: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return temp
#get the ultrasonic sensor
def get_ultra_sensor(self):
distance = SensorStatus.NOREADING
if self.config['ultra'] >= SensorStatus.DISABLED or not self.Configured:
return distance
bp = self.BP
ifMutexAcquire(USEMUTEX)
try:
distance = bp.get_sensor(self.ultra)
time.sleep(0.2)
self.config['ultra'] = SensorStatus.ENABLED
except brickpi3.SensorError as error:
self.log("ULTRASONIC: " + str(error))
self.config['ultra'] += 1
finally:
ifMutexRelease(USEMUTEX)
return distance
#returns the colour current sensed - "NOREADING", "Black", "Blue", "Green", "Yellow", "Red", "White", "Brown"
def get_colour_sensor(self):
if self.config[
'colour'] >= SensorStatus.DISABLED or not self.Configured:
return "NOREADING"
bp = self.BP
value = 0
colours = [
"NOREADING", "Black", "Blue", "Green", "Yellow", "Red", "White",
"Brown"
]
ifMutexAcquire(USEMUTEX)
try:
value = bp.get_sensor(self.colour)
time.sleep(0.01)
self.config['colour'] = SensorStatus.ENABLED
except brickpi3.SensorError as error:
self.log("COLOUR: " + str(error))
self.config['colour'] += 1
finally:
ifMutexRelease(USEMUTEX)
return colours[value]
#updates the thermal sensor by making continual I2C transactions through a thread
def __update_thermal_sensor_thread(self, name):
while self.CurrentCommand != "exit":
self.update_thermal_sensor()
#print("Thread running")
return
#updates the thermal sensor by making a single I2C transaction
def update_thermal_sensor(self):
if self.config['thermal'] >= SensorStatus.DISABLED:
self.CurrentCommand = 'exit' #end thread
return
bp = self.BP
TIR_I2C_ADDR = 0x0E # TIR I2C device address
TIR_AMBIENT = 0x00 # Ambient Temp
TIR_OBJECT = 0x01 # Object Temp
TIR_SET_EMISSIVITY = 0x02
TIR_GET_EMISSIVITY = 0x03
TIR_CHK_EMISSIVITY = 0x04
TIR_RESET = 0x05
try:
bp.transact_i2c(self.thermal, TIR_I2C_ADDR, [TIR_OBJECT], 2)
time.sleep(0.01)
except Exception as error:
self.log("THERMAL UPDATE: " + str(error))
finally:
pass
return
#return the infrared temperature - if usethread=True - it uses the thread set up in init
def get_thermal_sensor(self, usethread=True):
temp = SensorStatus.NOREADING
if self.config[
'thermal'] >= SensorStatus.DISABLED or not self.Configured:
return temp
bp = self.BP
if not usethread:
self.update_thermal_sensor() #not necessary if thread is running
ifMutexAcquire(USEMUTEX)
try:
value = bp.get_sensor(self.thermal) # read the sensor values
time.sleep(0.01)
self.config['thermal'] = SensorStatus.ENABLED
temp = (float)(
(value[1] << 8) + value[0]) # join the MSB and LSB part
temp = temp * 0.02 - 0.01 # Converting to Celcius
temp = temp - 273.15
except Exception as error:
self.log("THERMAL READ: " + str(error))
self.config['thermal'] += 1
finally:
ifMutexRelease(USEMUTEX)
return float("%3.f" % temp)
#disable thermal sensor - might be needed to reenable motors (they disable for some reason when thermal sensor is active)
def disable_thermal_sensor(self):
bp = self.BP
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.NONE)
return
#--------------MOTOR COMMANDS-----------------#
#simply turns motors on, dangerous because it does not turn them off
def move_power(self, power, deviation=0):
bp = self.BP
self.CurrentCommand = "move_power"
start = time.time()
timelimit = start + self.timelimit
bp.set_motor_power(self.rightmotor, power)
bp.set_motor_power(self.leftmotor, power + deviation)
while time.time() < timelimit and self.CurrentCommand != "stop":
continue
elapsed = time.time() - start
return elapsed #return the elapsed time.
#moves for the specified time (seconds) and power - use negative power to reverse
def move_power_time(self, power, t, deviation=0):
bp = self.BP
self.CurrentCommand = "move_power_time"
timelimit = time.time() + t
bp.set_motor_power(self.rightmotor, power)
bp.set_motor_power(self.leftmotor, power + deviation)
while time.time() < timelimit and self.CurrentCommand != "stop":
continue
self.CurrentCommand = "stop"
bp.set_motor_power(self.largemotors, 0)
return
#Rotate power and time, -power to reverse
def rotate_power_time(self, power, t):
self.CurrentCommand = "rotate_power_time"
bp = self.BP
target = time.time() + t
bp.set_motor_power(self.rightmotor, -power)
bp.set_motor_power(self.leftmotor, power)
while time.time() < target and self.CurrentCommand != 'stop':
continue
bp.set_motor_power(self.largemotors, 0) #stop
self.CurrentCommand = 'stop'
return
#Rotate power
def rotate_power(self, power):
self.CurrentCommand = "rotate_power_time"
bp = self.BP
start = time.time()
target = start + self.timelimit
bp.set_motor_power(self.rightmotor, -power)
bp.set_motor_power(self.leftmotor, power)
while time.time() < target and self.CurrentCommand != 'stop':
continue
elapsed = time.time() - start
bp.set_motor_power(self.largemotors, 0) #stop
self.CurrentCommand = 'stop'
return elapsed #returns the elapsed time after stop has been called
#Rotates the robot with power and degrees using the IMU sensor. Negative degrees = left.
#the larger the number of degrees and the lower the power, the more accurate
def rotate_power_degrees_IMU(self, power, degrees, marginoferror=3):
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return
self.CurrentCommand = "rotate_power_degrees_IMU"
bp = self.BP
symbol = '<'
limit = 0
if degrees == 0:
return
elif degrees < 0:
symbol = '>='
limit = degrees + marginoferror
else:
symbol = '<='
limit = degrees - marginoferror
power = -power
totaldegreesrotated = 0
lastrun = 0
elapsedtime = 0
starttime = time.time()
timelimit = starttime + self.timelimit
self.log("target degrees: " + str(degrees))
self.log(str(totaldegreesrotated) + str(symbol) + str(limit))
while eval("totaldegreesrotated" + str(symbol) +
"limit") and (self.CurrentCommand != "stop") and (
time.time() < timelimit
) and self.config['imu'] < SensorStatus.DISABLED:
lastrun = time.time()
bp.set_motor_power(self.rightmotor, power)
bp.set_motor_power(self.leftmotor, -power)
self.log("Total degrees rotated: " + str(totaldegreesrotated))
gyrospeed = self.get_gyro_sensor_IMU()[2] #roate around z-axis
totaldegreesrotated += (time.time() - lastrun) * gyrospeed
self.CurrentCommand = "stop"
bp.set_motor_power(self.largemotors, 0) #stop
elapsedtime = time.time() - starttime
return elapsedtime
#rotates the robot until faces targetheading - only works for a heading between 0 - 360
def rotate_power_heading_IMU(self, power, targetheading, marginoferror=3):
if self.config['imu'] >= SensorStatus.DISABLED or not self.Configured:
return
bp = self.BP
self.CurrentCommand = "rotate_power_heading"
if targetheading < 0:
targetheading += 360
elif targetheading > 360:
targetheading -= 360
heading = self.get_compass_IMU()
if heading == targetheading:
return
symbol = '<'
limit = 0
if heading < targetheading:
symbol = '<='
limit = targetheading - marginoferror
power = -power
else:
symbol = '>='
limit = targetheading + marginoferror
expression = 'heading' + symbol + 'limit'
self.log('heading' + symbol + str(limit))
elapsedtime = 0
starttime = time.time()
timelimit = starttime + self.timelimit
#start rotating until heading is reached
while (eval(expression) and
(self.CurrentCommand != "stop") and time.time() < timelimit
) and self.config['imu'] < SensorStatus.DISABLED:
bp.set_motor_power(self.rightmotor, -power)
bp.set_motor_power(self.leftmotor, power)
heading = self.get_compass_IMU()
self.log("Current heading: " + str(heading))
self.CurrentCommand = "stop"
bp.set_motor_power(self.largemotors, 0) #stop
elapsedtime = time.time() - starttime
return elapsedtime
#spins the medium motor - this can be used for shooter or claw
def spin_medium_motor(self, degrees):
self.CurrentCommand = "move_medium_motor"
degrees = -degrees #if negative -> reverse motor
bp = self.BP
if degrees == 0:
return
bp.offset_motor_encoder(self.mediummotor,
bp.get_motor_encoder(
self.mediummotor)) #reset encoder
limit = 0
symbol = '<'
currentdegrees = 0
if degrees > 0:
symbol = '<'
limit = degrees - 5
else:
symbol = '>'
limit = degrees + 5
expression = 'currentdegrees' + symbol + 'limit'
currentdegrees = bp.get_motor_encoder(self.mediummotor)
elapsedtime = 0
starttime = time.time()
timelimit = starttime + self.timelimit
while (eval(expression) and (self.CurrentCommand != "stop")
and (time.time() < timelimit)):
currentdegrees = bp.get_motor_encoder(
self.mediummotor) #where is the current angle
bp.set_motor_position(self.mediummotor, degrees)
currentdegrees = bp.get_motor_encoder(
self.mediummotor) #ACCURACY PROBLEM
self.CurrentCommand = "stop"
bp.set_motor_power(self.mediummotor, 0)
elapsedtime = time.time() - starttime
return elapsedtime
#log out whatever !!!!!THIS IS NOT WORKING UNLESS FLASK LOG USED, DONT KNOW WHY!!!!!
def log(self, message):
self.logger.info(message)
return
#stop all motors and set command to stop
def stop_all(self):
bp = self.BP
bp.set_motor_power(self.largemotors + self.mediummotor, 0)
self.CurrentCommand = "stop"
return
#returns the current command
def get_current_command(self):
return self.CurrentCommand
#returns a dictionary of all current sensors
def get_all_sensors(self):
sensordict = {} #create a dictionary for the sensors
sensordict['battery'] = self.get_battery()
sensordict['colour'] = self.get_colour_sensor()
sensordict['ultrasonic'] = self.get_ultra_sensor()
sensordict['thermal'] = self.get_thermal_sensor()
sensordict['acceleration'] = self.get_linear_acceleration_IMU()
sensordict['compass'] = self.get_compass_IMU()
sensordict['gyro'] = self.get_gyro_sensor_IMU()
sensordict['temperature'] = self.get_temperature_IMU()
sensordict['orientation'] = self.get_orientation_IMU()
return sensordict
#---EXIT--------------#
# call this function to turn off the motors and exit safely.
def safe_exit(self):
bp = self.BP
self.CurrentCommand = 'exit' #should exit thread but just incase
self.stop_all() #stop all motors
time.sleep(1)
self.disable_thermal_sensor()
self.log("Exiting")
bp.reset_all() # Unconfigure the sensors, disable the motors
time.sleep(2) #gives time to reset??
return
# Released under the MIT license (http://choosealicense.com/licenses/mit/).
# For more information see https://github.com/DexterInd/DI_Sensors/blob/master/LICENSE.md
#
# Python example program for the Dexter Industries IMU Sensor
from __future__ import print_function
from __future__ import division
import time
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
print(
"Example program for reading a Dexter Industries IMU Sensor on a GoPiGo3 AD1 port."
)
imu = InertialMeasurementUnit(bus="GPG3_AD1")
while True:
# Read the magnetometer, gyroscope, accelerometer, euler, and temperature values
mag = imu.read_magnetometer()
gyro = imu.read_gyroscope()
accel = imu.read_accelerometer()
euler = imu.read_euler()
temp = imu.read_temperature()
string_to_print = "Magnetometer X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Gyroscope X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Accelerometer X: {:.1f} Y: {:.1f} Z: {:.1f} " \
"Euler Heading: {:.1f} Roll: {:.1f} Pitch: {:.1f} " \
"Temperature: {:.1f}C".format(mag[0], mag[1], mag[2],
gyro[0], gyro[1], gyro[2],
import time
import numpy as np
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="GPG3_AD1")
print("This is the second test")
gpg = EasyGoPiGo3()
go = True
tstart = time.time()
n = 0
fdist = 1
tvec = []
tvec.append(0)
xddotvec = []
inac = imu.read_linear_acceleration()
inacx = inac[0]
#oldacx = inacx
xddotvec.append(inacx)
xdotvec = []
tvvec = []
tvvec.append(0)
xdotvec.append(0)
xvec = []
xvec.append(0)
while go == True:
n = n + 1
#print(t)
gpg.forward()
import time
import numpy as np
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="GPG3_AD1")
gpg = EasyGoPiGo3()
a = True
tinit = time.time()
xddotvec = np.array([])
xdotvec = np.array([])
xvec = np.array([])
tvec = np.array([])
speed = 150
gpg.set_speed(speed)
aold = 0
dist = 0
while a == True:
gpg.forward()
t = time.time() - tinit
accel = imu.read_linear_acceleration()
accelx = accel[1]
if accelx == aold:
aold = accelx
else:
t = time.time() - tinit
xddotvec = np.append(xddotvec, accelx)
tvec = np.append(tvec, t)
vel = np.trapz(xddotvec, tvec)
xdotvec = np.append(xdotvec, vel)