def __init__(self, api_easy):
self.api_easy = api_easy
self.api_imu = di_sensors_inertial_measurement_unit.InertialMeasurementUnit(
bus="GPG3_AD1")
self.thread_heading = threading.Thread(
target=self.__thread_heading_target, daemon=True)
self.thread_heading_lock = threading.Lock()
self.heading_history = collections.deque(
maxlen=10) ## TODO: make maxlen configurable
def __init__(self, addr=8, detect=True):
gopigo3.GoPiGo3.__init__(self)
self.scan_data = {}
# mutex sensors on IC2
self.distance_sensor = EasyDistanceSensor(port="RPI_1", use_mutex=True)
self.imu = inertial_measurement_unit.InertialMeasurementUnit(bus="RPI_1") # use_mutex=True
# buffer for reading the gyro
self.gyro_buffer = 0
self.stop()
def __init__(self): ## TODO: some of this should probably go in startup()?
self.api_full = gopigo3.GoPiGo3() # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object.
self.api_easy = easygopigo3.EasyGoPiGo3()
self.active = False
self.distance = self.api_easy.init_distance_sensor()
self.distance_servo = self.api_easy.init_servo("SERVO1")
self.camera = picamera.PiCamera()
self.camera_servo = self.api_easy.init_servo("SERVO2")
self.imu = inertial_measurement_unit.InertialMeasurementUnit(bus = "GPG3_AD1")
def __init__(self,
controler,
fps=25,
resolution=None,
servoPort="SERVO1",
motionPort="AD1"):
"""
Initialise le robot
:controler: le controler du robot, muni d'une fonction update et d'une fonction stop qui
rend in booleen (vrai a la fin du controle, faux sinon)
:fps: nombre d'appel a controler.update() par seconde (approximatif!)
:resolution: resolution de la camera
:servoPort: port du servo (SERVO1 ou SERVO2)
:motionPort: port pour l'accelerometre (AD1 ou AD2)
"""
self._gpg = EasyGoPiGo3()
self.controler = controler
self.fps = fps
self.LED_LEFT_EYE = self._gpg.LED_LEFT_EYE
self.LED_RIGHT_EYE = self._gpg.LED_RIGHT_EYE
self.LED_LEFT_BLINKER = self._gpg.LED_LEFT_BLINKER
self.LED_RIGHT_BLINKER = self._gpg.LED_RIGHT_BLINKER
self.LED_WIFI = self._gpg.LED_WIFI
self.MOTOR_LEFT = self._gpg.MOTOR_LEFT
self.MOTOR_RIGHT = self._gpg.MOTOR_RIGHT
try:
self.camera = picamera.PiCamera()
if resolution:
self.camera.resolution = resolution
except Exception as e:
print("Camera not found", e)
try:
self.servo = Servo(servoPort, self._gpg)
except Exception as e:
print("Servo not found", e)
try:
self.distanceSensor = ds_sensor.DistanceSensor()
except Exception as e:
print("Distance Sensor not found", e)
try:
self.imu = imu.InertialMeasurementUnit()
except Exception as e:
print("IMU sensor not found", e)
self._gpg.set_motor_limits(
self._gpg.MOTOR_LEFT + self._gpg.MOTOR_RIGHT, 0)
def orientate():
try:
imu = inertial_measurement_unit.InertialMeasurementUnit(bus="GPG3_AD1")
except Exception as msg:
print(str(msg))
print(
"Rotate the GoPiGo3 robot with your hand until it's fully calibrated")
try:
compass = imu.BNO055.get_calibration_status()[3]
except Exception as msg:
compass = 0
values_already_printed = []
max_conseq_errors = 3
while compass != 3:
state = ""
if compass == 0:
state = "not yet calibrated"
elif compass == 1:
state = "partially calibrated"
elif compass == 2:
state = "almost calibrated"
if not compass in values_already_printed:
print("The GoPiGo3 is " + state)
values_already_printed.append(compass)
try:
compass = imu.BNO055.get_calibration_status()[3]
except Exception as msg:
max_conseq_errors -= 1
sleep(0.09)
continue
print("The GoPiGo3 is Calibrated")
return imu
def orientate(trigger, simultaneous_launcher, sensor_queue):
"""
Thread-launched function for reading the compass data off of the IMU sensor. The data is then
interpreted and then it's loaded in a queue.
:param trigger: CTRL-C event. When it's set, it means CTRL-C was pressed and the thread needs to stop.
:param simultaneous_launcher: It's a barrier used for synchronizing all threads together.
:param sensor_queue: Queue where the processed data of the compass is put in.
:return: Nothing.
"""
time_to_put_in_queue = 0.2 # measured in seconds
time_to_wait_after_error = 0.5 # measured in seconds
# try instantiating an InertialMeasurementUnit object
try:
imu = inertial_measurement_unit.InertialMeasurementUnit(bus="GPG3_AD1")
except Exception as msg:
print(str(msg))
simultaneous_launcher.abort()
# start the calibrating process of the compass
print(
"Rotate the GoPiGo3 robot with your hand until it's fully calibrated")
try:
compass = imu.BNO055.get_calibration_status()[3]
except Exception as msg:
compass = 0
values_already_printed = []
max_conseq_errors = 3
while compass != 3 and not trigger.is_set() and max_conseq_errors > 0:
state = ""
if compass == 0:
state = "not yet calibrated"
elif compass == 1:
state = "partially calibrated"
elif compass == 2:
state = "almost calibrated"
if not compass in values_already_printed:
print("The GoPiGo3 is " + state)
values_already_printed.append(compass)
try:
compass = imu.BNO055.get_calibration_status()[3]
except Exception as msg:
max_conseq_errors -= 1
sleep(time_to_wait_after_error)
continue
# if CTRL-C was triggered or if the calibration failed
# then abort everything
if trigger.is_set() or max_conseq_errors == 0:
print("IMU sensor is not reacheable or kill event was triggered")
simultaneous_launcher.abort()
else:
state = "fully calibrated"
print("The GoPiGo3 is " + state)
# point of synchronizing all threads together (including main)
# it fails if abort method was called
try:
simultaneous_launcher.wait()
except threading.BrokenBarrierError as msg:
print("[orientate] thread couldn't fully start up")
# while CTRl-C is not pressed and while the synchronization went fine
while not (trigger.is_set() or simultaneous_launcher.broken):
five_values = 10
heading_list = []
max_conseq_errors = 3
# get the north point
# extract a couple of values before going to the next procedure
while five_values > 0 and max_conseq_errors > 0:
try:
heading_list.append(getNorthPoint(imu))
except Exception as msg:
max_conseq_errors -= 1
sleep(time_to_wait_after_error)
continue
five_values -= 1
if max_conseq_errors == 0:
print("IMU is not reacheable")
trigger.set()
break
# apply some filtering
heading_list = statisticalNoiseReduction(heading_list)
heading_avg = mean(heading_list)
# and then try to put it in the queue
# if the queue is full, then just go to the next iteration of the while loop
try:
sensor_queue.put(heading_avg, timeout=time_to_put_in_queue)
except queue.Full:
pass
import os
import pygame
import boto3
import pymysql.cursors
import di_sensors.inertial_measurement_unit as imu
gpg = easy.EasyGoPiGo3()
sqlconn = pymysql.connect(host='localhost',
user='******',
password='******',
db='robot')
servo = gpg.init_servo(port='SERVO1')
servo2 = gpg.init_servo(port='SERVO2')
distance_sensor = gpg.init_distance_sensor()
white_led = gpg.init_led('AD1')
imu_sensor = imu.InertialMeasurementUnit()
pygame.mixer.init()
polly = boto3.client('polly')
rek = boto3.client('rekognition')
rek_image_file = 'rek_pic.jpg'
sound_dir = '/home/pi/aaa_mikes_gopigo/gopigo/sounds/'
speed = 300
white_led_status = 'off'
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (368, 240)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(368, 240))
def __init__(self, api_easy):
self.api_easy = api_easy
self.api_imu = inertial_measurement_unit.InertialMeasurementUnit(
bus="GPG3_AD1")
from __future__ import print_function # use python 3 syntax but make it compatible with python 2 from __future__ import division # '' import time # import the time library for the sleep function import gopigo3 # import the GoPiGo3 drivers import sys # import sys for sys.exit() from di_sensors import inertial_measurement_unit # import the IMU drivers # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object. GPG = gopigo3.GoPiGo3() # Clear previous configurations GPG.reset_all() # define which I2C bus the IMU is connected to. imu = inertial_measurement_unit.InertialMeasurementUnit(bus="GPG3_AD1") # define which port the IR receiver is connected to. PORT_SENSOR_IR = GPG.GROVE_2 # how fast to drive when being controlled by the remote (degrees per second) DRIVE_SPEED = 175 # how fast to steer when being controlled by the remote (degrees per second) STEER_SPEED = 100 # constants used to define how agressively the robot should respond to: KGYROANGLE = 15 # overall angle KGYROSPEED = 1.7 # sudden changes in angle KPOS = 0.07 # deviation from the target position KSPEED = 0.1 # the actual speed of the motor
from di_sensors import inertial_measurement_unit
from easygopigo3 import *
import time
gpg = EasyGoPiGo3()
imu = inertial_measurement_unit.InertialMeasurementUnit()
turn_degree = 90
offset = 25
actual_turn_degree = turn_degree + turn_degree * offset / 360
gpg.set_speed(150)
try:
print(imu.read_euler())
#gpg.forward()
time.sleep(1)
gpg.turn_degrees(actual_turn_degree)
#gpg.turn_degrees(turn_degree)
time.sleep(1)
print(imu.read_euler())
time.sleep(1)
gpg.turn_degrees(actual_turn_degree)
#gpg.turn_degrees(turn_degree)
time.sleep(1)
print(imu.read_euler())
time.sleep(1)
gpg.turn_degrees(actual_turn_degree)
#gpg.turn_degrees(turn_degree)
time.sleep(1)
print(imu.read_euler())
gpg.turn_degrees(actual_turn_degree)
#gpg.turn_degrees(turn_degree)
time.sleep(1)
#!/usr/bin/env python import di_sensors.inertial_measurement_unit as IMU imu = IMU.InertialMeasurementUnit() mag = imu.read_magnetometer() gyro = imu.read_gyroscope() accel = imu.read_accelerometer() euler = imu.read_euler() temp = imu.read_temperature() print(mag, gyro, accel, euler, temp)