在前面的教学中，我们进行了控制，ADC，IO的相关教学，接下来我们将结合这些知识，实现一个避障控制的功能。
我们在四轮机器人前方部署两个红外接近传感器，实现机器人不断前进，当任意一个红外接近检测到障碍物时，则右转，两个同时检测到障碍物时，则后退，代码如下：
from uptech import UpTech
from up_camera import Camera
import cv2
import time
class FourWhellMove:
def __init__(self):
self.up = UpTech()
time.sleep(0.01)
self.up.CDS_Open()
time.sleep(0.01)
self.up.ADC_IO_Open()
time.sleep(0.01)
self.cam = Camera()
self.up.CDS_SetMode(1,1) #0舵机，1电机
self.up.CDS_SetMode(2,1)
self.up.CDS_SetMode(3,1)
self.up.CDS_SetMode(4,1)
time.sleep(0.05)
if(self.cam.open()):
while(True):
adc_0 = self.up.ADC_Get_Channel(0)
adc_1 = self.up.ADC_Get_Channel(1)
print("adc_0: " + str(adc_0) + " adc_1:" + str(adc_1))
frame = self.cam.snapshot()
if adc_0 < 500 and adc_1 < 500:
self.move_backward(512)
elif adc_0 < 500 or adc_1 < 500:
self.turn_right(512)
else:
self.move_forward(512)
self.cam.show(frame, "CYZX")
key = cv2.waitKey(100) & 0xFF # 等待100ms，并获取按键信息
if key == ord('q'): # 如果按下'q'键，则退出循环
self.stop()
break
def move_forward(self, speed):
self.up.CDS_SetSpeed(1,speed)
self.up.CDS_SetSpeed(3,speed)
self.up.CDS_SetSpeed(2,-speed)
self.up.CDS_SetSpeed(4,-speed)
def move_backward(self, speed):
self.up.CDS_SetSpeed(1,-speed)
self.up.CDS_SetSpeed(3,-speed)
self.up.CDS_SetSpeed(2,speed)
self.up.CDS_SetSpeed(4,speed)
def move_left(self, speed):
self.up.CDS_SetSpeed(1,-speed)
self.up.CDS_SetSpeed(3,speed)
self.up.CDS_SetSpeed(2,-speed)
self.up.CDS_SetSpeed(4,speed)
def move_right(self, speed):
self.up.CDS_SetSpeed(1,speed)
self.up.CDS_SetSpeed(3,-speed)
self.up.CDS_SetSpeed(2,speed)
self.up.CDS_SetSpeed(4,-speed)
def turn_left(self, speed):
self.up.CDS_SetSpeed(1,-speed)
self.up.CDS_SetSpeed(3,-speed)
self.up.CDS_SetSpeed(2,-speed)
self.up.CDS_SetSpeed(4,-speed)
def turn_right(self, speed):
self.up.CDS_SetSpeed(1,speed)
self.up.CDS_SetSpeed(3,speed)
self.up.CDS_SetSpeed(2,speed)
self.up.CDS_SetSpeed(4,speed)
def stop(self):
self.up.CDS_SetSpeed(1,0)
self.up.CDS_SetSpeed(2,0)
self.up.CDS_SetSpeed(3,0)
self.up.CDS_SetSpeed(4,0)
def close(self):
self.up.CDS_Close()
if __name__ == "__main__":
move = FourWhellMove()