触觉和听觉导航视觉综合案例

在这个任务中，可以使用触觉传感器（超声波）、听觉传感器（语音识别模块）、无线键盘（蓝牙键盘）和视觉设备（摄像头）来编写一个程序。

程序的要求是，如果超声波传感器检测到障碍物或者不是由超声波检测到障碍物而是机器人听到了相关的命令，就开启摄像头来进行人脸识别，然后通过蓝牙键盘控制机器人动作，使其监控家居环境的状况。

语音模块关键字的配置文件如下：

0:11 22 33 44:xiao mi za::

1:A5 05 05 5B:qian jin:p13=0:ff

2:A5 06 06 5B:hou tui:p13=1:01

3:A5 07 07 5B:zuo zhuan:p14=1:01

4:A5 08 08 5B:you zhuan:p14=0:01

5:A5 09 09 5B:qiu ming:p15=0:ff

底层控制板的程序如下：

#include 〈Wire.h〉

#include 〈Software Serial.h〉

Software Serial my Serial(2, 3); // RX, TX

const int ping Tig = 15;//PC1

cons tint ping Eco = 14;//PC0

cons int flag = 0;

#define SLAVE_ADDRESS 0x04

int number = 0;

int state = 0;

Char addr[4];

int INA = 4; //电机 A 正反转控制端

int PWMA = 5; //电机 A 调速端

int INB = 7; //电机 B 正反转控制端

int PWMB = 6; //电机 B 调速端

void motospd(int sp1,int sp2) //电机速度控制函数，括号内分别为左右电机速度值，{ //范围-255～+255，正值为正转，负值为反转。

if(sp1〉0)

digital Write(INA, HIGH);

else

digital Write(INA, LOW);

if(sp2〉0)

digital Write(INB, HIGH);

else

digital Write(INB, LOW);

analog Write(PWMA,abs (sp1));

analog Write(PWMB,abs (sp2));

}

}

void setup() {

pin Mode(INA,OUTPUT);

pin Mode(INB,OUTPUT);//配置电机驱动 IO 口为输出

pin Mode(ping Tig, OUTPUT);

pin Mode(ping PEco, INPUT);

Serial.begin(9600); // start serial for output

my Serial.begin(9600);

// initialize i2c as slave

Wire.begin(SLAVE_ADDRESS);

// define callbacks for i2c communication

Wire.on Receive(receive Data);

Wire.on Request(send Data);

Serial.println(＂Ready!＂);

}

void loop(){

long duration, inches, cm;

digital Write(ping Tig, LOW);

delay Microseconds(2);

digital Write(ping Tig, HIGH);

delay Microseconds(5);

digital Write(ping Tig, LOW);

duration = pulse In(ping Eco, HIGH);

inches = microseconds To Inches(duration);

cm = microseconds To Centimeters(duration);

if(cm 〈 20)//如果超声波探测在20cm处有障碍物

{

flag = 1;

}

if(my Serial.available()==4)//接收到4个字节

{

Addr[0] = my Serial.read();

Addr[1] = my Serial.read();

Addr[2] = my Serial.read();

Addr[3] = my Serial.read();

}

if(sensor Pin == 0)//感应到人时，才能语音控制程序

{

if(addr[1]== 0x05 )

{

motospd(100,100); //直行

}

Else if(addr[1]==0x06)

{

motospd(-100,-100); //后退

}

Else if(addr[1]==0x07)

{

motospd(50,100); //左转

}

Else if(addr[1]==0x08)

{

motospd(100,50); //右转

}

}

}

long microseconds To Inches(long microseconds)

{

return microseconds / 74 / 2;

}

long microseconds To Centimeters(long microseconds)

{

return microseconds / 29 / 2;

}

// callback for received data

void receive Data(int byte Count){

while(Wire.available())

number = Wire.read();

}

// callback for sending data

void send Data(){

Wire.write(flag);//为零时，上位机不处理；为1时，上位机处理

}

下面介绍嵌入式控制器程序设计。

无线蓝牙键盘功能设置如图7.5.1所示。

图7.5.1 无线蓝牙键盘功能设置

无线蓝牙键盘功能设置程序如下：

import smbus

import time

# for RPI version 1, use ＂bus = smbus.SMBus(0)＂

bus = smbus.SMBus(1)

# This is the address we setup in the Arduino Program

address = 0x04

from Simple CV import Camera, Display

from time import sleep

my Camera=Camera (prop set={ width＂: 320, ＂height＂:240})

my Display=Display(resolution=(320, 240))

while not my Display.is Done():

frame = my Camera.get Image()

faces = frame.find Haar Features(‘face’)

