class Control:
''' rover control class '''
def __init__(self, left=LEFT, right=RIGHT):
''' Constructor '''
self.drive = Robot(left=left, right=right)
def off(self):
''' Stop all motors '''
self.drive.stop()
def cmd(self, char, step=STEP):
''' Perform required command '''
if char == 'f':
self.drive.forward(SPEED)
time.sleep(step)
elif char == 'b':
self.drive.backward(SPEED)
time.sleep(step)
elif char == 'r':
self.drive.right(SPEED)
time.sleep(step)
elif char == 'l':
self.drive.left(SPEED)
time.sleep(step)
elif char == '#':
time.sleep(step)
class Movement:
def __init__(self, logger):
self._logger = logger
self._robot = Robot(left=(24, 23, 21), right=(20, 16, 18))
self._consumer = Consumer('http://192.168.1.183:8181/hub', ['movement'], self._move)
def _up(self):
self._robot.forward()
time.sleep(2)
self._robot.stop()
print('up')
def _down(self):
self._robot.backward()
time.sleep(2)
self._robot.stop()
print('down')
def _left(self):
self._robot.left()
time.sleep(2)
self._robot.stop()
print('left')
def _right(self):
self._robot.right()
time.sleep(2)
self._robot.stop()
print('right')
def _move(self, message):
switcher = {
'up': self._up,
'down': self._down,
'left': self._left,
'right': self._right,
}
try:
movement_data = json.loads(message.data)
# Get the function from switcher dictionary
movement = switcher.get(movement_data['direction'], lambda: "Invalid month")
# Execute the function
movement()
except Exception as e:
self._logger.error(str(e))
def start(self):
self._consumer.start_consumption()
self._logger.info("Movemement connected")
class Robobo:
def __init__(self):
self.robot = Robot(left=(7, 8), right=(10, 9))
self.sensor = DistanceSensor(15, 18)
self.direction = "forward"
self.isCameraActive = 0
def move(self, direction, speed):
self.direction = direction
if direction == "forward":
dist = self.getDistance()
if dist > 20:
self.robot.forward()
if direction == "backward":
self.robot.backward()
if direction == "left":
self.robot.left()
if direction == "right":
self.robot.right()
if direction == "stop":
self.robot.stop()
def getCameraStatus(self):
return self.isCameraActive
def camera(self, state):
if state == "start":
os.system('sudo /bin/sh /var/www/html/robotApi/runCamera.sh pi')
self.isCameraActive = 1
if state == "stop":
os.system('sudo /bin/sh /var/www/html/robotApi/stopCamera.sh pi')
self.isCameraActive = 0
def getDistance(self):
if self.direction == "forward" and round(self.sensor.distance * 100,
1) < 20:
self.robot.stop()
return round(self.sensor.distance * 100, 1)
return round(self.sensor.distance * 100, 1)
def main():
""" メイン関数 """
# 接続ピン
PIN_AIN1 = 6
PIN_AIN2 = 5
PIN_BIN1 = 26
PIN_BIN2 = 27
# 左右モーター設定(ON/OFF)
motors = Robot(left=(PIN_AIN1, PIN_AIN2),
right=(PIN_BIN1, PIN_BIN2),
pwm=False)
# ループ処理
while True:
# 0.2秒前進
motors.forward()
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
# 0.2秒後退
motors.backward()
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
# 0.2秒左旋回
motors.left()
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
# 0.2秒右旋回
motors.right()
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
def go_right(n_seconds):
robot.right(0.5)
sleep(n_seconds)
robot.stop()
sensor = DistanceSensor(echo=18, trigger=17)
robot.forward(0.3)
while True:
#print('Distance:', sensor, distance)
if sensor.distance > 0.01:
robot.forward(0.4)
if sensor.distance < 0.1:
print("i am too close")
robot.left(0.25)
if sensor.distance > 0.1:
print("opps iam going too far")
robot.right(0.25)
robot.stop()
go_forward(0.2)
go_left(0.25)
go_right(0.25)
from gpiozero import Robot
from time import sleep
robot = Robot(left=(4, 14), right=(17, 18))
for i in range(4):
robot.forward()
sleep(10)
robot.right()
sleep(1)
from time import sleep from gpiozero import Robot bot = Robot(left = (7, 8), right = (9, 10)) #7,8,9,10 are the raspberry pie GPIO pin marking while True: bot.forward() sleep(3) #sleep is used to import time bot.stop() bot.right() sleep(1) bot.stop()
pulse_end = time()
return pulse_end - pulse_start
def calculate_distance(duration):
speed = 343
distance = speed * duration / 2
return distance
while running:
duration = get_pulse_time()
distance = calculate_distance(duration)
if distance < 0.3:
olivia.left()
sleep(0.4)
olivia.forward()
sleep(1.1)
olivia.right()
sleep(0.5)
else:
olivia.forward()
if time() >= end_time:
running = False
olivia.stop()
sleep(0.06)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN,old_setting)
return ch
try:
us = UltraSound(6,12)
robot = Robot(left=(23,22),right=(17,18))
speed=1.0
while True:
obstdistance = us.measure()
print(obstdistance)
if obstdistance < 15:
speed=0.5
robot.stop()
robot.right(speed)
time.sleep(0.01)
robot.reverse(speed)
time.sleep(2)
speed =1.0
robot.forward(speed)
key = getchar()
if key == "w":
robot.stop()
time.sleep(0.01)
robot.forward(speed)
elif key == "z":
robot.stop()
time.sleep(0.01)
robot.backward(speed)
# Wait until input goes high - then wait for it to do low
while (GPIO.input(pin_echo) == 0):
pass
# Start timer
start_time = time.time()
# wait until it goes low again
while (GPIO.input(pin_echo) == 1):
current_time = time.time()
# If no response in reasonable time then not received a response (either too close to an object - or too far)
if ((current_time - start_time) > 0.05):
break
# Calculate response time
response_time = current_time - start_time
#distance is time * speed sound (34029cm/s) - divide by 2 for return journey
distance = response_time * 34029 / 2
# if we are close to a wall then turn right
if (distance < min_distance):
print("Too close " + str(distance) + " turning")
robot.right(speed_turn)
else:
robot.forward(speed_forward)
robot.close()
restore_terminal()
GPIO.cleanup()
from gpiozero import Robot
from time import sleep
# left/right=(전진,후진,PWM)
car = Robot(left=(17, 27, 22), right=(15, 18, 14), pwm=True)
while True:
cmd = input("> ")
if cmd == 'q':
break
elif cmd == 'l':
car.left(0.4)
elif cmd == 'r':
car.right(0.4)
elif cmd == 'g':
car.forward(0.3)
elif cmd == 'b':
car.backward(0.3)
elif cmd == 's':
car.stop()
else:
car.stop()
size = len(frame)
distance = distance_sensor.distance()
connection.write(struct.pack('<L', int(distance)))
connection.write(struct.pack('<L', size))
connection.write(frame)
connection.flush()
length = struct.unpack('<L', connection.read(struct.calcsize('<L')))[0]
action = connection.read(length).decode()
print(action)
if action == "stop":
robby.stop()
elif action == 'Normal_GO':
robby.backward(0.75)
elif action == 'Limit40_GO':
robby.backward(0.60)
elif action == 'left':
robby.left(1)
time.sleep(0.2)
elif action == 'right':
robby.right(1)
time.sleep(0.2)
elif action == 'Withdraw':
robby.forward(1)
time.sleep(0.8)
robby.left(1)
time.sleep(0.5)
finally:
connection.close()
client_socket.close()
from gpiozero import Robot from time import sleep blackWinter = Robot(left=(7, 8), right=(9, 10)) blackWinter.forward() sleep(1) blackWinter.stop() blackWinter.right() sleep(.55) blackWinter.stop() blackWinter.forward() sleep(2) blackWinter.stop() blackWinter.right() sleep(.55) blackWinter.stop() blackWinter.forward() sleep(2) blackWinter.stop() blackWinter.right() sleep(.55) blackWinter.stop() blackWinter.forward() sleep(1) blackWinter.stop()
from gpiozero import Robot from time import sleep motor = Robot(left=(17,18), right=(27,22)) motor.forward() sleep(2) motor.right(0.4) sleep(3) motor.stop()
from gpiozero import Robot # importamos todo lo modulos necesarios from time import sleep robby = Robot(left=(7, 8), right=(9, 10)) # definimos las conexiones del robot robby.forward(0.4) # nos movemos hacia adelante con 40% de velocidad sleep(5) # esperamos 5 segundos robby.right(0.4) # nos giramos a la derecha con 40% de velocidad sleep(5) # esperamos 5 segundos robby.stop() # paramos
from gpiozero import Robot
from time import sleep
# left/right=(전진, 후진, PWM)
car = Robot(left=(17, 27, 22), right=(15, 18, 14), pwm=True)
while True:
cmd = input("> ")
if cmd == "q":
break
elif cmd == 'l':
car.left(0.5)
elif cmd == 'r':
car.right(0.5)
elif cmd == 'g':
car.forward(0.3)
elif cmd == 'b':
car.backward(0.3)
elif cmd == 's':
car.stop()
else:
car.stop()
myRob = Robot(left=(7, 8), right=(9, 10))
controller = None
devices = [evdev.InputDevice(path) for path in evdev.list_devices()]
for device in devices:
if device.name == 'PC Game Controller':
controller = evdev.InputDevice(device.path)
for event in controller.read_loop():
if event.type == 3:
if event.code == 1: # Up and Down arrows
if event.value == 0:
print("Robot go forward:")
myRob.forward()
elif event.value == 255:
print("Robot go backward")
myRob.backward()
else:
print("Robot stopped")
myRob.stop()
if event.code == 0:
if event.value == 0:
print("Robot go left")
myRob.left()
if event.value == 255:
print("Robot turn right")
myRob.right()
if event.value == 128:
print("Robot no horizontal")
myRob.stop()
from gpiozero import Robot from time import sleep robot = Robot(left=(18, 3), right=(15, 14)) speed = 0.6 robot.forward(speed=speed) sleep(1) robot.backward(speed=speed) sleep(1) robot.left(speed=speed) sleep(1) robot.right(speed=speed) sleep(1) robot.stop()
from gpiozero import Robot from time import sleep speed = 0.4 motors = Robot(right=(3, 4), left=(2, 14)) motors.forward(speed=speed) sleep(1) motors.left(speed=speed) sleep(1) motors.right(speed=speed) sleep(1) motors.backward(speed=speed) sleep(1) motors.stop()
from gpiozero import Robot
from time import sleep
robotfront = Robot(left=(4, 14), right=(17, 18))
robotback = Robot(left=(22, 23), right=(24, 25))
robotfront.forward(speed=0.5)
robotback.forward(speed=0.5)
print(" Going Forward All Four Wheels")
sleep(5)
robotback.stop()
robotfront.right()
print(" Turning Right")
sleep(5)
robotfront.forward(speed=1.0)
print(" Going Forward front wheels only")
sleep(5)
robotfront.stop()
robotback.forward(speed=1.0)
print(" Going Forward back wheels only")
sleep(5)
robotback.stop()
print(" Turning Left Back wheels only")
robotback.left(speed=0.75)
sleep(5)
robotfront.stop()
robotback.reverse()
print(" Going Backwards back wheels only")
sleep(5)
robotfront.stop()
robotback.stop()
wii.rpt_mode = cwiid.RPT_BTN
while True:
last_direction = current_direction
# Convert speed from percentage to float (0 to 1)
float_speed = speed / 100
if (current_direction == "forward"):
robot.forward(float_speed)
# rev
elif (current_direction == "backward"):
robot.backward(float_speed)
elif (current_direction == "left"):
robot.left(float_speed)
elif (current_direction == "right"):
robot.right(float_speed)
# stop
else:
robot.stop()
time.sleep(delay)
# Get next key pressed
buttons = wii.state["buttons"]
# set button to stop so that if no buttons pressed we stop
current_direction = "stop"
# + and - = quit
if ((buttons & cwiid.BTN_PLUS) and (buttons & cwiid.BTN_MINUS)):
break
wii.rpt_mode = cwiid.RPT_BTN
while True:
last_direction = current_direction
# Convert speed from percentage to float (0 to 1)
float_speed = speed / 100
if (current_direction == "forward") :
robot.forward(float_speed)
# rev
elif (current_direction == "backward") :
robot.backward(float_speed)
elif (current_direction == "left") :
robot.left(float_speed)
elif (current_direction == "right") :
robot.right(float_speed)
# stop
else :
robot.stop()
time.sleep(delay)
# Get next key pressed
buttons = wii.state["buttons"]
# set button to stop so that if no buttons pressed we stop
current_direction = "stop"
# + and - = quit
if ((buttons & cwiid.BTN_PLUS) and (buttons & cwiid.BTN_MINUS)) :
break
#define a robot (it's called Burt! :-) ), with the GPIO pin mapping as per the GPIO in the RobotV2.md file
burt_the_robot = Robot(
left=(8, 7), right=(21, 20)
) # dont change this pin mapping, otherwise your robot will be different to the others!
#set the speed. 1 = 100%, 0.5 = 50% and so on...
speed = 0.7
#go forward indefinitely
burt_the_robot.forward(speed)
#sleep for 2seconds
sleep(2)
#spin right indefinitely
burt_the_robot.right(speed)
#sleep for 2seconds
sleep(2)
#spin left indefinitely
burt_the_robot.left(speed)
#sleep for 2seconds
sleep(2)
#go backwards indefinitely
burt_the_robot.backward(speed)
#sleep for 2seconds
sleep(2)
class FollowBot(object):
def __init__(self):
GPIO.cleanup()
self.__leftencoder = Encoder(21)
self.__rightencoder = Encoder(27)
self.__robot = Robot(left=(23, 24), right=(26, 22))
self.__en1 = 12
self.__en2 = 13
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.__en1, GPIO.OUT)
GPIO.output(self.__en1, GPIO.HIGH)
GPIO.setup(self.__en2, GPIO.OUT)
GPIO.output(self.__en2, GPIO.HIGH)
def moveforward(self, dis, speed):
enc1 = 0
enc2 = 0
SAMPLETIME = 0.125
TARGET = speed
KP = 0.02
e1 = self.__leftencoder
e2 = self.__rightencoder
m1_speed = 0
m2_speed = 0
while (enc1 < 2435 * dis):
print("e1 {} e2 {}".format(e1.value, e2.value))
e1_error = TARGET - e1.value
e2_error = TARGET - e2.value
m1_speed += e1_error * KP
m2_speed += e2_error * KP
m1_speed = max(min(1, m1_speed), 0)
m2_speed = max(min(1, m2_speed), 0)
self.__robot.value = (m1_speed, m2_speed)
self.__robot.forward()
enc1 = enc1 + e1.value
enc2 = enc2 + e2.value
e1.reset()
e2.reset()
sleep(SAMPLETIME)
self.__robot.stop()
def movebackward(self, dis, speed):
enc1 = 0
enc2 = 0
SAMPLETIME = 0.125
TARGET = speed
KP = 0.02
e1 = self.__leftencoder
e2 = self.__rightencoder
m1_speed = 0
m2_speed = 0
while (enc1 < 2435 * dis):
print("e1 {} e2 {}".format(e1.value, e2.value))
e1_error = TARGET - e1.value
e2_error = TARGET - e2.value
m1_speed += e1_error * KP
m2_speed += e2_error * KP
m1_speed = max(min(1, m1_speed), 0)
m2_speed = max(min(1, m2_speed), 0)
self.__robot.value = (m1_speed, m2_speed)
self.__robot.backward()
enc1 = enc1 + e1.value
enc2 = enc2 + e2.value
e1.reset()
e2.reset()
sleep(SAMPLETIME)
self.__robot.stop()
def reset(self):
e1 = self.__leftencoder
e2 = self.__rightencoder
e1.reset()
e2.reset()
def rotateLeft(self, angle):
enc1 = 0
SAMPLETIME = 0.125
n = 1052
e1 = self.__leftencoder
e2 = self.__rightencoder
while (e1.value < n * angle / 90.0):
self.__robot.left()
print("{} -{}- {}".format(e1.value, e2.value, n * angle / 90.0))
sleep(SAMPLETIME)
self.__robot.stop()
def rotateRight(self, angle):
enc2 = 0
SAMPLETIME = 0.125
n = 590
e1 = self.__leftencoder
e2 = self.__rightencoder
while (e2.value < n * angle / 90.0):
self.__robot.left()
self.__robot.right()
print("{} -{}- {}".format(e1.value, e2.value, n * angle / 90.0))
sleep(SAMPLETIME)
self.__robot.stop()
from gpiozero import Robot
from time import sleep
robby = Robot(left=(11, 9), right=(13, 10))
speed = 1.0
turnspeed = 0.8
robby.forward(speed)
print("forward")
sleep(3)
robby.right(turnspeed)
print("right")
sleep(3)
robby.forward(speed)
print("forward")
sleep(3)
robby.left(turnspeed)
print("left")
sleep(3)
print("Stop")
robby.stop()
def ziellos():
car = Robot(motor_L, motor_R)
random.seed()
richtungsliste = []
links = 0
vorne = 1
rechts = 2
hinten = 3
abstand = 40
abstand_vorne = 10
abstand_fehlerwert = 1700
while True:
vorne_d = distance(GPIO_ECHO_V)
rechts_d = distance(GPIO_ECHO_R)
links_d = distance(GPIO_ECHO_L)
hinten_d = distance(GPIO_ECHO_H)
car.forward() # fahre erstmal vorwärts
#füge erlaubte richtungen basierend auf dem abstand zur liste hinzu
if vorne_d > abstand_vorne:
car.stop()
vorne_d = distance(GPIO_ECHO_V)
if vorne_d > abstand_vorne: richtungsliste.append(vorne)
if links_d > abstand:
car.stop()
links_d = distance(GPIO_ECHO_L)
if links_d > abstand and links_d < abstand_fehlerwert:
richtungsliste.append(links)
if rechts_d > abstand:
car.stop()
rechts_d = distance(GPIO_ECHO_R)
if rechts_d > abstand and rechts_d < abstand_fehlerwert:
richtungsliste.append(rechts)
if not richtungsliste: richtungsliste.append(hinten)
print(richtungsliste)
richtung = random.choice(richtungsliste)
richtungsliste.clear()
# wähle richtung und reagiere dementsprechend
if richtung == links:
print("links!\t vorne: ", vorne_d, "\t rechts :", rechts_d,
"\t links: ", links_d, "\t hinten: ", hinten_d)
if vorne > abstand_vorne:
car.forward(0.6)
time.sleep(0.45)
car.left(0.77)
time.sleep(1.25)
car.forward(0.6)
time.sleep(1)
elif richtung == vorne:
car.forward(0.6)
elif richtung == rechts:
print("links!\t vorne: ", vorne_d, "\t rechts :", rechts_d,
"\t links: ", links_d, "\t hinten: ", hinten_d)
if vorne > abstand_vorne:
car.forward(0.6)
time.sleep(0.45)
car.right(0.77)
time.sleep(1.25)
car.forward(0.6)
time.sleep(1)
elif richtung == hinten:
print("links!\t vorne: ", vorne_d, "\t rechts :", rechts_d,
"\t links: ", links_d, "\t hinten: ", hinten_d)
car.left(0.6)
time.sleep(1)
class FollowBot(object):
def __init__(self):
GPIO.cleanup()
self.__leftencoder = Encoder(21)
self.__rightencoder = Encoder(27)
self.__robot = Robot(left=(23, 24), right=(26, 22))
self.__en1 = 12
self.__en2 = 13
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.__en1, GPIO.OUT)
GPIO.output(self.__en1, GPIO.HIGH)
GPIO.setup(self.__en2, GPIO.OUT)
GPIO.output(self.__en2, GPIO.HIGH)
def moveforward(self, dis, speed):
enc1 = 0
enc2 = 0
SAMPLETIME = 0.125
TARGET = speed
KP = 0.02
e1 = self.__leftencoder
e2 = self.__rightencoder
m1_speed = 0
m2_speed = 0
while (enc1 < 2435 * dis):
print("e1 {} e2 {}".format(e1.value, e2.value))
e1_error = TARGET - e1.value
e2_error = TARGET - e2.value
m1_speed += e1_error * KP
m2_speed += e2_error * KP
m1_speed = max(min(1, m1_speed), 0)
m2_speed = max(min(1, m2_speed), 0)
self.__robot.value = (m1_speed, m2_speed)
self.__robot.forward()
enc1 = enc1 + e1.value
enc2 = enc2 + e2.value
e1.reset()
e2.reset()
sleep(SAMPLETIME)
self.__robot.stop()
def reset(self):
e1 = self.__leftencoder
e2 = self.__rightencoder
e1.reset()
e2.reset()
def rotateLeft(self, angle):
enc1 = 0
SAMPLETIME = 0.125
n = 1052
e1 = self.__leftencoder
e2 = self.__rightencoder
while (e1.value < n * angle / 90.0):
self.__robot.left()
print("{} -{}- {}".format(e1.value, e2.value, n * angle / 90.0))
sleep(SAMPLETIME)
self.__robot.stop()
def rotateRight(self, angle):
enc2 = 0
SAMPLETIME = 0.125
n = 590
e1 = self.__leftencoder
e2 = self.__rightencoder
while (e2.value < n * angle / 90.0):
self.__robot.left()
self.__robot.right()
print("{} -{}- {}".format(e1.value, e2.value, n * angle / 90.0))
sleep(SAMPLETIME)
self.__robot.stop()
def Zlocation():
xyzlocation = rospy.Subscriber('/visp_auto_tracker/object_position',
xyzposition, callback)
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I %s", data.data)
def scanLidar():
rospy.init_node('scanLidar', anonymous=True)
rospy.Subscriber("/scan", LaserScan, callback)
rospy.spin()
def fahr_los_righthand():
car = Robot(motor_L, motor_R)
abstand = 40
abstand_vorne = 10
abstand_fehlerwert = 1700
while True:
vorne = distance(GPIO_ECHO_V)
rechts = distance(GPIO_ECHO_R)
links = distance(GPIO_ECHO_L)
hinten = distance(GPIO_ECHO_H)
# solange rechts kein gang fahr gradeaus
if rechts < abstand and vorne > abstand_vorne:
car.forward(0.6)
print("vorwärts!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
# Wenn gang rechts dann stop
elif rechts > abstand:
car.stop()
time.sleep(0.1)
print("rechts versuch!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
rechts = distance(GPIO_ECHO_R)
# Erneute Prüfung von Abstand rechts
if rechts > abstand and rechts < abstand_fehlerwert:
if vorne > abstand_vorne:
car.forward(0.6)
time.sleep(0.37)
car.right(0.6)
print("rechts!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
time.sleep(1.25) # drehe nach rechts
car.forward(0.6)
time.sleep(1.3)
# wenn nur links gang fahre links
elif rechts < abstand and vorne < abstand and links > abstand:
car.stop()
print("links versuch!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
time.sleep(0.1)
links = distance(GPIO_ECHO_L)
if links > abstand and links < abstand_fehlerwert:
if vorne > abstand_vorne:
car.forward(0.6)
time.sleep(0.37)
car.left(0.6)
print("links!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
time.sleep(1.25)
car.forward(0.6)
time.sleep(1)
# Sackgasse. wende auto
elif hinten > abstand_vorne:
car.stop()
time.sleep(0.1)
print("wenden versuch!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
vorne = distance(GPIO_ECHO_V)
rechts = distance(GPIO_ECHO_R)
links = distance(GPIO_ECHO_L)
hinten = distance(GPIO_ECHO_H)
if rechts < abstand and vorne < abstand and links < abstand and hinten > abstand_vorne:
car.left(0.6)
print("wenden!\t vorne: ", vorne, "\t rechts :", rechts,
"\t links: ", links, "\t hinten: ", hinten)
time.sleep(1.1)
class Driver:
"""Controls the motors and the motion direction and speed.
"""
_NORMAL_SPEED = 0.5
_TURBO_SPEED = 1.0
def __init__(self):
self._commands = [
0, # forward
0, # backward
0, # left
0, # right
0, # turbo
]
self._robot = Robot(left=(_LEFT_MOTOR_NEG_PIN,
_LEFT_MOTOR_POS_PIN),
right=(_RIGHT_MOTOR_POS_PIN,
_RIGHT_MOTOR_NEG_PIN))
# A Driver exposes a number of multiprocess Events objects that
# external objects can use to signal the need of an emergency stops.
# It is up to the caller to clear the safety stop Event.
self._safety_stop_event = mp.Event()
self._safety_stop_forward_event = mp.Event()
self._safety_stop_backward_event = mp.Event()
_logger.debug('{} initialized'.format(self.__class__.__name__))
def _move(self):
if self._safety_stop_event.is_set():
if self._robot.left_motor.is_active or self._robot.right_motor.is_active:
# Both motors must be completely still.
self._robot.stop()
# Not further actions allowed in case of full safety stop.
return
# In case of forward/backward safety stop, motors cannot spin in the
# same forbidden direction. At most one is allowed to let the robot
# spin in place.
if self._safety_stop_forward_event.is_set():
if self._robot.left_motor.value > 0 and self._robot.right_motor.value > 0:
self._robot.stop()
return
if self._safety_stop_backward_event.is_set():
if self._robot.left_motor.value < 0 and self._robot.right_motor.value < 0:
self._robot.stop()
return
if sum(self._commands[:4]) == 0:
# All the motion commands are unset: stop the motors.
self._robot.stop()
return
# Setting both "forward" and "backward" or "left" and "right"
# is not allowed. Maintain the current course.
if (self._commands[COMMAND_FORWARD] and self._commands[COMMAND_BACKWARD]) or \
(self._commands[COMMAND_LEFT] and self._commands[COMMAND_RIGHT]):
_logger.warning('Invalid command configuration')
return
speed = self._TURBO_SPEED if self._commands[COMMAND_TURBO] \
else self._NORMAL_SPEED
if not self._commands[COMMAND_FORWARD] and not self._commands[COMMAND_BACKWARD]:
# Only left-right commands provided.
if self._commands[COMMAND_LEFT]:
self._robot.left(speed)
elif self._commands[COMMAND_RIGHT]:
self._robot.right(speed)
else:
assert False, 'Reached unexpected condition'
else:
# Move forward or backward, possible also turning left or right.
kwargs = dict(speed=speed)
# We already checked that left and right cannot be set together.
if self._commands[COMMAND_LEFT]:
kwargs['curve_left'] = 0.5
elif self._commands[COMMAND_RIGHT]:
kwargs['curve_right'] = 0.5
# We already checked that forward and backward cannot be set together.
if self._commands[COMMAND_FORWARD]:
if self._safety_stop_forward_event.is_set():
return
self._robot.forward(**kwargs)
elif self._commands[COMMAND_BACKWARD]:
if self._safety_stop_backward_event.is_set():
return
self._robot.backward(**kwargs)
def set_command(self, command_code, command_value):
"""Receives an external command, stores it and processes it.
Args:
command_code (int): What command to execute.
command_value (int): The value associated with this command. Often
1 to set and 0 to cancel.
"""
if command_code < 0 or command_code >= len(self._commands):
# Unrecognized command.
_logger.warning('Unrecognized command code: '
'{}'.format(command_code))
return
self._commands[command_code] = command_value
self._move()
def stop(self):
"""Stops all the motors at the same time.
"""
for idx in range(len(self._commands)):
self._commands[idx] = 0
self._move()
@property
def safety_stop_event(self):
return self._safety_stop_event
@property
def safety_stop_forward_event(self):
return self._safety_stop_forward_event
@property
def safety_stop_backward_event(self):
return self._safety_stop_backward_event
def close(self):
self._robot.stop()
self._robot.close()
_logger.debug('{} stopped'.format(self.__class__.__name__))
if cmd[0] == 'f':
print('Forward', end=' ')
robot.forward(0.4)
if duration == 'd':
inch_forward()
elif cmd[0] == 'b':
print('Backward', end=' ')
robot.backward(0.4)
elif cmd[0] == 'l':
print('Left', end=' ')
robot.left(0.4)
elif cmd[0] == 'r':
print('Right', end=' ')
robot.right(0.4)
elif cmd[0] == 'd':
print("Distance")
distance()
try:
duration = float(duration)
except ValueError:
duration = 0
if duration > 0:
print(duration)
time.sleep(duration / 7)
robot.stop()
from gpiozero import DistanceSensor from gpiozero import Robot from time import sleep from signal import pause robot = Robot(right=(19, 26), left=(6, 13)) robot.forward() sleep(5) robot.right(0.5) sleep(5)
class R2_D2():
def __init__(self):
#movement variables
self.legs = Robot(left=(5, 6), right=(17, 27))
self.turning_right = False
self.turning_left = False
self.moving_forward = False
self.moving_backward = False
#light variables
#used to blink red-blue light
self.light = RGBLED(red=16, green=20, blue=21)
self.light.color = (1, 0, 0)
self.red = True
self.time_since_last_blink = 0
#function calls Robot.right() if not already moving right
def turn_right(self):
if (not self.turning_right):
self.legs.right()
self.turning_right = True
#funtion calls Robot.left() if not already moving left
def turn_left(self):
if (not self.turning_left):
self.legs.left()
self.turning_left = True
#function calls Robot.forward() if not already moving forward
def move_forward(self):
if (not self.moving_forward):
self.legs.forward()
self.moving_forward = True
#function calls Robot.backward() if not already moving backward
def move_backward(self):
if (not self.moving_backward):
self.legs.backward()
self.moving_backward = True
#functions stops all robot movements, sets all movement variables to false
def stop_movement(self):
self.turning_right = False
self.turning_left = False
self.moving_forward = False
self.moving_backward = False
moving = False
keys = pygame.key.get_pressed()
if (keys[pygame.K_UP]):
self.move_forward()
moving = True
elif (keys[pygame.K_RIGHT]):
self.turn_right()
moving = True
elif (keys[pygame.K_DOWN]):
self.move_backward()
moving = True
elif (keys[pygame.K_LEFT]):
self.turn_left()
moving = True
if (not moving):
self.legs.stop()
#plays a sound based on given input, does nothing if input invalid
#assumes pygame is initialized
def play_sound(self, sound_name):
sound_location = './sounds/' + sound_name + '.wav'
sound = pygame.mixer.Sound(sound_location)
pygame.mixer.Sound.play(sound)
#updates all time dependent component of R2-D2
#expected delta_time in seconds
def update(self, delta_time):
#update light
self.update_light(delta_time)
#keeps track of time since last light blink and blinks light
#if that time is greater than 1 second.
#delta time parameter is in seconds
def update_light(self, delta_time):
self.time_since_last_blink = self.time_since_last_blink + delta_time
#blink about every 1 second
if (self.time_since_last_blink >= 1.0):
self.time_since_last_blink = 0
if (self.red):
self.red = False
self.light.color = (0, 0, 1)
else:
self.red = True
self.light.color = (1, 0, 0)
t1 = threading.Thread(target=print_square)
#t2 = threading.Thread(target=user_input)
# starting thread 1
t1.start()
# starting thread 2
#t2.start()
while 1:
i=input("Please enter a number: ")
print ('wrong input')
if i== 'w':
robot.forward()
elif i=='s':
robot.backward()
elif i=='a':
robot.left()
elif i=='d':
robot.right()
elif i=='x':
robot.stop()
elif i=='r':
speed+=0.1
elif i=='f':
speed-=0.1
else:
print ('wrong input')
en1.value=speed
# Wait until input goes high - then wait for it to do low
while (GPIO.input(pin_echo)==0):
pass
# Start timer
start_time = time.time()
# wait until it goes low again
while (GPIO.input (pin_echo)==1):
current_time = time.time()
# If no response in reasonable time then not received a response (either too close to an object - or too far)
if ((current_time - start_time)>0.05):
break
# Calculate response time
response_time = current_time - start_time;
#distance is time * speed sound (34029cm/s) - divide by 2 for return journey
distance = response_time * 34029 / 2
# if we are close to a wall then turn right
if (distance < min_distance) :
print ("Too close " + str(distance) + " turning")
robot.right(speed_turn)
else :
robot.forward(speed_forward)
robot.close()
restore_terminal()
GPIO.cleanup()
