def main():
# instantiate the distance object
#sensor = DistanceSensor()
my_sensor = EasyDistanceSensor()
#sensor.start_continuous()
rospy.init_node("distance_sensor")
pub_distance = rospy.Publisher("~distance", Range, queue_size=10)
msg_range = Range()
msg_range.header.frame_id = "distance"
msg_range.radiation_type = Range.INFRARED
msg_range.min_range = 0.02
msg_range.max_range = 3.0
rate = rospy.Rate(rospy.get_param('~hz', 1))
while not rospy.is_shutdown():
# read distance in meters
read_distance = my_sensor.read() / 100.0
msg_range.range = read_distance
# Add timestamp
msg_range.header.stamp = rospy.Time.now()
# Print current distance
print msg_range.range * 1000, " mm"
# Publish distance message
pub_distance.publish(msg_range)
rate.sleep()
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()
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
#!/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 the modules
from di_sensors.easy_distance_sensor import EasyDistanceSensor
from time import sleep
# instantiate the distance object
my_sensor = EasyDistanceSensor()
# and read the sensor iteratively
while True:
read_distance = my_sensor.read()
print("distance from object: {} mm".format(read_distance))
sleep(0.1)
class PiggyParent(gopigo3.GoPiGo3):
'''
UTILITIES
'''
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")
# buffer for reading the gyro
self.gyro_buffer = 0
self.stop()
def calibrate(self):
"""allows user to experiment on finding centered midpoint and even motor speeds"""
print("Calibrating...")
self.servo(self.MIDPOINT)
response = str.lower(input("Am I looking straight ahead? (y/n): "))
if response == 'n':
while True:
response = str.lower(
input("Turn right, left, or am I done? (r/l/d): "))
if response == "r":
self.MIDPOINT -= 25
print("Midpoint: " + str(self.MIDPOINT))
self.servo(self.MIDPOINT)
elif response == "l":
self.MIDPOINT += 25
print("Midpoint: " + str(self.MIDPOINT))
self.servo(self.MIDPOINT)
else:
print("Midpoint temporarily saved to: " +
str(self.MIDPOINT) +
"\nYou'll need to update your magic number.")
break
else:
print('Cool, %d is the correct self.MIDPOINT' % self.MIDPOINT)
response = str.lower(
input("Do you want to check if I'm driving straight? (y/n)"))
if 'y' in response:
while True:
self.set_motor_limits(self.MOTOR_LEFT, self.LEFT_DEFAULT)
self.set_motor_limits(self.MOTOR_RIGHT, self.RIGHT_DEFAULT)
print("LEFT: {} // RIGHT: {} ".format(self.MOTOR_LEFT,
self.MOTOR_RIGHT))
self.fwd()
time.sleep(1)
self.stop()
response = str.lower(
input("Reduce left, reduce right or drive? (l/r/d): "))
if response == 'l':
self.LEFT_SPEED -= 5
elif response == 'r':
self.RIGHT_SPEED -= 5
elif response == 'd':
self.fwd()
time.sleep(1)
self.stop()
else:
break
def quit(self):
"""Terminates robot movement and settings then closes app"""
print("\nIt's been a pleasure.\n")
self.reset_all()
sys.exit(1)
'''
MOVEMENT
'''
def deg_fwd(self, deg):
"""Zeroes current encorder values then moves forward based on degrees given"""
self.offset_motor_encoder(self.MOTOR_LEFT,
self.get_motor_encoder(self.MOTOR_LEFT))
self.offset_motor_encoder(self.MOTOR_RIGHT,
self.get_motor_encoder(self.MOTOR_RIGHT))
self.set_motor_position(self.MOTOR_LEFT + self.MOTOR_RIGHT, deg)
def turn_by_deg(self, deg):
"""Rotates robot relative to it's current heading. If told -20, it
will rotate left by 20 degrees."""
# get our current angle
current = self.get_heading()
# calculate delta
goal = current + deg
# LOOP AROUND THE 360 marker
goal %= 360
# call turn to deg on the delta
self.turn_to_deg(goal)
def turn_to_deg(self, deg):
"""Turns to a degree relative to the gyroscope's readings. If told 20, it
will rotate until the gyroscope reads 20."""
# error check
goal = abs(deg) % 360
current = self.get_heading()
turn = self.right # connect it to the method without the () to activate
if (current - goal > 0 and current - goal < 180) or \
(current - goal < 0 and (360 - goal) + current < 180):
turn = self.left
# while loop - keep turning until my gyro says I'm there
while abs(deg - self.get_heading()) > 10:
turn(primary=70, counter=-70)
#time.sleep(.05) # avoid spamming the gyro
# once out of the loop, hit the brakes
self.stop()
# report out to the user
print("\n{} is close enough to {}.\n".format(self.get_heading(), deg))
def fwd(self, left=50, right=50):
"""Blindly charges your robot forward at default power which needs to be configured in child class"""
if self.LEFT_DEFAULT and left == 50:
left = self.LEFT_DEFAULT
if self.RIGHT_DEFAULT and right == 50:
right = self.RIGHT_DEFAULT
self.set_motor_power(self.MOTOR_LEFT, left)
self.set_motor_power(self.MOTOR_RIGHT, right)
def right(self, primary=90, counter=0):
"""Rotates right by powering the left motor to default"""
self.set_motor_power(self.MOTOR_LEFT, primary)
self.set_motor_power(self.MOTOR_RIGHT, counter)
def left(self, primary=90, counter=0):
"""Rotates left by powering the left motor to 90 by default and counter motion 0"""
self.set_motor_power(self.MOTOR_LEFT, counter)
self.set_motor_power(self.MOTOR_RIGHT, primary)
def back(self, left=-50, right=-50):
if self.LEFT_DEFAULT and left == -50:
left = -self.LEFT_DEFAULT
if self.RIGHT_DEFAULT and right == -50:
right = -self.RIGHT_DEFAULT
self.set_motor_power(self.MOTOR_LEFT, left)
self.set_motor_power(self.MOTOR_RIGHT, right)
def servo(self, angle):
"""Moves the servo to the given angle"""
print("Servo moving to: {} ".format(angle))
self.set_servo(self.SERVO_1, angle)
time.sleep(.05)
def stop(self):
"""Cut power to the motors"""
print("\n--STOPPING--\n")
self.set_motor_power(self.MOTOR_LEFT + self.MOTOR_RIGHT, 0)
'''
SENSORS
'''
def read_distance(self):
"""Returns the GoPiGo3's distance sensor in MM over IC2"""
d = self.distance_sensor.read_mm()
print("Distance Sensor Reading: {} mm ".format(d))
return d
def get_heading(self):
"""Returns the heading from the IMU sensor, or if there's a sensor exception, it returns
the last saved reading"""
try:
self.gyro_buffer = self.imu.read_euler()[0]
print("Gyroscope sensor is at: {} degrees ".format(
self.gyro_buffer))
except Exception as e:
print("----- PREVENTED GYRO SENSOR CRASH -----")
print(e)
return self.gyro_buffer
from di_sensors.easy_distance_sensor import EasyDistanceSensor
import time
import rospy
from geometry_msgs.msg import Twist
if __name__ == '__main__':
rospy.init_node("runnerDistanceNode")
pub = rospy.Publisher("runnerDistance", Twist, queue_size=3)
my_sensor = EasyDistanceSensor(address=0x2a, use_mutex=False)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
read_distance = my_sensor.read()
data = Twist()
data.linear.x = read_distance / 100
pub.publish(data)
rospy.loginfo(f"Distance: {read_distance/100}m")
rate.sleep()
def __init__(self):
super().__init__()
self.imu = InertialMeasurementUnit(bus="GPG3_AD1")
self.ds = EasyDistanceSensor()
# 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 Temperature Humidity Pressure Sensor
from __future__ import print_function
from __future__ import division
# do the import stuff
from di_sensors.easy_distance_sensor import EasyDistanceSensor
from time import time, sleep
from threading import Thread, Event, get_ident
# instantiate the distance object
my_sensor = EasyDistanceSensor(use_mutex = True)
start_time = time()
runtime = 2.0
# create an event object for triggering the "shutdown" of each thread
stop_event = Event()
# target function for each thread
def readingSensor():
while not stop_event.is_set():
thread_id = get_ident()
distance = my_sensor.read()
print("Thread ID = {} with distance value = {}".format(thread_id, distance))
sleep(0.001)
# create an object for each thread
thread1 = Thread(target = readingSensor)
import urllib2
import base64
import picamera
import json
from subprocess import call
import time
import datetime
import atexit
from googleapiclient import discovery
import google.auth
from easygopigo3 import EasyGoPiGo3
from di_sensors.easy_distance_sensor import EasyDistanceSensor
gpg = EasyGoPiGo3()
button = gpg.init_button_sensor("AD1") # button on GoPiGo3's AD1 port
ds = EasyDistanceSensor() # distance sensor on I2C port
atexit.register(gpg.stop) # If we exit, stop the motors
#Calls the Espeak TTS Engine to read aloud a sentence
# This function is going to read aloud some text over the speakers
def sound(spk):
# -ven+m7: Male voice
# The variants are +m1 +m2 +m3 +m4 +m5 +m6 +m7 for male voices and +f1 +f2 +f3 +f4 which simulate female voices by using higher pitches. Other variants include +croak and +whisper.
# Run the command espeak --voices for a list of voices.
# -s180: set reading to 180 Words per minute
# -k20: Emphasis on Capital letters
# to enable audio output on the audio jack
# otherwise, instead of one, select 2 for HDMI or 0 for automatic
