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 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 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
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 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)
cl = 0.1
def inrange():
global cl
robot.stop()
robot.backward(curve_right=0.5)
sleep(2)
#robot.right()
#robot.reverse()
sleep(1)
robot.forward(curve_right=cl)
def outrange():
global cl
robot.stop()
robot.forward(curve_right=cl)
sensor = DistanceSensor(echo=20,
trigger=21,
max_distance=1,
threshold_distance=0.15)
sensor.when_in_range = inrange
sensor.when_out_of_range = outrange
#robot = Robot(left=(26, 19), right=(6, 13))
robot = Robot(right=(19, 26), left=(6, 13))
robot.forward(curve_right=cl)
pause()
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)
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()
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()
count = 0
sleeptimer = 0.3
camera.start_preview()
def take_noods(s):
camera.capture('train/0.jpg')
while True:
inp = input('enter:')
#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":
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()
from gpiozero import Robot
from time import sleep
# left/right=(전진,후진,PWM)
robot = Robot(left=(17, 27, 22), right=(15, 18, 14), pwm=True)
for i in range(4):
robot.forward(0.5) # 0 ~ 1 사이
sleep(1)
robot.stop()
sleep(1)
robot.right(0.5)
sleep(1)
robot.stop()
sleep(1)
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)
print ("Robot control - use arrow buttons to control direction")
print ("Speed " + str(speed) +"% - use +/- to change speed")
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"]
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()
# r <n> Right for n steps
while True:
text = input("Next: ").strip().lower()
if text[0] =='q':
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()
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()
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)
from gpiozero import Robot
from time import sleep
#left/right(전진,후진,PWM)
robot = Robot(left=(17, 27, 22), right=(15, 18, 14), pwm=True)
for i in range(4):
robot.forward(0.3) # 0 ~ 1사이
sleep(1)
robot.stop()
sleep(1)
robot.right(0.5)
sleep(1)
robot.stop()
sleep(1)
from gpiozero import Robot import time robot = Robot(left=(22, 23), right=(17, 21)) #time.sleep(30) robot.forward(0.5) time.sleep(1) robot.right(0.5) time.sleep(0.5) robot.left(0.5) time.sleep(0.5)
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()
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()
from gpiozero import Robot from time import sleep robot = Robot(left=(4, 14), right=(17, 18)) i = 1 robot.forward(speed=1.0) print(i, " Going Forward") sleep(5) robot.right() print(i, " Turning Right") sleep(2) robot.forward(speed=1.0) print(i, " Going Forward") sleep(5) robot.left(speed=0.75) robot.stop()
robot.forward(0.5)
sleep(n_seconds)
robot.stop()
def go_left(n_seconds):
robot.left(0.5)
sleep(n_seconds)
robot.stop()
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)
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
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 robot = Robot(left=(5, 6), right=(22, 27)) robot.forward(0.2) sleep(0.5) robot.backward(0.2) sleep(0.5) robot.stop()
print("Robot control - use arrow buttons to control direction")
print("Speed " + str(speed) + "% - use +/- to change speed")
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"]
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)
elif key == "a":
robot.stop()
time.sleep(0.01)
robot.left(speed)
elif key == "d":
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()
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()
