def setup(self):
if self.option == "a":
# --- Import local libraries ---
import direct.directbase.DirectStart
from RobotModel3D import Robot
from main_simulation3d import Simulation
self.sim = Simulation()
j = 1 # For the number of a collision sphere
for i in numpy.arange(0, COLLISION_DIST / 30, 0.5):
self.sim.sColl = self.sim.initCollisionSphere(
self.sim.robot.robotModel, False,
Point3(0, (COLLISION_DIST / 30) - i + 1, 1), j)
base.cTrav.addCollider(self.sim.sColl[0],
self.sim.collHandEvent)
self.sim.accept('into-' + self.sim.sColl[1], self.sim.collide)
j += 1
self.robot = self.sim.robot
return self.robot
else:
# --- Import local libraries ---
from easygopigo3 import EasyGoPiGo3
from RobotDexter import Dexter
gpg = EasyGoPiGo3()
self.robot = Dexter(gpg)
return self.robot
def setup(self):
if self.option == "a":
# --- Import local libraries ---
import direct.directbase.DirectStart
from RobotModel3D import Robot
from main_simulation3d import Simulation
self.sim = Simulation()
for i in numpy.arange(0, COLLISION_DIST / 30, 0.5):
self.sim.sColl = self.sim.initCollisionSphere(
self.sim.robot.robotModel, True,
Point3(0, (COLLISION_DIST / 30) - i, 1))
base.cTrav.addCollider(self.sim.sColl[0],
self.sim.collHandEvent)
self.sim.accept('into-' + self.sim.sColl[1], self.sim.collide)
self.robot = self.sim.robot
return self.robot
else:
# --- Import local libraries ---
from easygopigo3 import EasyGoPiGo3
from RobotDexter import Dexter
#import main_robotA #Camera thread continious
gpg = EasyGoPiGo3()
self.robot = Dexter(gpg)
return self.robot
def blinkers():
gpg = EasyGoPiGo3()
global robot_operating
while (robot_operating):
if (ldr_right.light_detected and not ldr_left.light_detected):
gpg.set_led(gpg.LED_BLINKER_RIGHT, 0)
gpg.set_led(gpg.LED_BLINKER_LEFT, 255)
gpg.set_led(gpg.LED_EYE_LEFT, 0)
gpg.set_led(gpg.LED_EYE_RIGHT, 0)
sleep(0.02)
elif (ldr_left.light_detected and not ldr_right.light_detected):
gpg.set_led(gpg.LED_BLINKER_RIGHT, 255)
gpg.set_led(gpg.LED_BLINKER_LEFT, 0)
gpg.set_led(gpg.LED_EYE_LEFT, 0)
gpg.set_led(gpg.LED_EYE_RIGHT, 0)
sleep(0.02)
elif (ldr_right.light_detected and ldr_left.light_detected):
gpg.set_led(gpg.LED_BLINKER_RIGHT, 0)
gpg.set_led(gpg.LED_BLINKER_LEFT, 0)
gpg.set_led(gpg.LED_EYE_LEFT, 255)
gpg.set_led(gpg.LED_EYE_RIGHT, 255)
sleep(0.02)
else:
gpg.set_led(gpg.LED_BLINKER_RIGHT, 0)
gpg.set_led(gpg.LED_BLINKER_LEFT, 0)
gpg.set_led(gpg.LED_EYE_LEFT, 0)
gpg.set_led(gpg.LED_EYE_RIGHT, 0)
sleep(0.02)
def main():
# start the camera in a separate thread
stop_camera = Event()
camera_thread = Thread(target=camera, args=(stop_camera,))
camera_thread.start()
global gpg3, control_queue
gpg3 = EasyGoPiGo3()
gpg3.set_speed(180)
control_queue = Queue()
stop_robot = Event()
robot_thread = Thread(target=robot, args=(stop_robot,))
robot_thread.start()
prev_signal = ' '
while True:
signal = key()
# quit signal
if signal == 'q':
break
elif control_queue.qsize() <= 0 or prev_signal != signal:
prev_signal = signal
control_queue.put(signal)
# stop everything
stop_camera.set()
stop_robot.set()
exit()
def obstacle():
# Initialize testbed motors
gpg = EasyGoPiGo3()
gpg.forward()
[circle_detected, center, obj_distance] = detection_demo()
gpg.stop()
if (center[0] > 320):
direction = 'right'
rotate_dir = -1
else:
direction = 'left'
rotate_dir = 1
# Rotate camera gradually until object is no longer detected
angle_rotated = 0
while (circle_detected):
gpg.turn_degrees(rotate_dir * 10)
[circle_detected, center,
obj_distance] = detection_demo(infinite_loop=False)
angle_rotated += 10
# Final turn to avoid grazing object after circle no longer in view
gpg.turn_degrees(rotate_dir * 20)
gpg.forward()
time.sleep(5)
gpg.stop()
def robot_drive(self, omf, uangle):
gpg = EasyGoPiGo3()
gpg.set_speed(180)
#drive = gpg.forward()
n = 0
rightAngle = False
#print(n)
if omf < 0:
rightAngle = True
while not rightAngle:
orientationangle = self.imu.getHeadingDeg()
print("orientationangle = ", orientationangle)
if uangle - 9 <= orientationangle and uangle + 9 >= orientationangle:
rightAngle = True
#print("1 orientationangle = ", orientationangle, 'uangle = ', uangle)
#print("same though")
else:
#print('current:',self.imu.getHeadingDeg())
#print('wanted :', uangle)
temp = 0
temp2 = 0
for i in range(8, 359):
#temp += 1
#tempCurrent
#print('i', i)
#print(int(orientationangle), int(uangle))
if int(orientationangle) + i > 360:
if (int(orientationangle) + i) - 360 == int(uangle):
temp = i
#print('temp assigned: special', temp, i)
if int(orientationangle) + i == int(uangle):
temp = i
#print('temp assigned:', temp, i)
if int(orientationangle) - i < 0:
if 360 + (int(orientationangle) - i) == int(uangle):
temp2 = i
if int(orientationangle) - i == int(uangle):
temp2 = i
#print('temp2 assigned:', temp2, i)
if temp != 0 and temp2 != 0:
#print('breaks at', i)
break
#print('1:', temp, '2:', temp2)
if temp2 < temp:
gpg.turn_degrees(-1 * temp2)
print('turned')
#print("2 orientationangle = ", orientationangle, 'uangle = ', uangle, 'diff cc=', temp2)
else:
gpg.turn_degrees(temp)
print('turned')
#print("2 orientationangle = ", orientationangle, 'uangle = ', uangle, 'diff cw=', temp)
if omf > 0:
gpg.forward()
print('moved')
if omf < 0:
gpg.stop()
print('stopped')
"""elif uangle - 10 < orientationangle:
def __init__(self, mock_controls=False):
if mock_controls:
self.gpg = MagicMock()
else:
self.gpg = EasyGoPiGo3() # instantiating a EasyGoPiGo3 object
self.gpg = MagicMock() # instantiating a EasyGoPiGo3 object
self.flashing = False
self.executor = ThreadPoolExecutor(max_workers=1)
self.executor.submit(self.flash_lights)
def __init__(self, dir_path, params=None):
self.dir_path = dir_path
self.calibrated = True
self.gpg = EasyGoPiGo3()
if params is None:
self.rad = 200
self.h_spd = 400
self.m_spd = 200
self.l_spd = 30
else:
self.rad = params['rad']
self.h_spd = params['h_spd']
self.m_spd = params['m_spd']
self.l_spd = params['l_spd']
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)
def move(bearing, leftMotorSpeed, rightMotorSpeed, pid):
gpg = EasyGoPiGo3()
motorSpeed = 50
output = pid(bearing)
print(output)
# calculate motor speedss
leftMotorSpeed = int(motorSpeed + output)
rightMotorSpeed = int(motorSpeed + output)
if leftMotorSpeed == 0:
leftMotorSpeed = 1
if rightMotorSpeed == 0:
rightMotorSpeed = 1
if abs(bearing) <= 5:
rightMotorSpeed = 100
leftMotorSpeed = 100
gpg.set_motor_dps(gpg.MOTOR_LEFT, dps=leftMotorSpeed)
gpg.set_motor_dps(gpg.MOTOR_RIGHT, dps=rightMotorSpeed)
return leftMotorSpeed, rightMotorSpeed, pid
def __init__(self):
try:
self.gopigo3_robot = EasyGoPiGo3() # init gopigo3
self.gopigo3_robot.set_speed(500) # adjust this accordingly
except IOError:
print("GoPiGo3 robot not detected")
exit(0)
try:
self.distance_sensor = self.gopigo3_robot.init_distance_senseor() # init dist sensor
except:
print("GoPiGo3 distance sensor not detected")
exit(0)
try:
self.servo = self.gopigo3_robot.init_servo() # init servo
self.servo.reset_servo() # reset servo to straight ahead
except:
print("GoPiGo3 servo not detected")
exit(0)
def movement():
gopigo3n = EasyGoPiGo3()
while (True):
if (ldr_right.light_detected and not ldr_left.light_detected):
#print("Right LDR detected!")
gopigo3n.left()
sleep(0.02)
elif (ldr_left.light_detected and not ldr_right.light_detected):
#print("Left LDR detected!")
gopigo3n.right()
sleep(0.02)
elif (ldr_right.light_detected and ldr_left.light_detected):
#print("No LDRs detected!")
gopigo3n.stop()
sleep(0.02)
else:
#print("Both LDR detected!")
gopigo3n.forward()
sleep(0.02)
def objectAvoidance():
sleep(3)
try:
gopigo3 = EasyGoPiGo3()
distance_sensor = gopigo3.init_distance_sensor()
except IOError as msg:
print("GoPiGo3 robot and sensors are not detected")
sys.exit(1)
gopigo3_stationary = True
global robot_operating
while robot_operating:
current_distance = distance_sensor.read_mm()
sleep(0.02)
determined_speed = 0
if current_distance < MN_DISTANCE and current_distance > TOOCLOSE_DISTANCE:
gopigo3_stationary = True
gopigo3.stop()
sleep(0.02)
elif current_distance < TOOCLOSE_DISTANCE:
gopigo3_stationary = False
percent_speed = float(current_distance -
MN_DISTANCE) / (MX_DISTANCE - MN_DISTANCE)
determined_speed = MN_SPEED + (MX_SPEED - MN_SPEED) * percent_speed
gopigo3.set_speed(determined_speed)
gopigo3.drive_cm(-10, True)
else:
gopigo3_stationary = False
if current_distance == NO_OBSTACLE:
determined_speed = MX_SPEED
else:
percent_speed = float(current_distance - MN_DISTANCE) / (
MX_DISTANCE - MN_DISTANCE)
determined_speed = MN_SPEED + (MX_SPEED -
MN_SPEED) * percent_speed
gopigo3.set_speed(determined_speed)
#gopigo3.forward()
gopigo3.stop()
def __init__(self,
image_model_dir,
cone_model_dir,
image_dir,
cone_image_dir,
params,
boundaries,
log_dir='logs'):
self.gpg = EasyGoPiGo3()
self.dist_sensor = self.gpg.init_distance_sensor(port="AD1")
self.servo = self.gpg.init_servo("SERVO1")
self.servo.rotate_servo(100)
self.params = params
self.boundaries = boundaries
self.image_dir = image_dir
# Image Model
self.image_model = ObjectClassificationModel(model_dir=image_model_dir,
image_dir=image_dir,
min_conf_threshold=0.3,
use_TPU=True)
# Cone Detection Model
self.cone_detection_model = ConeClassificationModel(
model_dir=cone_model_dir,
image_dir=cone_image_dir,
graph_name='cone_detect.tflite',
min_conf_threshold=0.3,
use_TPU=True)
# Log File
if not os.path.exists(log_dir):
os.makedirs(log_dir)
self.log_path = os.path.join(log_dir,
'log_' + str(date.today()) + '.txt')
def Main():
robotCar = EasyGoPiGo3()
while True:
command = speech.run()
if command == 'go':
print(command)
robotCar.forward()
elif command == 'back':
print(command)
robotCar.backward()
elif command == 'left':
print(command)
robotCar.left()
elif command == 'right':
print(command)
robotCar.right()
elif command == 'stop':
print(command)
robotCar.stop()
elif command == 'quit':
print(command)
robotCar.stop()
break
else:
print("No command heard")
continue
import time
import math
from easygopigo3 import EasyGoPiGo3
import easygopigo3 as easy
gpg = easy.EasyGoPiGo3()
gpg = EasyGoPiGo3() # Create object instance of the robot
# Drawing a square:
length = 30 # Side length
for i in range(4):
gpg.drive_cm(length) # Drive forward for length cm
gpg.turn_degrees(90) # Rotate 90 degrees to the right
time.sleep(5) # Time delay between figures
# Drawing a circle:
# uses the .orbit(D, R) command where D is the degrees you'll be
# rotating and R the radius [cm] of said circle.
gpg.orbit(360, 40)
# Drawing a rectangle
short_side = 30
long_side = 60
for i in range(2):
gpg.drive_cm(short_side)
gpg.turn_degrees(90)
gpg.drive_cm(long_side)
gpg.turn_degrees(90)
def squarepath(trigger):
gopigo3_robot = EasyGoPiGo3()
my_distance_sensor = gopigo3_robot.init_distance_sensor()
#set NORTH before staring
right = 1
i = 0
with open('problem1_pathtrace.csv', 'w') as csvfile:
fieldnames = ['row_num', 'encoder_value','distance_value']
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
writer.writeheader()
def drive_and_turn(i,right,dist=999):
gopigo3_robot.reset_encoders()
#drive autonomously until Ctrl-C pressed
#check if an obstacle faced within 250mm (25cm)
while not (trigger.is_set() or dist<=250):
gopigo3_robot.forward()
dist = my_distance_sensor.read_mm()
print("Distance Sensor Reading: {} mm ".format(dist))
#enocder average values to get distance in cm moved
encoders_read = round(gopigo3_robot.read_encoders_average())
#write sensor values to a file
row = [i, encoders_read, dist]
with open('problem1_pathtrace.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
i+=1
if(encoders_read) >= 50:
#if 50 cm crossed, stop and take turn
gopigo3_robot.stop()
if(right):
gopigo3_robot.turn_degrees(90)
else:
gopigo3_robot.turn_degrees(-90)
#recursive drive square
drive_and_turn(i,right)
break
if not trigger.is_set():
#when object within 25cm encountered
gopigo3_robot.stop()
gopigo3_robot.turn_degrees(180)
#travel back rest of the distance (using enooder value which may not be robust value)
#and take a turn(L/R) before continuing the square path again
gopigo3_robot.drive_cm(encoders_read)
if(right):
gopigo3_robot.turn_degrees(-90)
right = 0
else:
gopigo3_robot.turn_degrees(90)
right = 1
drive_and_turn(i,right)
return
if not trigger.is_set():
drive_and_turn(i,right)
#returns when trigger is set
return
def robotControl(trigger, simultaneous_launcher, motor_command_queue, sensor_queue):
"""
Thread-launched function for orientating the robot around. It gets commands from the keyboard as well
as data from the sensor through the sensor_queue 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 motor_command_queue: Queue containing commands from the keyboard. The commands are read from within main.
:param sensor_queue: Processed data off of the IMU. The queue is intended to be read.
:return: Nothing.
"""
time_to_wait_in_queue = 0.1 # measured in
# try to connect to the GoPiGo3
try:
gopigo3_robot = EasyGoPiGo3()
#TODO CODE################
#my_distance_sensor = gopigo3_robot.init_distance_sensor()
#TODO CODE################
except IOError:
print("GoPiGo3 robot not detected")
simultaneous_launcher.abort()
except gopigo3.FirmwareVersionError:
print("GoPiGo3 board needs to be updated")
simultaneous_launcher.abort()
except Exception:
print("Unknown error occurred while instantiating GoPiGo3")
simultaneous_launcher.abort()
# synchronizing point between all threads
# if abort method was called, then the synch will fail
try:
simultaneous_launcher.wait()
except threading.BrokenBarrierError as msg:
print("[robotControl] thread couldn't be launched")
# if threads were successfully synchronized
# then set the GoPiGo3 appropriately
if not simultaneous_launcher.broken:
gopigo3_robot.stop()
gopigo3_robot.set_speed(MOTORS_SPEED)
direction_degrees = None
move = False
acceptable_error_percent = 8
command = "stop"
rotational_factor = 0.30
accepted_minimum_by_drivers = 6
# while CTRL-C is not pressed, the synchronization between threads didn't fail and while the batteries' voltage isn't too low
while not (trigger.is_set() or simultaneous_launcher.broken or gopigo3_robot.volt() <= MINIMUM_VOLTAGE):
# read from the queue of the keyboard
try:
command = motor_command_queue.get(timeout = time_to_wait_in_queue)
motor_command_queue.task_done()
except queue.Empty:
pass
# make some selection depending on what every command represents
if command == "stop":
move = False
elif command == "move":
move = True
if command == "west":
direction_degrees = -90.0
elif command == "east":
direction_degrees = 90.0
elif command == "north":
direction_degrees = 0.0
elif command == "south":
direction_degrees = 180.0
# if a valid orientation was selected
if direction_degrees is not None:
# read data and calculate orientation
heading = sensor_queue.get()
if direction_degrees == 180.0:
heading_diff = (direction_degrees - abs(heading)) * (-1 if heading < 0 else 1)
error = abs(heading_diff / direction_degrees) * 100
else:
heading_diff = direction_degrees - heading
error = abs(heading_diff / 180) * 100
how_much_to_rotate = int(heading_diff * rotational_factor)
if DEBUG is True:
print("direction_degrees {} heading {} error {} heading_diff {}".format(direction_degrees, heading, error, heading_diff))
# check if the heading isn't so far from the desired orientation
# if it needs correction, then rotate the robot
if error >= acceptable_error_percent and abs(how_much_to_rotate) >= accepted_minimum_by_drivers:
gopigo3_robot.turn_degrees(how_much_to_rotate, blocking = True)
# command for making the robot move of stop
if move is False:
gopigo3_robot.stop()
else:
#TODOCODE###########
#set NORTH when staring
squarepath(trigger)
#TODOCODE###########
sleep(0.001)
# if the synchronization wasn't broken
# then stop the motors in case they were running
if not simultaneous_launcher.broken:
gopigo3_robot.stop()
import cv2 as cv import numpy as np import time import select import sys from time import sleep from picamera import PiCamera import picamera.array from easygopigo3 import EasyGoPiGo3 GPG = EasyGoPiGo3() cam = PiCamera() #-----CONFIG----- # Camera WIDTH = 128 HEIGHT = 80 # Canny CANNY_THRESHOLD_1 = 50 CANNY_THRESHOLD_2 = 150 # HoughLines RHO_RESOLUTION = 1 THETA_RESOLUTION = np.pi / 180 VOTE_THRESHOLD = 5 MIN_LINE_LENGTH = 15 MAX_LINE_GAP = 20 # Motion control FORWARD_POWER = 50 #-----FUNCTIONS-----
def runner(event_stopper):
try:
with open('config.json', 'r') as f:
config = json.load(f)
except Exception as err:
logger.exception(err)
event_stopper.set()
return
# start another process to render the graphical representation
# of what the camera sees
queue = MQueue(5)
video_process = Process(target=video_worker, args=(queue, event_stopper))
video_process.start()
try:
# initialize devices
pixy = pixy2.Pixy2I2C(address=0x54)
gopigo3 = EasyGoPiGo3()
logger.info('initialized pixy2 and gopigo3')
# turn on the built-in LED for better visibility
pixy.set_lamp(1)
logger.info('turn on pixy2 lamp')
# set it to follow white lanes
pixy.set_mode(pixy2.LINE_MODE_WHITE_LINE)
logger.info('set pixy2 line mode on white')
# get the number of frames per second
fps = pixy.get_fps()
period = 1 / fps
logger.info('pixy2 running at {} fps'.format(fps))
# camera resolution
width, height = pixy.get_resolution()
logger.info('pixy2 camera width={} height={}'.format(width, height))
# keep previous measurements for smoothing out the data
previous_angle = 0
previous_magnitude = 0
previous_ema_angle = 0
previous_ema_magnitude = 0
frame_index = 0
alfa = config['alfa']
yotta = config['yotta'] # magnitude importance
logger.info('alfa (exp. moving average factor) = {:3.2f}'.format(alfa))
logger.info('yotta (magnitude importance) = {:3.2f}'.format(yotta))
# the 2 PIDs
c1 = PID(Kp=config['pid1']['Kp'],
Ki=config['pid1']['Ki'],
Kd=config['pid1']['Kd'],
previous_error=0.0,
integral_area=0.0)
c2 = PID(Kp=config['pid2']['Kp'],
Ki=config['pid2']['Ki'],
Kd=config['pid2']['Kd'],
previous_error=0.0,
integral_area=0.0)
pid_switch_point = config['pid-switch-point']
logger.info('PID 1 = {}'.format(str(c1)))
logger.info('PID 2 = {}'.format(str(c2)))
logger.info('PID switchpoint = {}'.format(pid_switch_point))
set_point = 0
max_motor_speed = config['max-motor-speed']
logger.info('max motor speed = {}'.format(max_motor_speed))
while not event_stopper.is_set():
# track each step's time
start = time()
# get the main features from the pixy2
features = pixy.get_main_features(features=1)
frame = None
if features:
if 'vectors' in features:
frame = features['vectors']
else:
frame = []
frame_index += 1
else:
gopigo3.stop()
sleep(period)
continue
# calculate the heading of the lane and detect how long the lane is
lanes, win_size, roi = detect_lanes_from_vector_frame(
frame, height, width)
angle, magnitude = calculate_direction_and_magnitude(
lanes, win_size, previous_angle, previous_magnitude)
# run the determined heading and magnitude through an exponential moving average function
# this has the effect of smoothing the results and of ignoring the short term noise
ema_angle = exponential_moving_average(angle, previous_ema_angle,
alfa)
ema_magnitude = exponential_moving_average(magnitude,
previous_ema_magnitude,
alfa)
# save the measurements for the next iteration
previous_angle = angle
previous_magnitude = magnitude
previous_ema_angle = ema_angle
previous_ema_magnitude = ema_magnitude
angle = ema_angle
magnitude = ema_magnitude
# push the processed information to the graphical renderer (on a separate process)
if not queue.full():
queue.put({
'frame_idx': frame_index,
'frame': frame,
'resolution': (height, width),
'fps': fps,
'angle': angle,
'magnitude': magnitude,
'lanes': lanes,
'win_size': win_size
})
# run the pid controller
error = angle - set_point
c1.integral_area += error
c2.integral_area += error
pid1 = True
if magnitude < pid_switch_point:
c = c1
c2.integral_area = 0.0
else:
c = c2
c1.integral_area = 0.0
pid1 = False
# calculate the correction
correction = c.Kp * error + c.Ki * c.integral_area + c.Kd * (
error - c.previous_error)
c1.previous_error = error
c2.previous_error = error
# determine motor speeds
left_motor_speed = int((
(1 - yotta) + yotta * magnitude / 100) * max_motor_speed +
correction)
right_motor_speed = int((
(1 - yotta) + yotta * magnitude / 100) * max_motor_speed -
correction)
logger.debug(
'using PID {} | angle: {:3d} magnitude: {:3d} | left speed: {:3d} right speed: {:3d}'
.format(1 if pid1 else 0, int(angle), int(magnitude),
left_motor_speed, right_motor_speed))
# actuate the motors
gopigo3.set_motor_dps(gopigo3.MOTOR_LEFT, dps=left_motor_speed)
gopigo3.set_motor_dps(gopigo3.MOTOR_RIGHT, dps=right_motor_speed)
# make it run at a specific loop frequency
# equal to camera's fps
end = time()
diff = end - start
remaining_time = period - diff
if remaining_time > 0:
sleep(remaining_time)
# and turn the built-in LED off when the program ends
# and stop the gopigo3
pixy.set_lamp(0)
gopigo3.stop()
except Exception as e:
event_stopper.set()
logger.exception(e)
finally:
video_process.join()
def runner(event_stopper):
try:
pixy = pixy2.Pixy2I2C(address=0x54)
gopigo3 = EasyGoPiGo3()
target_signature = 1
tracked_index = None
# call it once and discard the data
# this is meant to automatically switch to block detection
get_signature_blocks(pixy, sigmap=target_signature)
# pid controller & exponential moving average settings
pid = PID(Kp=400.0,
Ki=0.0,
Kd=0.0,
previous_error=0.0,
integral_area=0.0)
width, height = pixy.get_resolution()
loop_frequency = 60
period = 1 / loop_frequency
alfa = 0.15 # 20%
previous_ema_speed = 0.0
after_how_many_cycles_to_stop = 30
cycle = 0
logger.info('set ' + str(pid))
logger.info('exponential moving average alfa={}'.format(alfa))
logger.info('stop motors after {} cycles of inactivity'.format(
after_how_many_cycles_to_stop))
logger.info('control loop running at {}Hz'.format(loop_frequency))
# ratio-to-speed interpolation
ratio_to_speed = interpolate.interp1d(x=[1, 30],
y=[100, 350],
fill_value=(0, 350),
bounds_error=False)
while not event_stopper.is_set():
start = time()
# get the detected blocks
blocks = get_signature_blocks(pixy, sigmap=target_signature)
# sleep if nothing is detected
if not blocks:
sleep(period)
if cycle % after_how_many_cycles_to_stop == 0:
cycle = 1
gopigo3.stop()
else:
cycle += 1
continue
# initialize the tracked_index if it's the case
if not tracked_index:
tracked_index = blocks[0].track_index
logger.info('change tracked index to {}'.format(tracked_index))
# select the best block based on our tracked index
# if it ain't there, then pick a new one
block = None
for _block in blocks:
if _block.track_index == tracked_index:
block = _block
if not block:
tracked_index = blocks[0].track_index
block = blocks[0]
logger.info('change tracked index to {}'.format(tracked_index))
# gives us the error
error = block.x_center / width - 0.5
# calculate motor speeds based on how far away the object is
block_to_image_ratio = height * width / (block.height *
block.width)
unaveraged_motor_speed = ratio_to_speed(block_to_image_ratio)
base_motor_speed = exponential_moving_average(
unaveraged_motor_speed, previous_ema_speed, alfa)
previous_ema_speed = base_motor_speed
# run the pid controller
pid.integral_area += error
correction = pid.Kp * error + pid.Ki * pid.integral_area + pid.Kd * pid.previous_error
pid.previous_error = error
# calculate motor speeds based on pid's correction
left_motor_speed = int(base_motor_speed + correction)
right_motor_speed = int(base_motor_speed - correction)
logger.debug('motor speed left={} right={}'.format(
left_motor_speed, right_motor_speed))
# actuate the motors
gopigo3.set_motor_dps(gopigo3.MOTOR_LEFT, dps=left_motor_speed)
gopigo3.set_motor_dps(gopigo3.MOTOR_RIGHT, dps=right_motor_speed)
end = time()
remaining_time = period - (end - start)
if remaining_time > 0:
sleep(remaining_time)
gopigo3.stop()
except Exception:
event_stopper.set()
def obstacleFinder(trigger, put_on_hold, simultaneous_launcher, sensor_queue):
leftmost_degrees = 30
rightmost_degrees = 150
current_servo_position = leftmost_degrees
middle = 90
step = 10
servo_delay = 0.035
wait_before_it_starts = 0
distance_trigger = 25
how_much_to_rotate = 150
to_the_right = True
# try to connect to the GoPiGo3
try:
gopigo3_robot = EasyGoPiGo3()
except IOError:
print("GoPiGo3 robot not detected")
simultaneous_launcher.abort()
# initialize a Servo object
servo = gopigo3_robot.init_servo("SERVO1")
# try to connect to the distance sensor
try:
distance_sensor = gopigo3_robot.init_distance_sensor()
except IOError:
print("DistanceSensor couldn't be found")
simultaneous_launcher.abort()
except gopigo3.FirmwareVersionError:
print("GoPiGo3 board needs to be updated")
simultaneous_launcher.abort()
except Exception:
print("Unknown error occurred while instantiating GoPiGo3")
simultaneous_launcher.abort()
# rotate the servo to the desired start-up position
servo.rotate_servo(leftmost_degrees)
sleep(wait_before_it_starts)
# check if an error has occurred during all the above processes
try:
simultaneous_launcher.wait()
except threading.BrokenBarrierError:
print("[obstacleFinder] thread couldn't be launched")
if not simultaneous_launcher.broken:
print("[obstacleFinder] thread fully active")
# and if everything is okay
# start collecting, interpreting and sending messages
# to the thread-launched [robotController] function
while not trigger.is_set() and not simultaneous_launcher.broken:
possible_routes = 0
deadends = 0
sonar_samples = []
# if the thread is put on hold by [robotController]
# then wait until it's allowed to proceed
if not put_on_hold.is_set() and sensor_queue.empty():
# when the servo is rotating to the right
if to_the_right is True:
# rotate to the leftmost_degrees position in case the head is in the middle
servo.rotate_servo(leftmost_degrees)
sleep(servo_delay)
# read the distance to the target at every specific orientation of the servo
# and go with the best greedy-like solution
while current_servo_position <= rightmost_degrees:
servo.rotate_servo(current_servo_position)
sleep(servo_delay)
distance = distance_sensor.read()
current_servo_position += step
possible_routes += 1
# print("left", distance, 90 - current_servo_position)
if distance > distance_trigger:
sonar_samples.append([distance, 90 - current_servo_position])
else:
deadends += 1
to_the_right = False
current_servo_position = rightmost_degrees
# when the servo is rotating to the left
elif to_the_right is False:
# rotate to the leftmost_degrees position in case the head is in the middle
servo.rotate_servo(rightmost_degrees)
sleep(servo_delay)
# read the distance to the target at every specific orientation of the servo
# and go with the best greedy-like solution
while current_servo_position >= leftmost_degrees:
servo.rotate_servo(current_servo_position)
sleep(servo_delay)
distance = distance_sensor.read()
current_servo_position -= step
possible_routes += 1
# print("right", distance, 90 - current_servo_position)
if distance > distance_trigger:
sonar_samples.append([distance, 90 - current_servo_position])
else:
deadends += 1
to_the_right = True
current_servo_position = leftmost_degrees
# remove the following line to make the servo rotational behavior more time-efficient
# by removing it, the time needed to take the measurements is halved
servo.rotate_servo(middle)
# if the sonar (servo + distance sensor) couldn't find a distance >= threshold distance
if deadends == possible_routes:
# then rotate a lot in order to find a new spot
sensor_queue.put([0, how_much_to_rotate])
else:
# do k-means clustering on samples taken from the rotating servo
sample_size = len(sonar_samples)
if sample_size < 3:
kmeans = KMeans(n_clusters = sample_size)
else:
kmeans = KMeans(n_clusters = 3)
kmeans.fit(sonar_samples)
params = kmeans.cluster_centers_
idx = getIndexOfHighestValueInList(params, 0)
# push the orientation which leads to the farthest object detected
# aka takes the "freest" route
sensor_queue.put(params[idx])
sleep(0.01)
# move the servo to the middle when the thread is being stopped
servo.rotate_servo(middle)
def robotController(trigger, put_on_hold, simultaneous_launcher, sensor_queue):
# try to connect to the GoPiGo3
try:
gopigo3_robot = EasyGoPiGo3()
except IOError:
print("GoPiGo3 robot not detected")
simultaneous_launcher.abort()
except gopigo3.FirmwareVersionError:
print("GoPiGo3 board needs to be updated")
simultaneous_launcher.abort()
except Exception:
print("Unknown error occurred while instantiating GoPiGo3")
simultaneous_launcher.abort()
# set a lower speed of the GoPiGo3
gopigo3_robot.set_speed(300)
gopigo3_robot.stop() # in case the GoPiGo3 is moving, stop it
previous = 0 # see the following instructions
how_much_of_distance = 0.30 # measured in percentage
# if the robot is unplugged from the battery pack
# or if the batteries are low
# then abort everything
if gopigo3_robot.volt() <= MINIMUM_VOLTAGE:
print("Voltage too low")
simultaneous_launcher.abort()
# check if an error has occurred during all the above processes
try:
simultaneous_launcher.wait()
except threading.BrokenBarrierError:
print("[robotController] thread couldn't be launched")
if not simultaneous_launcher.broken:
print("[robotController] thread fully active")
# if everything is fine
# start polling messages from [obstacleFinder] function (which is thread-launched)
while not trigger.is_set() and not simultaneous_launcher.broken:
try:
(distance_to_walk, rotation) = sensor_queue.get_nowait()
sensor_queue.task_done()
except queue.Empty:
sleep(0.001)
continue
print("Rotating at {} degrees and driving for {} cm".format(rotation, distance_to_walk * how_much_of_distance))
put_on_hold.set()
gopigo3_robot.turn_degrees(rotation, blocking = True)
gopigo3_robot.drive_cm(distance_to_walk * how_much_of_distance, blocking = False)
# give some time for the robot to start moving
# before taking measurements of its speed
sleep(0.3)
# while the robot is moving and CTRL-C hasn't been pressed
while gopigo3_robot.get_motor_status(gopigo3_robot.MOTOR_LEFT)[3] != 0 and \
gopigo3_robot.get_motor_status(gopigo3_robot.MOTOR_RIGHT)[3] != 0 and \
not trigger.is_set():
sleep(0.001)
put_on_hold.clear()
sleep(0.001)
# stop the motors when the thread is being stopped
gopigo3_robot.stop()
#!/usr/bin/env python import time from easygopigo3 import EasyGoPiGo3 from di_sensors import line_follower car = EasyGoPiGo3() car.stop() car.drive_cm(150) car.stop() car.turn_degrees(270)
def Main():
print(" _____ _____ _ _____ ____ ")
print(" / ____| | __ (_)/ ____| |___ \ ")
print(" | | __ ___ | |__) || | __ ___ __) |")
print(" | | |_ |/ _ \| ___/ | | |_ |/ _ \ |__ < ")
print(" | |__| | (_) | | | | |__| | (_) | ___) |")
print(" \_____|\___/|_| |_|\_____|\___/ |____/ ")
print(" ")
# initializing an EasyGoPiGo3 object and a DistanceSensor object
# used for interfacing with the GoPiGo3 and with the distance sensor
try:
gopigo3 = EasyGoPiGo3()
distance_sensor = gopigo3.init_distance_sensor()
print("intialised objects succesfully")
except IOError as msg:
print("GoPiGo3 robot not detected or DistanceSensor not installed.")
debug(msg)
sys.exit(1)
except FirmwareVersionError as msg:
print("GoPiGo3 firmware needs to updated.")
debug(msg)
sys.exit(1)
except Exception as msg:
print("Error occurred. Set debug = True to see more.")
debug(msg)
sys.exit(1)
if DEBUG is True:
distance_sensor.enableDebug()
# variable that says whether the GoPiGo3 moves or is stationary
# used during the runtime
gopigo3_stationary = True
global robot_operating
# while the script is running
while robot_operating:
# read the distance from the distance sensor
current_distance = distance_sensor.read_mm()
determined_speed = 0
# if the sensor can't be detected
if current_distance == ERROR:
print("Cannot reach DistanceSensor. Stopping the process.")
robot_operating = False
# if the robot is closer to the target
elif current_distance < MIN_DISTANCE:
# then stop the GoPiGo3
gopigo3_stationary = True
gopigo3.stop()
# if the robot is far away from the target
else:
gopigo3_stationary = False
# if the distance sensor can't detect any target
if current_distance == NO_OBSTACLE:
# then set the speed to maximum
determined_speed = MAX_SPEED
else:
# otherwise, calculate the speed with respect to the distance from the target
percent_speed = float(current_distance - MIN_DISTANCE) / (
MAX_DISTANCE - MIN_DISTANCE)
determined_speed = MIN_SPEED + (MAX_SPEED -
MIN_SPEED) * percent_speed
# apply the changes
gopigo3.set_speed(determined_speed)
gopigo3.forward()
# and last, print some stats
print("Current distance : {:4} mm Current speed: {:4} Stopped: {}".
format(current_distance, int(determined_speed),
gopigo3_stationary is True))
# give it some time,
# otherwise you'll have a hard time exiting the script
sleep(0.08)
# and finally stop the GoPiGo3 from moving
gopigo3.stop()
def Main():
print(" _____ _____ _ _____ ____ ")
print(" / ____| | __ (_)/ ____| |___ \ ")
print(" | | __ ___ | |__) || | __ ___ __) |")
print(" | | |_ |/ _ \| ___/ | | |_ |/ _ \ |__ < ")
print(" | |__| | (_) | | | | |__| | (_) | ___) |")
print(" \_____|\___/|_| |_|\_____|\___/ |____/ ")
print(" ")
print(
"To move the robot around using the mouse buttons press 1 and enter.")
print(
"To move the robot around using the movements of the mouse press 2 and enter."
)
# read data from the keyboard
# if it fails reading the right values, then the script exits
try:
choice = int(input("choice (1/2) = "))
except ValueError:
print("Invalid number read")
sys.exit(1)
if not (choice == 1 or choice == 2):
print("Invalid number entered")
sys.exit(1)
# now the choice var can either be 1 or 2
# show different menus depending on the choice var
print(
"\nWith this script you can control your GoPiGo3 robot with a wireless mouse."
)
if choice == 1:
print(
"1. Left + Right buttons of the mouse - move the GoPiGo3 forward")
print("2. Left button of the mouse - move the GoPiGo3 to the left")
print("3. Right button of the mouse - move the GoPiGo3 to the right")
print("4. Middle button of the mouse - move the GoPiGo3 backward")
else:
print("1. Move the mouse forward - for moving the GoPiGo3 forward")
print("2. Move the mouse backward - for moving the GoPiGo3 backward")
print(
"3. Move the mouse to the left - for rotating the GoPiGo3 to the left"
)
print(
"4. Move the mouse to the right - for rotating the GoPiGo3 to the right"
)
# Wait for an input to start
input("\nPress Enter to start")
# create an instance of the EasyGoPiGo3 class
# if it fails instantiating the object, then the scripts exits
try:
robot = EasyGoPiGo3()
except IOError:
print("GoPiGo3 not detected")
sys.exit(1)
except gopigo3.FirmwareVersionError:
print("Please update your GoPiGo3 firmware")
sys.exit(1)
except Exception:
print("Something went wrong")
sys.exit(1)
# stops the robot from moving when exiting the script
# the cleanup_func is called after the signal_handler function
atexit.register(cleanup_func, gopigo3=robot)
print(
"\nIn order to stop the script, press CTRL-C and move your mouse a little bit"
)
# open file for reading the continuous stream of data from the mouse
with open("/dev/input/mice", "rb") as mouse_input:
left_button = 0
middle_button = 0
right_button = 0
x_axis = 0
y_axis = 0
# as long as CTRL-C hasn't been pressed
while signal_not_called:
# read the mouse's values
# this is a blocking function
(left_button, middle_button, right_button, x_axis,
y_axis) = getMouseValues(mouse_input)
# if we have the first choice (see the menu)
if choice == 1:
# when both the mouse's buttons are pressed
# move forward
if left_button == True and right_button == True:
robot.forward()
# when just the left button is pressed
# move to the left
elif left_button == True and right_button == False:
robot.left()
# when just the right button is pressed
# move to the right
elif left_button == False and right_button == True:
robot.right()
# when the middle button is pressed
# move backward
elif middle_button == True:
robot.backward()
# when no button is pressed
# stop the robot from moving
elif middle_button == False or (left_button == False
and right_button == False):
robot.stop()
# if we have the second choice (see the menu)
else:
# if the mouse is moved to the left
# then move the robot to the left
if x_axis < -MOUSE_THRESH:
robot.left()
# if the mouse is moved to the right
# then move the robot to the right
elif x_axis > MOUSE_THRESH:
robot.right()
# if the mouse is moved backward
# then move the robot backward
elif y_axis < -MOUSE_THRESH:
robot.backward()
# if the mouse is moved forward
# then move the robot forward
elif y_axis > MOUSE_THRESH:
robot.forward()
# if the mouse is not moving in any direction
# then stop the robot from moving
else:
robot.stop()
sleep(0.10)
from easygopigo3 import EasyGoPiGo3
import math
import time
from picamera.array import PiRGBArray
from picamera import PiCamera
import argparse
from fanshim import FanShim
fanshim = FanShim()
fanshim.set_fan(True)
utils = raspberry_utils.Utils()
cap = None
easy = EasyGoPiGo3()
easy.set_left_eye_color((10,20,30))
# def center_cone():
# b_box_rec = cones_data['cone-0']['bouding_box_center']
# print(f"Center of the rectangle-{b_box_rec}")
# print(f"Center of the screen-{center_of_the_screen}")
# cv2.line(frame_back, b_box_rec, center_of_the_screen, [0, 255, 0], 3)
# if center_of_the_screen[0] != b_box_rec[0]: # if the angle is not equal to 90 only do thistohis else we are getting divde by 0 error
# angle = int(math.atan((b_box_rec[1] - center_of_the_screen[1])/(b_box_rec[0] - center_of_the_screen[0])) * 180/math.pi )
# else:
# angle = 90
# print(F"Angle is{angle}")
# cv2.putText(frame_back, f"Angle: {angle}", (center_of_the_screen[0]-10, center_of_the_screen[1]-10), cv2.FONT_HERSHEY_SIMPLEX, .5, (0,255, 255), 1, cv2.LINE_AA)
#
# #if b_box_rec[0] > center_of_the_screen[0]:
#######################
### Web Server Stuff ##
#######################
# Directory Path can change depending on where you install this file. Non-standard installations
# may require you to change this directory.
directory_path = '/home/pi/Dexter/GoPiGo3/Projects/RemoteCameraRobot/static'
training_path = '/home/pi/test/training'
data_file = training_path + '/' + 'training_data.csv'
MAX_FORCE = 5.0
MIN_SPEED = 100
MAX_SPEED = 300
try:
gopigo3_robot = EasyGoPiGo3()
except IOError:
logging.critical('GoPiGo3 is not detected.')
sys.exit(1)
except FirmwareVersionError:
logging.critical('GoPiGo3 firmware needs to be updated')
sys.exit(2)
except Exception:
logging.critical("Unexpected error when initializing GoPiGo3 object")
sys.exit(3)
HOST = "0.0.0.0"
WEB_PORT = 5000
app = Flask(__name__, static_url_path='')
state = 'stop'
#record while drive
from scavear_main import Listener
from scavear_main import Scavear
#import multiprocessing as mp
from threading import Thread
import time
from easygopigo3 import EasyGoPiGo3
from datetime import date
gpg = EasyGoPiGo3()
def drive(seconds = 150):
now = time.time()
while time.time() - now < seconds:
drive_rectangle()
def drive_rectangle(side=5):
# gpg.set_speed(h_spd)
gpg.forward()
time.sleep(side)
gpg.stop()
gpg.turn_degrees(95)
gpg.forward()
time.sleep(side)
gpg.stop()
gpg.turn_degrees(95)
gpg.forward()
def stop():
gpg = EasyGoPiGo3()
gpg.stop()
