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)
return ch
try:
us = UltraSound(6,12)
robot = Robot(left=(23,22),right=(17,18))
speed=1.0
while True:
obstdistance = us.measure()
print(obstdistance)
if obstdistance < 15:
speed=0.5
robot.stop()
robot.right(speed)
time.sleep(0.01)
robot.reverse(speed)
time.sleep(2)
speed =1.0
robot.forward(speed)
key = getchar()
if key == "w":
robot.stop()
time.sleep(0.01)
robot.forward(speed)
elif key == "z":
robot.stop()
time.sleep(0.01)
robot.backward(speed)
elif key == "a":
robot.stop()
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 < 1.5:
robot.stop()
elif obstdistance < 15:
speed=0.5
robot.stop()
robot.right(speed)
time.sleep(0.01)
robot.reverse()
time.sleep(0.95)
speed =1.0
robot.forward(speed)
#key = getchar()
key="/"
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":
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()
print(" Stopped")
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()
from gpiozero import Robot from time import sleep robot = Robot(left=(4, 14), right=(17, 18)) robot.forward() # full speed forwards robot.forward(0.5) # half speed forwards robot.backward() # full speed backwards robot.backward(0.5) # half speed backwards robot.stop() # stop the motor robot.value = 0.5 # half speed forwards robot.value = -0.5 # half speed backwards robot.value = 0 # stop robot.reverse() # reverse direction at same speed, e.g... robot.forward(0.5) # going forward at half speed robot.reverse() # now going backwards at half speed robot.right() robot.left()
