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 MadmaxWheelbase():
def __init__(self):
self.wheel_base = Robot(
left=(sets['motor_in1_left'],sets['motor_in2_left'],sets['motor_enabler_left']),
right=(sets['motor_in3_right'],sets['motor_in4_right'],sets['motor_enabler_right']))
self.speed = 0.3
def forward(self, speed = None):
if speed:
self.speed = speed
self.wheel_base.forward(self.speed)
def backward(self, speed = None):
if speed:
self.speed = speed
self.wheel_base.backward(self.speed)
def stop(self):
self.wheel_base.stop()
def set_speed(self, speed):
self.speed = speed
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 Jeeno:
def __init__(self):
self.front_sensor = DistanceSensor(echo = 22, trigger = 27)
#self.back_sensor = DistanceSensor(echo = , trigger = )
self.robot = Robot(left=(6,12), right=(5,13))
self.move()
def move(self):
self.front_sensor.when_out_of_range = self.robot.forward
self.front_sensor.when_in_range = self.turn
def turn(self):
self.robot.stop()
if choice(['L','R']) == 'L':
self.left()
else:
self.right()
def left(self):
self.robot.forward(curve_left=1)
sleep(1)
self.robot.stop()
def right(self):
self.robot.forward(curve_right=1)
sleep(1)
self.robot.stop()
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
# 左右モーター設定(PWM)
motors = Robot(left=(PIN_AIN1, PIN_AIN2),
right=(PIN_BIN1, PIN_BIN2),
pwm=True)
# ループ処理
while True:
# 0.2秒前進(50%)
motors.forward(speed=0.5)
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
# 0.2秒後退(50%)
motors.backward(speed=0.5)
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
# 0.2秒左カーブ(50%)前進(100%)
motors.forward(speed=1, curve_left=0.5)
sleep(0.2)
# 0.2秒逆転
motors.reverse()
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
# 0.2秒右カーブ(50%)前進(100%)
motors.forward(speed=1, curve_right=0.5)
sleep(0.2)
# 0.2秒逆転
motors.reverse()
sleep(0.2)
# 0.2秒停止
motors.stop()
sleep(0.2)
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)
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()
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()
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
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()
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__))
ch=sys.stdin.read(1)
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)
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()
class EliConMotion(object):
def __initAttr(self):
self.__pwmLeft = OutputDevice(25)
self.__pwmRight = OutputDevice(17)
self.__pwmLeft.on()
self.__pwmRight.on()
self.__robot = Robot(left=(23, 24), right=(22, 27))
def __setSpeed(self, left_speed, right_speed, bForward=True):
mf_speed = 1
if bForward is False:
mf_speed = -1
self.__robot.value = (mf_speed * left_speed, mf_speed * right_speed)
def clear(self):
self.__pwmLeft.off()
self.__pwmRight.off()
def stop(self):
self.__initAttr()
self.__robot.stop()
def __turn(self, angle, lm, rm):
INIT_SPEED = 1.0
KP = INIT_SPEED / (2.0 * angle)
KI = KP / 100
speed = INIT_SPEED
self.__setSpeed(lm * speed, rm * speed)
g = Gyro()
totalDeviation = 0
while True:
ret = g.getAngles()
curAngle = math.fabs(ret[2])
if curAngle >= angle: # - 20: #substarct 20 to compensate for overshoot
self.__robot.stop()
break
print('curangle, speed %s %s' % (curAngle, speed))
deviation = angle - curAngle
speed = (KP * deviation) + (KI * totalDeviation)
speed = min(speed, 1.0)
self.__setSpeed(lm * speed, rm * speed)
totalDeviation += deviation
def turn_right(self, angle):
self.__initAttr()
self.__turn(angle, 1, -1)
def turn_left(self, angle):
self.__initAttr()
self.__turn(angle, -1, 1)
def move(self, bForward=True):
self.__initAttr()
KP = 0.1
SAMPLETIME = 0.000001
SPEED = 0.9
m_left_speed = SPEED
m_right_speed = SPEED
self.__setSpeed(m_left_speed, m_right_speed, bForward)
g = Gyro()
TARGET = g.getAngles()[2]
while True:
angle = g.getAngles()[2]
deviation = math.fabs(angle - TARGET)
if deviation != 0:
correction = (deviation * KP)
if bForward is False:
if angle >= 0:
m_right_speed += correction
m_left_speed -= correction
else:
m_right_speed -= correction
m_left_speed += correction
else:
if angle <= 0:
m_right_speed += correction
m_left_speed -= correction
else:
m_right_speed -= correction
m_left_speed += correction
m_right_speed = min(max(m_right_speed, 0.8), 1)
m_left_speed = min(max(m_left_speed, 0.8), 1)
#print('Correction : %s, Left: %s, Right: %s\n' % (correction, m_left_speed, m_right_speed))
self.__setSpeed(m_left_speed, m_right_speed, bForward)
time.sleep(SAMPLETIME)
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)
print(f"curr={n} head2={new_direction}")
continue
return
rotate_to_direction(180)
'''
for i in range(4):
rotate_to_direction(i * 90)
TwoBlackWinter.forward(0.6)
sleep(0.8)
n = int(sensor.get_bearing())
n = n + 90
if n > 360:
n = n - 360
print(f"At end of turn compass heading is: {n}")
'''
TwoBlackWinter.stop()
#print("setting full speed ahead straight 3 secs")
#blackWinter.value = (1.0, 1.0)
#blackWinter.forward()
#sleep(1)
#print("3 sec turning left")
#blackWinter.value = (0.1, 1.0)
## blackWinter.forward()
#sleep(11.5)
#blackWinter.stop()
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)
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 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 motor = Robot(left=(4, 14), right=(24, 25)) motor.left() sleep(5) motor.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
# 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()
#ако е равно на 2
print('I\'m moving backwards')
#извежда в конзолата, че се движи назад
zorry.backward(0.3)
#роботът се придвижва назад
elif key == '4':
#ако е въведено 4
print('I\'m moving left')
#за да завие робота наляво
zorry.left(0.4)
#намаляваме скоростта с която се движи десния мотор
zorry.right(0.3)
#
elif key == '6':
#ако е въведено 6
print('I\'m moving right')
#за да завие надясно
zorry.right(0.4)
#намаляваме скоростта с която се движи левия мотор
zorry.left(0.3)
#
else:
print('stop')
#в противен случай извежда стоп в конзолата
zorry.stop()
#и роботът спира
break
#цикълът приключва
gpio.cleanup()
#затваря стриймовете за да могат да бъдат използвани отново в друг код
def move2position(positions, s_pins, m_pins):
""" Move the robot and claw to the specified position.
Arg:
positions: array of positions [x_axis, y_axis, z_axis, chasis]
s_pins: sensor pins [us_x, us_y, us_z, us_chasis, c_switch]
m_pins: motor pins [dc_x, dc_y, dc_z, dc_wheel_l, dc_wheel_r]
Return: None
"""
# Declare flags
flags = isxnan(positions)
# movement along x-axis
while not flags[0]:
if distance(s_pins[0]) > positions[0]:
# move backwards
dc_motor(m_pins[0])
elif distance(s_pins[0]) < positions[0]:
# move forwards
dc_motor(m_pins[0], True)
else:
flags[0] = True
# movement along y-axis
while not flags[1]:
if distance(s_pins[1]) > positions[1]:
# move to the right
dc_motor(m_pins[1])
elif distance(s_pins[1]) < positions[1]:
# move to the right
dc_motor(m_pins[1], True)
else:
flags[1] = True
# movement along z-axis
while not flags[2]:
if distance(s_pins[2]) > positions[2]:
# move upwards
dc_motor(m_pins[2])
elif distance(s_pins[2]) < positions[2]:
# move upwards
dc_motor(m_pins[2], True)
else:
flags[2] = True
# chasis movement
while not flags[3]:
if distance(s_pins[3]) > positions[3]:
# !!! move backwards
chasis = Robot(left=(m_pins[3][0],m_pins[3][1]), right=(m_pins[3][2],m_pins[3][3]))
chasis.backward()
sleep(0.5)
chasis.stop()
elif distance(s_pins[3]) > positions[3]:
# !!! move backwards
chasis = Robot(left=(m_pins[3][0],pins[3][1]), right=(pins[3][2],pins[3][3]))
chasis.forward()
sleep(0.5)
chasis.stop()
else:
flags[3] = True
# flap movement
while not flags[4]:
# open flap
if positions[4] == 1:
while(GPIO.input(s_pins[4][0]) == GPIO.LOW)
dc_motors((m_pins[4],m_pins[5]))
flags[4] = True
else
while(GPIO.input(s_pins[4][1]) == GPIO.LOW)
dc_motors((m_pins[4],m_pins[5]), True)
flags[4] = True
class CDbot:
#contructor of class
def __init__(self):
window = np.zeros((100,100)) #initialising a black 100x100 window for using escape to quit
self.colour = {'red':0,'green':0,'blue':0,-1:0} #control dictionary for logic
left = (4,14) #left wheel gpio pins
right = (17,18) #right wheel gpio pins
self.robot = Robot(left,right) #robot initialisation class call
self.cap = cv2.VideoCapture(0) #recording video from camera 0 (i.e. the camera on rpi)
while(True):
ret, image = self.cap.read() #read from camera
if cv2.waitKey(1) != 27 :
cv2.imshow('window',window) #show the window previously made
self.botmove(image) #main bot moving method call
else:
break #exit if escape key is pressed
#bot movement mechanics
def botmove(self,image):
self.colour = {'red':0, 'blue':0, 'green':0,-1:0}
self.colour[self.colour_detect(image)]=1
if self.colour['red'] == 1:
#print('stop')
self.robot.stop() #stops robot if red is verified
elif self.colour['green'] == 1:
#print('forward')
self.robot.forward() #moves robot forward if green is verified
elif self.colour['blue'] == 1:
#print('reverse')
self.robot.reverse() #moves robot reverse if blue is verified
else:
pass #don't do anything
#colour detection code
def colour_detect(image):
hsv=cv2.cvtColor(image,cv2.COLOR_BGR2HSV)
lower_blue = np.array([100,100,100])
upper_blue = np.array([140,255,255])
lower_red = np.array([140,100,100])
upper_red = np.array([180,255,255])
lower_green = np.array([60,100,100])
upper_green = np.array([100,255,255])
blue_mask = cv2.inRange(hsv, lower_blue, upper_blue)
red_mask = cv2.inRange(hsv, lower_red, upper_red)
green_mask = cv2.inRange(hsv, lower_green, upper_green)
(w,h,c)=hsv.shape
image_area = w*h
blue_area = 0
red_area = 0
green_area = 0
res = cv2.bitwise_and(image,image, mask = green_mask)
cv2.imshow('res',res)
_,contours_blue,_ = cv2.findContours(blue_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
_,contours_red,_ = cv2.findContours(red_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
_,contours_green,_ = cv2.findContours(green_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
for cnt_b in contours_blue:
blue_area = blue_area + cv2.contourArea(cnt_b)
for cnt_r in contours_red:
red_area = red_area + cv2.contourArea(cnt_r)
for cnt_g in contours_green:
green_area = green_area + cv2.contourArea(cnt_g)
blue_ratio = blue_area/image_area
red_ratio = red_area/image_area
green_ratio = green_area/image_area
colour_ratios = [blue_ratio, red_ratio, green_ratio]
if(max(colour_ratios)>0.2):
if(max(colour_ratios) == colour_ratios[0]):
return 'blue'
elif(max(colour_ratios) == colour_ratios[1]):
return 'red'
else:
return 'green'
else:
return -1
#destructor of class
def __del__(self):
print('Program terminated')
self.cap.release()
cv2.destroyAllWindows()
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)
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()
elif left_distance > threshold and centre_distance < threshold and right_distance > threshold:
if left_distance < right_distance:
go_right(fast)
sleep(0.3)
else:
go_left(fast)
sleep(0.3)
elif left_distance > threshold2 and centre_distance > threshold2 and right_distance > threshold2:
go_forwards(fast)
dead_end = 0
elif left_distance < threshold2 or centre_distance < threshold2 or right_distance < threshold2:
go_forwards(slow)
if dead_end == 5:
print("Can't get through - turning around!")
if left_distance < right_distance :
go_right(fast)
else:
go_left(fast)
sleep(1)
dead_end = 0
except Exception as excep:
print(excep)
burt_the_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()
# This uses a hbridge module
# guide found @ https://projects.raspberrypi.org/en/projects/build-a-buggy/2
# and merged with another guide found @ https://gpiozero.readthedocs.io/en/v1.1.0/recipes.html#keyboard-controlled-robot
from evdev import InputDevice, list_devices, ecodes
from gpiozero import Robot
johnny = Robot(left=(7,8), right=(9,10))
devices = [InputDevice(device) for device in list_devices()]
keyboard = devices[0] # this may vary
keypress_actions = {
ecodes.KEY_UP: johnny.forward,
ecodes.KEY_DOWN: johnny.backward,
ecodes.KEY_LEFT: johnny.left,
ecodes.KEY_RIGHT: johnny.right,
}
for event in keyboard.read_loop():
if event.type == ecodes.EV_KEY:
if event.value == 1: # key down
keypress_actions[event.code]()
if event.value == 0: # key up
johnny.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
