def safe_north_point(self):
"""
Determines the heading of the north point.
This function doesn't take into account the declination.
:return: The heading of the north point measured in degrees. The north point is found at **0** degrees.
:rtype: int
.. note::
In case of an exception occurring within this method, **0** is returned.
"""
ifMutexAcquire(self.use_mutex)
try:
x, y, z = self.read_magnetometer()
except:
x, y, z = 0, 0, 0
finally:
ifMutexRelease(self.use_mutex)
# using the x and z axis because the sensor is mounted vertically
# the sensor's top face is oriented towards the front of the robot
heading = -atan2(-x, z) * 180 / pi
# adjust it to 360 degrees range
if heading < 0:
heading += 360
elif heading > 360:
heading -= 360
return heading
def __init__(self, port="I2C", led_state=False, use_mutex=False):
"""
Constructor for initializing a link to the `Light Color Sensor`_.
:param str port = "I2C": The port to which the distance sensor is connected to. Can also be connected to ports ``"AD1"`` or ``"AD2"`` of the `GoPiGo3`_. If you're passing an **invalid port**, then the sensor resorts to an ``"I2C"`` connection. Check the :ref:`hardware specs <hardware-interface-section>` for more information about the ports.
:param bool led_state = False: The LED state. If it's set to ``True``, then the LED will turn on, otherwise the LED will stay off. By default, the LED is turned off.
:param bool use_mutex = False: When using multiple threads/processes that access the same resource/device, mutexes should be enabled.
:raises ~exceptions.OSError: When the `Light Color Sensor`_ is not reachable.
:raises ~exceptions.RuntimeError: When the chip ID is incorrect. This happens when we have a device pointing to the same address, but it's not a `Light Color Sensor`_.
"""
self.use_mutex = use_mutex
try:
bus = ports[port]
except KeyError:
bus = "RPI_1SW"
# in case there's a distance sensor that hasn't been instanciated yet
# attempt to move it to another address
ifMutexAcquire(self.use_mutex)
try:
VL53L0X.VL53L0X(bus=bus)
except:
pass
ifMutexRelease(self.use_mutex)
ifMutexAcquire(self.use_mutex)
try:
super(self.__class__, self).__init__(led_state=led_state, bus=bus)
except Exception as e:
raise
finally:
ifMutexRelease(self.use_mutex)
self.led_state = led_state
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
def read(self, representation="raw"):
"""
Read the sensors' values from either line follower.
:param str representation="raw": It's set by-default to ``"raw"`` , but it can also be ``"bivariate"``, ``"bivariate-str"`` or ``"weighted-avg"``.
:raises ~exceptions.OSError: If the line follower sensor is not reachable.
Each of the line followers' order of the sensors' values is the same as the one in each read method of them both: :py:meth:`di_sensors.line_follower.LineFollower.read_sensors` and :py:meth:`di_sensors.line_follower.LineFollowerRed.read_sensors`.
For ``representation="raw"``
For this, raw values are returned from the line follower sensor. Values range between **0** and **1** and there can be 5 or 6 values returned depending on what line follower sensor is used.
For ``representation="bivariate"``
In this case, a list with the length equal to the number of sensors present on the given line follower is returned. Values are either **0** (for black) or **1** (for white).
In order to get good results, make sure the line follower is properly calibrated.
For ``representation="bivariate-str"``
Same as ``"bivariate"`` except that **0** is replaced with letter `b` (for black) and **1** with `w` (for white).
For ``representation="weighted-avg"``
Returns a 2-element tuple. The first element is an estimated position of the line.
The estimate is computed using a weighted average of each sensor value (regardless of which line follower sensor is used),
so that if the black line is on the left of the line follower, the returned value will be in the **0.0-0.5** range and
if it's on the right, it's in the **0.5-1.0** range,
thus making **0.5** the center point of the black line.
Keep in mind that the sensor's orientation is determined by the order
of the returned sensor values and not by how the sensor is positioned on the robot.
Check :py:meth:`~di_sensors.line_follower.LineFollower.read_sensors` and :py:meth:`~di_sensors.line_follower.LineFollowerRed.read_sensors` methods to see
how the values are returned.
If the line follower sensor ends up on a surface with an homogeneous color (or shade of grey), the returned value will circle around **0.5**.
The 2nd element is an integer taking 3 values: **1** if the line follower only detects black, **2** if it only detects white and **0** for the rest of cases.
"""
ifMutexAcquire(self.use_mutex)
try:
if representation == 'raw':
return self.sensor.read_sensors()
elif representation == 'bivariate':
raw_vals = self.sensor.read_sensors()
return self._bivariate(raw_vals)
elif representation == 'bivariate-str':
raw_vals = self.read('bivariate')
return self._bivariate_str(raw_vals)
elif representation == 'weighted-avg':
raw_vals = self.sensor.read_sensors()
return self._weighted_avg(raw_vals)
else:
pass
except Exception as e:
raise
finally:
ifMutexRelease(self.use_mutex)
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
def read_mm(self):
"""
Reads the distance in millimeters.
:returns: Distance from target in millimeters.
:rtype: int
.. note::
1. Sensor's range is **5-2300** millimeters.
2. When the values are out of the range, it returns **3000**.
"""
# 8190 is what the sensor sends when it's out of range
# we're just setting a default value
mm = 8190
readings = []
attempt = 0
# try 3 times to have a reading that is
# smaller than 8m or bigger than 5 mm.
# if sensor insists on that value, then pass it on
while (mm > 8000 or mm < 5) and attempt < 3:
ifMutexAcquire(self.use_mutex)
try:
mm = self.read_range_single()
except Exception as e:
print(e)
mm = 0
finally:
ifMutexRelease(self.use_mutex)
attempt = attempt + 1
time.sleep(0.001)
# add the reading to our last 3 readings
# a 0 value is possible when sensor is not found
if (mm < 8000 and mm > 5) or mm == 0:
readings.append(mm)
if len(readings) > 3:
readings.pop(0)
# calculate an average and limit it to 5 > X > 3000
if len(readings) > 1: # avoid division by 0
mm = round(sum(readings) / float(len(readings)))
if mm > 3000:
mm = 3000
return mm
def get_orientation_IMU(self):
readings = (NOREADING,NOREADING,NOREADING)
if self.config['imu'] >= DISABLED or not self.Configured:
return readings
ifMutexAcquire(USEMUTEX)
try:
readings = self.imu.read_euler()
time.sleep(0.01)
self.config['imu'] = ENABLED
except Exception as error:
self.log("IMU Orientation: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return readings
def get_gyro_sensor_IMU(self):
gyro_readings = (NOREADING,NOREADING,NOREADING)
if self.config['imu'] >= 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'] = ENABLED
except Exception as error:
self.log("IMU GYRO: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return gyro_readings
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
def safe_calibration_status(self):
"""
Returns the calibration level of the magnetometer of the `InertialMeasurementUnit Sensor`_.
:returns: Calibration level of the magnetometer. Range is **0-3** and **-1** is returned when the sensor can't be accessed.
:rtype: int
"""
ifMutexAcquire(self.use_mutex)
try:
status = self.BNO055.get_calibration_status()[3]
except Exception as e:
status = -1
finally:
ifMutexRelease(self.use_mutex)
return status
def reconfig_bus(self):
"""
Use this method when the `InertialMeasurementUnit Sensor`_ becomes unresponsive but it's still plugged into the board.
There will be times when due to improper electrical contacts, the link between the sensor and the board gets disrupted - using this method restablishes the connection.
.. note::
Sometimes the sensor won't work just by calling this method - in this case, switching the port will do the job. This is something that happens
very rarely, so there's no need to worry much about this scenario.
"""
ifMutexAcquire(self.use_mutex)
self.BNO055.i2c_bus.reconfig_bus()
ifMutexRelease(self.use_mutex)
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
def get_colour_sensor(self):
if self.config['colour'] >= SensorStatus.DISABLED:
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 Exception as error:
self.log("COLOUR: " + str(error))
self.config['colour'] += 1
ifMutexRelease(USEMUTEX)
return colours[value]
def safe_raw_colors(self):
"""
Returns the color as read by the `Light Color Sensor`_.
The colors detected vary depending on the lighting conditions of the nearby environment.
:returns: The RGBA values from the sensor. RGBA = Red, Green, Blue, Alpha (or Clear). Range of each element is between **0** and **1**. **-1** means an error occured.
:rtype: tuple(float,float,float,float)
"""
ifMutexAcquire(self.use_mutex)
try:
self.set_led(True, True) # turn light on to take color readings
r, g, b, c = self.get_raw_colors()
self.set_led(self.led_state, True) # return to default setting
except:
r, g, b, c = [-1] * 4
finally:
ifMutexRelease(self.use_mutex)
return (r, g, b, c)
def get_colour_sensor(self):
if self.config['colour'] >= 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'] = ENABLED
except brickpi3.SensorError as error:
self.log("COLOUR: " + str(error))
self.config['colour'] += 1
finally:
ifMutexRelease(USEMUTEX)
return colours[value]
def get_linear_acceleration_IMU(self):
readings = (NOREADING,NOREADING,NOREADING)
if self.config['imu'] >= 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'] = ENABLED
except Exception as error:
self.log("IMU Acceleration: " + str(error))
self.config['imu'] += 1
finally:
ifMutexRelease(USEMUTEX)
return readings
def safe_humidity(self):
"""
Read the relative humidity as a percentage.
:returns: Percentage of the relative humidity.
:rtype: float
:raises ~exceptions.OSError: When the sensor cannot be reached.
"""
ifMutexAcquire(self.use_mutex)
try:
humidity = self.get_humidity()
except Exception as e:
raise
finally:
ifMutexRelease(self.use_mutex)
return round(humidity, 0)
def safe_pressure(self):
"""
Read the air pressure in pascals.
:returns: The air pressure in pascals.
:rtype: float
:raises ~exceptions.OSError: When the sensor cannot be reached.
"""
ifMutexAcquire(self.use_mutex)
try:
pressure = self.get_pressure()
except Exception as e:
raise
finally:
ifMutexRelease(self.use_mutex)
return round(pressure, 0)
def safe_fahrenheit(self):
"""
Read temperature in Fahrenheit degrees.
:returns: Temperature in Fahrenheit degrees.
:rtype: float
:raises ~exceptions.OSError: When the sensor cannot be reached.
"""
ifMutexAcquire(self.use_mutex)
try:
temp = self.get_temperature_fahrenheit()
except Exception as e:
raise
finally:
ifMutexRelease(self.use_mutex)
return round(temp, 0)
def __init__(self, port="I2C", use_mutex=False):
"""
Creates a :py:class:`~easygopigo3.EasyDistanceSensor` object which can be used for interfacing with a `distance sensor`_.
:param string bus = ``"I2C"``: the bus for the sensor. For the GoPiGo3, options also include ``"GPG3_AD1"`` and ``"GPG3_AD2"``.
:param bool use_mutex = False: When using multiple threads/processes that access the same resource/device, mutexes should be enabled. Check the :ref:`hardware specs <hardware-interface-section>` for more information about the ports.
:raises ~exceptions.OSError: When the distance sensor is not connected to the designated bus/port, where in this case it must be ``"I2C"``. Most probably, this means the distance sensor is not connected at all.
To see where the ports are located on the `GoPiGo3`_ robot, please take a look at the following diagram: :ref:`hardware-ports-section`.
"""
self.descriptor = "Distance Sensor"
self.use_mutex = use_mutex
# let's be kind to the user who may get confused between ports and buses
# let's allow for all of them
possible_ports = {
"I2C": "RPI_1SW",
"AD1": "GPG3_AD1",
"AD2": "GPG3_AD2",
"RPI_1SW": "RPI_1SW",
"RPI_1": "RPI_1",
"RPI_1HW":
"RPI_1", # doesn't really exist but can be a reflex for some users as we do have RPI_1SW
"GPG3_AD1": "GPG3_AD1",
"GPG3_AD2": "GPG3_AD2"
}
port = port.upper() # force to uppercase
# switch quietly to default value if we receive gibberish
if port in possible_ports.keys():
bus = possible_ports[port]
else:
bus = "RPI_1SW"
ifMutexAcquire(self.use_mutex)
try:
distance_sensor.DistanceSensor.__init__(self, bus=bus)
except Exception as e:
# remove print so that it doesn't show when starting Scratch
# print("Distance Sensor init: {}".format(e))
raise
finally:
ifMutexRelease(self.use_mutex)
def safe_raw_colors(self):
"""
Returns the color as read by the `Light Color Sensor`_.
The colors detected vary depending on the lighting conditions of the nearby environment.
:returns: The RGBA values from the sensor. RGBA = Red, Green, Blue, Alpha (or Clear). Range of each element is between **0** and **1**.
:rtype: tuple(float,float,float,float)
"""
ifMutexAcquire(self.use_mutex)
try:
self.set_led(self.led_state)
r, g, b, c = self.get_raw_colors()
except:
pass
finally:
ifMutexRelease(self.use_mutex)
return (r, g, b, c)
def update_thermal_sensor(self):
ifMutexAcquire(USEMUTEX)
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.config['thermal'] = SensorStatus.DISABLED
self.thread_running = False #exit thread
self.log("THERMAL UPDATE: " + str(error))
ifMutexRelease(USEMUTEX)
return
def safe_read_magnetometer(self):
"""
Read the magnetometer values.
:returns: Tuple containing X, Y, Z values in *micro-Teslas* units. You can check the X, Y, Z axes on the sensor itself.
:rtype: (float,float,float)
.. note::
In case of an exception occurring within this method, a tuple of 3 elements where all values are set to **0** is returned.
"""
ifMutexAcquire(self.use_mutex)
try:
x, y, z = self.read_magnetometer()
except Exception as e:
x, y, z = 0, 0, 0
finally:
ifMutexRelease(self.use_mutex)
return x, y, z
def safe_read_euler(self):
"""
Read the absolute orientation.
:returns: Tuple of euler angles in degrees of *heading*, *roll* and *pitch*.
:rtype: (float,float,float)
:raises ~exceptions.OSError: When the sensor is not reachable.
"""
ifMutexAcquire(self.use_mutex)
try:
x, y, z = self.read_euler()
except Exception as e:
# print("safe read euler: {}".format(str(e)))
# x, y, z = 0, 0, 0
raise
finally:
ifMutexRelease(self.use_mutex)
return x, y, z
def get_thermal_sensor(self, usethread=True):
temp = NOREADING
if self.config['thermal'] >= 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'] = 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)
def __init__(self, use_mutex=False):
"""
Creates a :py:class:`~easygopigo3.EasyDistanceSensor` object which can be used for interfacing with a `distance sensor`_.
:param bool use_mutex = False: When using multiple threads/processes that access the same resource/device, mutexes should be enabled. Check the :ref:`hardware specs <hardware-interface-section>` for more information about the ports.
:raises ~exceptions.OSError: When the distance sensor is not connected to the designated bus/port, where in this case it must be ``"I2C"``. Most probably, this means the distance sensor is not connected at all.
To see where the ports are located on the `GoPiGo3`_ robot, please take a look at the following diagram: :ref:`hardware-ports-section`.
"""
self.descriptor = "Distance Sensor"
self.use_mutex = use_mutex
ifMutexAcquire(self.use_mutex)
try:
distance_sensor.DistanceSensor.__init__(self)
except Exception as e:
print("Distance Sensor init: {}".format(e))
raise
finally:
ifMutexRelease(self.use_mutex)
def get_compass_IMU(self):
heading = NOREADING
if self.config['imu'] >= DISABLED or not self.Configured:
return heading
ifMutexAcquire(USEMUTEX)
try:
(x, y, z) = self.imu.read_magnetometer()
time.sleep(0.01)
self.config['imu'] = 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
def __init__(self, port="AD1", use_mutex=False):
"""
Constructor for initializing link with the `InertialMeasurementUnit Sensor`_.
:param str port = "AD1": The port to which the IMU sensor gets connected to. Can also be connected to port ``"AD2"`` of a `GoPiGo3`_ robot or to any ``"I2C"`` port of any of our platforms. If you're passing an **invalid port**, then the sensor resorts to an ``"I2C"`` connection. Check the :ref:`hardware specs <hardware-interface-section>` for more information about the ports.
:param bool use_mutex = False: When using multiple threads/processes that access the same resource/device, mutexes should be enabled.
:raises RuntimeError: When the chip ID is incorrect. This happens when we have a device pointing to the same address, but it's not a `InertialMeasurementUnit Sensor`_.
:raises ~exceptions.OSError: When the `InertialMeasurementUnit Sensor`_ is not reachable.
"""
self.use_mutex = use_mutex
try:
bus = ports[port]
except KeyError:
bus = "RPI_1SW"
ifMutexAcquire(self.use_mutex)
try:
# print("INSTANTIATING ON PORT {} OR BUS {} WITH MUTEX {}".format(port, bus, use_mutex))
super(self.__class__, self).__init__(bus=bus)
# on GPG3 we ask that the IMU be at the back of the robot, facing outward
# We do not support the IMU on GPG2 but leaving the if statement in case
if bus != "RPI_1SW":
self.BNO055.set_axis_remap(BNO055.AXIS_REMAP_X,
BNO055.AXIS_REMAP_Z,
BNO055.AXIS_REMAP_Y,
BNO055.AXIS_REMAP_POSITIVE,
BNO055.AXIS_REMAP_NEGATIVE,
BNO055.AXIS_REMAP_POSITIVE)
except Exception as e:
print("Initiating error: " + str(e))
raise
finally:
sleep(
0.1
) # add a delay to let the IMU stabilize before control panel can pull from it
ifMutexRelease(self.use_mutex)
def __init__(self, port="I2C", use_mutex=False):
"""
Constructor for initializing link with the `Temperature Humidity Pressure Sensor`_.
:param str port = "I2C": The port to which the THP sensor is connected to. Can also be connected to ports ``"AD1"`` or ``"AD2"`` of the `GoPiGo3`_. If you're passing an **invalid port**, then the sensor resorts to an ``"I2C"`` connection. Check the :ref:`hardware specs <hardware-interface-section>` for more information about the ports.
:param bool use_mutex = False: When using multiple threads/processes that access the same resource/device, mutexes should be enabled.
:raises ~exceptions.OSError: When the sensor cannot be reached.
"""
self.use_mutex = use_mutex
try:
bus = ports[port]
except KeyError:
bus = "RPI_1SW"
ifMutexAcquire(self.use_mutex)
try:
super(self.__class__, self).__init__(bus=bus)
except Exception as e:
raise
finally:
ifMutexRelease(self.use_mutex)
def safe_calibrate(self):
"""
Once called, the method returns when the magnetometer of the `InertialMeasurementUnit Sensor`_ gets fully calibrated. Rotate the sensor in the air to help the sensor calibrate faster.
.. note::
Also, this method is not used to trigger the process of calibrating the sensor (the IMU does that automatically),
but its purpose is to block a given script until the sensor reports it has fully calibrated.
If you wish to block your code until the sensor calibrates and still have control over your script, use
:py:meth:`~di_sensors.easy_inertial_measurement_unit.EasyIMUSensor.safe_calibration_status` method along with a ``while`` loop to continuously check it.
"""
status = -1
while status < 3:
ifMutexAcquire(self.use_mutex)
try:
new_status = self.BNO055.get_calibration_status()[3]
except:
new_status = -1
finally:
ifMutexRelease(self.use_mutex)
if new_status != status:
status = new_status