if faces:

for face in faces:

print “Face at:”+str(face.coordinaes())

else:

print “No faces detected.”

frame.save(my Display)

Sleep(.1)

try:

import RPi.GPIO as GPIO

except Runtime Error:

print(＂Error importing RPi.GPIO! This must be run as root using sudo＂)

import sys, tty, termios

# get a character from the command line

# Linux/Unix only - but that's OK as RPi.GPIO is Raspberry Pi Linux only

def getch() :

fd = sys.stdin.fileno()

old_settings = termios.tcgetattr(fd)

try:

tty.setraw(sys.stdin.fileno())

ch = sys.stdin.read(1)

finally:

termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)

return ch

# Motor PINs

MOTOR1A = 17 #left fwd

MOTOR1B = 18 #left rev

MOTOR2A = 23 #right fwd

MOTOR2B = 22 #right rev

# freq of pwm outputs

PWM_FREQ = 50 #50hz

# uses processor pin numbering

GPIO.setmode(GPIO.BCM)

# speed = pwm duty cycle, 0 = off, 100 = max

speed = 50

# list to convert key presses into motor on/off values to correspond with the

direction

# direction based on number keypad or letters eg.

# 8 = fwd, 2 = rev, 4 = left, 5 = right, 7 = fwd left, 9 = fwd right, 1 = rev

left, 3 = rev right

# the key for the list is the character

# values are all tuples which hold (motor1, motor2)

# values are 0 = motor stop, 1 = motor fwd (A), 2 = motor rev (B)

direction = {

# number keys

'1' : (2, 0),

'2' : (2, 2),

'3' : (0, 2),

'4' : (1, 2),

'5' : (0, 0),

'6' : (2, 1),

'7' : (1, 0),

'8' : (1, 1),

'9' : (0, 1),

# keyboard keys

'r' : (1, 0),

't' : (1, 1),

'y' : (0, 1),

'f' : (1, 2),

'g' : (0, 0),

'h' : (2, 1),

'c' : (2, 0),

'v' : (2, 2),

'b' : (0, 2)

}

current_direction = (0, 0)

# setup pins

GPIO.setup(MOTOR1A, GPIO.OUT)

GPIO.setup(MOTOR1B, GPIO.OUT)

GPIO.setup(MOTOR2A, GPIO.OUT)

GPIO.setup(MOTOR2B, GPIO.OUT)

# set pins as PWM

pin1A = GPIO.PWM(MOTOR1A, PWM_FREQ)

pin1B = GPIO.PWM(MOTOR1B, PWM_FREQ)

pin2A = GPIO.PWM(MOTOR2A, PWM_FREQ)

pin2B = GPIO.PWM(MOTOR2B, PWM_FREQ)

# start PWM

pin1A.start (0)

pin1B.start (0)

pin2A.start (0)

pin2B.start (0)

while True:

## Motor 1

# fwd

if current_direction[0] == 1 :

pin1B.Change Duty Cycle(0)

pin1A.Change Duty Cycle(speed)

# rev

elif current_direction[0] == 2 :

pin1A.Change Duty Cycle(0)

pin1B.Change Duty Cycle(speed)

# stop

else :

pin1A.Change Duty Cycle(0)

pin1B.Change Duty Cycle(0)

## Motor 2

# fwd

if current_direction[1] == 1 :

pin2B.Change Duty Cycle(0)

pin2A.Change Duty Cycle(speed)

# rev

elif current_direction[1] == 2 :

pin2A.Change Duty Cycle(0)

pin2B.Change Duty Cycle(speed)

# stop

else :

pin2A.Change Duty Cycle(0)

pin2B.Change Duty Cycle(0)

# Get next key pressed

ch = getch()

# q = quit

if (ch == 'q') :

break

elif (ch == '+' or ch == 'p') :

speed += 10

if speed 〉 100 :

speed = 100

print ＂Speed : ＂+str(speed)+＂\n＂

elif (ch == '-' or ch == 'l') :

speed -= 10

if speed 〈 0 :

speed = 0

print ＂Speed : ＂+str(speed)+＂\n＂

elif (ch in direction.keys()) :

current_direction = direction[ch]

print ＂Direction ＂+str(current_direction[0])

+str(current_direction[1])+＂\n＂

# Stop and cleanup the PWM

pin1A.stop()

pin1B.stop()

pin2A.stop()

pin1B.stop()

GPIO.cleanup()

def write Number(value):

bus.write_byte(address, value)

# bus.write_byte_data(address, 0, value)

return -1

def read Number():

number = bus.read_byte(address)

# number = bus.read_byte_data(address, 1)

return number