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 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
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)
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)
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)
break
try:
cmd, duration = text.split()
except ValueError:
duration = '0'
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
elif direction == "forward":
burt_the_robot.forward(speed_val)
else:
burt_the_robot.backward(speed_val)
sleep(sleep_val)
image_tg= "/home/pi/Pictures/" + str(myColour) + "-" + str(filecnt) + "-" + str(pix_cnt) + "-" + str(l) + "-" + str(m) + "-" + str(r) + "-" + ".png"
filecnt = filecnt + 1
call(["cp", img_dest, image_tg])
# Remove pictures afterwards.
# show the imgs
#cv2.imshow("img", np.hstack([img, output]))
#cv2.waitKey(0)
##
burt_the_robot.backward(speed_val)
sleep(sl)
sleep(sl)
burt_the_robot.stop()
def dpad(pos):
global counter
# pressed any button yet to change it by 1
if pos.top:
print("up") # If the position of the press is up, then print 'up'
set_colour() # and call the set_colour() function
if pos.bottom:
print("down") # If the position of the press is down, print 'down'
counter += 1 # and change 'counter' by 1 because we have now pressed
# the bottom part of the button
if counter <= 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__))
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()
#dir = sys.stdin.read (1)
if inp == "w":
take_noods('w')
robot.forward(maxSpeed)
time.sleep(4)
robot.stop()
elif inp == "a":
take_noods('a')
#robot.right(.2)
#robot.forward (maxSpeed, curve_left = turnSpeed + .2)
robot.forward(maxSpeed, curve_right=turnSpeed + .4)
time.sleep(sleeptimer)
robot.stop()
elif inp == "s":
take_noods('s')
robot.backward(maxSpeed)
time.sleep(sleeptimer)
robot.stop()
elif inp == "d":
take_noods('d')
#robot.left(.2)
robot.forward(maxSpeed, curve_left=turnSpeed + .4)
time.sleep(sleeptimer)
robot.stop()
#robot.forward (maxSpeed, curve_right = turnSpeed + .2)
elif inp == "o":
break
elif inp == "x":
take_noods('x')
while True:
frame = camera.take_frame()
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:
zorry = Robot(left=(4, 17), right=(27, 22))
#избираме пиновете, които ще използваме, за да свържем мотора с raspberry-то
while True:
key = input()
#тук ще съхраняваме входа от клавиатурата
if key == '8':
#ако е 8
print('I\'m moving forward')
#извежда в конзолата, че се движи напред
zorry.forward(0.5)
#роботът се придвижва напред
elif key == '2':
#ако е равно на 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)
#намаляваме скоростта с която се движи левия мотор
machine = Robot(right=(4, 27), left=(18, 24))
while True:
if ((GPIO.input(left_sensor) == True) &
(GPIO.input(right_sensor) == True)):
print('nothing is in front')
sleep(1) # change sleep time according to the reaction of the robot
machine.forward()
print('forward')
sleep(1)
elif ((GPIO.input(left_sensor) == False) &
(GPIO.input(right_sensor) == True)):
print('left switch pressed')
sleep(1)
machine.backward()
print('reverse')
sleep(1)
machine.stop()
print('hold')
sleep(1)
machine.right()
print('right')
sleep(1)
machine.stop
print('hold')
sleep(1)
elif ((GPIO.input(left_sensor) == True) &
(GPIO.input(right_sensor) == False)):
print('right switch pressed')
wii.rumble = 1
time.sleep(0.5)
wii.rumble = 0
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"
bocht = 0.16
while True:
d = afstand()
print('Afstand is {:f}'.format(d))
if d > 60:
robby.forward(1, curve_right=bocht)
time.sleep(0.2)
elif (d > 40) & (d < 60):
robby.forward((d - 40) / 20, curve_right=bocht)
time.sleep(0.2)
elif d < 10:
robby.backward(0.5, curve_right=bocht)
time.sleep(1)
robby.left(1)
time.sleep(3)
else:
robby.stop()
time.sleep(2)
kwispel = 0
while kwispel < 20:
robby.left()
time.sleep(0.1)
robby.right()
time.sleep(0.1)
kwispel = kwispel + 1
if j.get_axis(0) < 0:
robby.forward()
robby.left()
if j.get_button(0):
robby.forward()
elif j.get_axis(0) > 0:
robby.forward()
robby.right()
if j.get_button(0):
robby.forward()
elif j.get_axis(0) == 0:
if not j.get_button(0):
robby.stop()
if event.type == pygame.JOYBUTTONDOWN:
print("Button Pressed")
if j.get_button(0):
robby.forward()
if j.get_button(4):
robby.backward()
elif event.type == pygame.JOYBUTTONUP:
print("Button Released")
robby.stop()
except KeyboardInterrupt:
print("EXITING NOW")
j.quit()
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
#used when preforming specific actions for fleeing Darth Vader
self.turning_around = False
self.time_turning_around = 0
self.running_away = False
self.time_running_away = 0
self.fleeing = False
#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
#used to make random movements
self.time_stopped = 0
self.time_moving = 0
self.moving = False
#used for reaction to characters
self.seeing_Leia = False
self.seen_Leia = False
self.time_seeing_Leia = 0
self.seeing_Obiwan = False
self.time_seeing_Obiwan = 0
self.seeing_Vader = False
self.time_seeing_Vader = 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
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)
#update character sightings
self.update_character_sighting(delta_time)
#check if now completing special fleeing movements
if (self.fleeing):
self.update_fleeing(delta_time)
else:
#we are not fleeing, continue making random movements
self.update_random_movement(delta_time)
#updates the times of seeing different characters (so R2 doesn't react more than once when seeing someone)
#expected delta_time is in seconds
def update_character_sighting(self, delta_time):
if (self.seeing_Leia):
self.seen_Leia = True
#if this is the first time we're seeing her (for now), say hello!
if (self.time_seeing_Leia == 0):
self.play_sound('cute')
self.time_moving = 0
self.moving = False
self.stop_movement()
self.time_seeing_Leia = self.time_seeing_Leia + delta_time
else:
self.time_seeing_Leia = 0
if (self.seeing_Obiwan):
#if this is the first time we're seeing him (for now), say hello or deliver Leia's message
if (self.time_seeing_Obiwan == 0):
self.time_moving = 0
self.moving = False
self.stop_movement()
if (self.seen_Leia):
self.play_sound('helpme_short')
else:
self.play_sound('excited')
self.time_seeing_Obiwan = self.time_seeing_Obiwan + delta_time
else:
self.time_seeing_Obiwan = 0
if (self.seeing_Vader):
#if this is the first time we're seeing him (for now), run away!
if (self.time_seeing_Vader == 0):
self.fleeing = True
self.turning_around = True
self.time_seeing_Vader = self.time_seeing_Vader + delta_time
else:
self.time_seeing_Vader = 0
#decides whether to stop or choose a new random movement
#stops last for 3 seconds, movements last for 3 seconds
#expected delta_time in seconds
def update_random_movement(self, delta_time):
if (self.moving):
#update time
self.time_moving = self.time_moving + delta_time
#check to see if we should stop moving
if (self.time_moving >= 3.0):
#reset everything and stop moving
self.time_moving = 0
self.moving = False
self.stop_movement()
else:
self.time_stopped = self.time_stopped + delta_time
#check to see if it's time for a new random movement
if (self.time_stopped >= 3.0):
#reset stopping variables
self.time_stopped = 0
self.moving = True
#choose a new random movement
random_movement = random.randint(1, 5)
if (random_movement == 1):
self.move_backward()
elif (random_movement == 2):
self.turn_right()
elif (random_movement == 3):
self.turn_left()
else:
self.move_forward()
#updates R2's fleeing movements
#expects delta_time in seconds
def update_fleeing(self, delta_time):
if (self.turning_around):
if (self.time_turning_around == 0):
#we just saw Darth Vader!
self.stop_movement()
self.play_sound('scream')
self.turn_right()
else:
#check if we should stop turning
if (self.time_turning_around >= 2.0):
#stop turning, start running
self.turning_around = False
self.time_turning_around = 0
self.running_away = True
#update time if we are still turning around
if (self.turning_around):
self.time_turning_around = self.time_turning_around + delta_time
if (self.running_away):
if (self.time_running_away == 0):
#we just started running away
self.move_forward()
else:
#check if we should stop running
if (self.time_running_away >= 3.0):
#stop running
self.running_away = False
self.fleeing = False
self.time_running_away = 0
#update time if we are still running away
if (self.running_away):
self.time_running_away = self.time_running_away + 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)
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()
wii.rumble = 1
time.sleep(0.5)
wii.rumble = 0
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"
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()
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()
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)
elif key == "a":
robot.stop()
time.sleep(0.01)
robot.left(speed)
elif key == "d":
robot.stop()
time.sleep(0.01)
robot.right(speed)
elif key == " ": # space bar
robot.stop()
elif key == "q": # q key for quit
break
else:
print(key)
from gpiozero import Robot from gpiozero import Motor from time import sleep print 'start' robot = Robot(left=(22, 27), right=(4, 17)) sleep(.5) robot.backward(.5) sleep(.5) #for i in range(4): # robot.forward(.5) # sleep(1) # robot.right(.5) # sleep(.3) #motor = Motor(forward=4, backward=17) #while True: # motor.forward() # sleep(5) # motor.backward() # sleep(5) print 'end'
screen = curses.initscr()
curses.noecho()
curses.cbreak()
screen.keypad(True)
try:
while True:
print 'loop'
char = screen.getch()
if char == ord('q'):
break
elif char == curses.KEY_UP:
robot.forward(.6)
elif char == curses.KEY_DOWN:
robot.backward(.6)
elif char == curses.KEY_RIGHT:
robot.right(.5)
#time.sleep(.5)
elif char == curses.KEY_LEFT:
robot.left(.5)
#time.sleep(.5)
elif char == ord('p'):
robot.stop()
finally:
#Close down curses properly, inc turn echo back on!
curses.nocbreak()
screen.keypad(0)
curses.echo()
curses.endwin()
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 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 robot = Robot(left=(5, 6), right=(22, 27)) robot.forward(0.2) sleep(0.5) robot.backward(0.2) sleep(0.5) robot.stop()
myled.off()
time.sleep(0.25)
while True:
char = getch()
if (char == "q"):
bot.stop()
exit(0)
if (char == "a"):
print('Left pressed')
bot.left()
time.sleep(button_delay)
if (char == "d"):
print('Right pressed')
bot.right()
time.sleep(button_delay)
elif (char == "w"):
print('Up pressed')
bot.forward()
time.sleep(button_delay)
elif (char == "s"):
print('Down pressed')
bot.backward()
time.sleep(button_delay)
bot.stop()
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
#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) #stop, Burt! burt_the_robot.stop()
