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.inertial_measurement_unit()
except Exception as e:
print("IMU sensor not found", e)
self._gpg.set_motor_limits(
self._gpg.MOTOR_LEFT + self._gpg.MOTOR_RIGHT, 0)
#!/usr/bin/env python
# GoPiGo3 imports
from di_sensors import distance_sensor
import gopigo3
import time
# ROS imports
import rospy
# TODO Int16 import is now obsolete (I think)
from std_msgs.msg import Int16
from sensor_msgs.msg import LaserScan
# Instantiate GoPiGo object
objGpg = gopigo3.GoPiGo3()
# Instantiate DistanceSensor object
objDstSns = distance_sensor.DistanceSensor()
# Variables
stepSweep = 10
sweepDir = 1
def scan():
global stepSweep, sweepDir
# Maximum count to the left
countMax = 2420
# Minimum count to the right
countMin = 620
# Populate the LaserScan message
numReadings = (countMax - countMin) / abs(stepSweep)
now = rospy.get_rostime()
msgScan = LaserScan()
#!/usr/bin/env python
#
# https://www.dexterindustries.com/
#
# Copyright (c) 2017 Dexter Industries
# 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 Distance Sensor
from __future__ import print_function
from __future__ import division
import time
from di_sensors import distance_sensor
print(
"Example program for reading a Dexter Industries Distance Sensor on a GoPiGo3 AD1 port."
)
ds = distance_sensor.DistanceSensor("GPG3_AD1")
while True:
# read the distance as a single-shot sample
print("%4dmm" % ds.read_range_single())
if ds.timeout_occurred():
print(" TIMEOUT")
#!/usr/bin/env python
#
# https://www.dexterindustries.com
#
# Copyright (c) 2017 Dexter Industries
# 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 Distance Sensor
from __future__ import print_function
from __future__ import division
import time
from di_sensors import distance_sensor
print(
"Example program for reading a Dexter Industries Distance Sensor on an I2C port."
)
ds = distance_sensor.DistanceSensor()
# configure for continuous samples
ds.start_continuous()
while True:
# read the distance
print("%4dmm" % ds.read_range_continuous())
if ds.timeout_occurred():
print(" TIMEOUT")