l_sensor = LineSensor(LS)
r_sensor = LineSensor(RS)
speed = 0.68
def motor_speed():
while True:
l_detect = int(l_sensor.value)
r_detect = int(r_sensor.value)
## Stage 1
if l_detect == 0 and r_detect == 0:
l_motor = 1
r_motor = 1
## Stage 2
if l_detect == 0 and r_detect == 1:
l_motor = -1
if l_detect == 1 and r_detect == 0:
r_motor = -1
#print(r, l)
yield (r_motor * speed, l_motor * speed)
robot.source = motor_speed()
sleep(60)
robot.stop()
robot.source = None
robot.close()
l_sensor.close()
r_sensor.close()
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 ((buttons & cwiid.BTN_PLUS) and (buttons & cwiid.BTN_MINUS)) :
break
if (buttons & cwiid.BTN_PLUS):
speed += 10
if speed > 100 :
speed = 100
print ("Speed : "+str(speed))
if (buttons & cwiid.BTN_MINUS):
speed -= 10
if speed < 0 :
speed = 0
print ("Speed : "+str(speed))
if (buttons & cwiid.BTN_UP):
current_direction = "forward"
if (buttons & cwiid.BTN_DOWN):
current_direction = "backward"
if (buttons & cwiid.BTN_LEFT):
current_direction = "left"
if (buttons & cwiid.BTN_RIGHT):
current_direction = "right"
if (buttons & cwiid.BTN_A and camera_enable == True):
timestring = time.strftime("%Y-%m-%dT%H.%M.%S", time.gmtime())
print ("Taking photo " +timestring)
camera.capture(photo_dir+'/photo_'+timestring+'.jpg')
# Only print direction if it's changed from previous
if (current_direction != last_direction) :
print ("Direction "+current_direction)
robot.close()
end_time = time() + duration
running = True
while running:
left_detect = left_sensor.value
right_detect = right_sensor.value
# Rule 1
if left_detect == 0 and right_detect == 0:
left_mot = 1
right_mot = 1
# Rule 2
elif left_detect == 1:
left_mot = -1
right_mot = 1
# Rule 3
elif right_detect == 1:
left_mot = 1
right_mot = -1
robin.left_motor.value = left_mot * speed
robin.right_motor.value = right_mot * speed
if time() >= end_time:
running = False
robin.stop()
robin.close()
left_sensor.close()
right_sensor.close()
from gpiozero import Robot
import time
print('Testing Sandy Boy!')
robo = Robot(right=(7,8), left=(20,21))
print('Forward ^')
robo.forward()
time.sleep(3)
print('backward V')
robo.backward()
time.sleep(3)
print('Right ->')
robo.right()
time.sleep(3)
print('Left <-')
robo.left()
time.sleep(3)
robo.stop()
robo.close()
print('Success !!')
