#include < Voice.h>
#include < Servo.h>
#include < SPI.h>
#include < Pixy.h>
const byte Rst = A2; //6;
const byte Data = A1; //7; 片选
const byte Busy = A0; //8; 语音
const int trigPin = 8; //超声波trigle引脚
const int echoPin = 9; //超声波echo引脚
const int horPin = 6; //水平转动
const int verPin = 7; //上下转动
const int distMin = 10; //定义响应的范围下限
const int distMax = 40; //定义响应的范围上限
byte readBuffer[4];
unsigned int Command; //接受按键：红外遥控器和手机app
Pixy pixy;
Voice voice(Rst, Data, Busy);
Servo horSer;
Servo verSer;
const int RANGE = 6;
const int maxVerAngle = 80;
const int minVerAngle = 10;
const int verAngleDefault = 40;
int dist = 0;
int angleHor = 90;
int angleVer = verAngleDefault;
int preDist = 0; //用于保存先前的距离状态，用于判断是靠近还是远离
int nearOrLeft = 0; //状态变量：0-靠近 1-远离
int StopMove = false; // 小车运动功能 ：false 关闭 true 开启
byte followFlag = 0; //自动跟随 0关闭 1 打开超声波 2 视觉跟随
byte broadCastFlag = 0; // 距离播报 0 关闭 1 距离播报 2 迎宾小人
byte ballScanFlag = 0; // 静止找球 0 关闭 1 打开
byte followDist = 40;
byte followBalance = 15;
byte FunctionFlag = 0; // A B C D 按键对应的功能打开或关闭
void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
horSer.attach(horPin);
verSer.attach(verPin);
horSer.write(angleHor);
delay(300);
verSer.write(angleVer);
delay(300);
}
void loop() {
Command = 0;
getCommand(); //获取按键
Serial.println(Command, HEX);
switch (Command) {
case 0x4c53 : //左转
TurnLeft();
break;
case 0x5253 : // 右转
TurnRight();
break;
case 0x5553: //前进
Forward();
break;
case 0x4453: //后退
Backward();
break;
case 0x4452: //停止
break;
case 0x444c: //减速
break;
case 0x554c : //加速
break;
case 0x5552 : //全速
// speedPWM = maxSpeedPwm;
break;
case 0x4141 : //A 自动跟随
if (followFlag == 0) { //打开超声波跟随
voice.VoiceWord(21);
voice.VoiceWord(66);
voice.VoiceWord(36);
followFlag = 1;
FunctionFlag = 1; // Function 功能打开
} else if (followFlag == 1) { //打开视觉跟随
voice.VoiceWord(21);
voice.VoiceWord(66);
voice.VoiceWord(37);
followFlag = 2;
FunctionFlag = 1; // Function 功能打开
} else if (followFlag == 2) { //打开视觉跟随
voice.VoiceWord(22);
followFlag = 0;
FunctionFlag = 0; // Function 功能打开
}
break;
case 0x4242 : //B
if (broadCastFlag == 0) { //打开距离播报
voice.VoiceWord(21);
voice.VoiceWord(66);
voice.VoiceWord(36);
broadCastFlag = 1;
FunctionFlag = 1; // Function 功能打开
} else if (broadCastFlag == 1) { //打开迎宾小人
voice.VoiceWord(21);
voice.VoiceWord(66);
voice.VoiceWord(37);
broadCastFlag = 2;
FunctionFlag = 1; // Function 功能打开
} else if (broadCastFlag == 2) { //打开视觉跟随
voice.VoiceWord(22);
broadCastFlag = 0;
FunctionFlag = 0; // Function 功能打开
}
break;
case 0x4343 : //C
if (ballScanFlag == 0) { //
voice.VoiceWord(21);
voice.VoiceWord(66);
voice.VoiceWord(37);
ballScanFlag = 1;
FunctionFlag = 1; // Function 功能打开
} else if (ballScanFlag == 1) { //静止找球
voice.VoiceWord(21);
voice.VoiceWord(66);
voice.VoiceWord(37);
ballScanFlag = 2;
FunctionFlag = 1; // Function 功能打开
} else if (ballScanFlag == 2) { //转动找球
voice.VoiceWord(22);
FunctionFlag = 0; // Function 功能打开
}
break;
case 0x4444 : //D 小车运动功能停止
StopMove = StopMove ? false : true;
if (StopMove)
voice.VoiceWord(31);
else
voice.VoiceWord(32);
break;
}
// Serial.print('Z');
// Serial.println('Z');
}
void getCommand() {
char flag = 1;
uint8_t num = 0;
while (flag) {
if (Serial.available()) {
uint8_t tempBuffer = Serial.read();
readBuffer[num] = tempBuffer;
num++;
if (tempBuffer == 0x0D) {
Command = (readBuffer[0] < < 8) | (readBuffer[1]);
flag = 0;
}
}
if (followFlag == 1) // 超声波自动跟随
{
followDriveUltraSonic();
}
if (broadCastFlag == 1) // 距离播报
{
followDriveUltraSonic();
}
if (broadCastFlag == 2) // 迎宾小人
{
welcom();
}
if (broadCastFlag == 1) // 静止找球
{
Ball_Scan_Stop();
}
if (broadCastFlag == 2) // 转动找球
{
Ball_Scan_Rotate();
}
}
}
//******************************
// 功能：头部静止，转动车体找球
// 参数：无
// 返回值：全局变量dist 距离值cm
//*******************************
void Ball_Scan_Rotate() {
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
// grab blocks!
blocks = pixy.getBlocks();
// If there are detect blocks, print them!
if (blocks)
{
i++;
// do this (print) every 50 frames because printing every
// frame would bog down the Arduino
// if (i % 2 == 0) {
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
for (j = 0; j < 1; j++) //blocks
{
int x = pixy.blocks[j].x;
int y = pixy.blocks[j].y;
int width = pixy.blocks[j].width;
int height = pixy.blocks[j].height;
int angle = 0;
if (x < PIXY_MAX_X / 2 - RANGE) {
TurnRight();
}
if (x > PIXY_MAX_X / 2 + RANGE) {
TurnLeft();
}
if (y < PIXY_MAX_Y / 2 - RANGE) {
angle = map(y, PIXY_MAX_Y / 2, 0, 0, 5);
angleVer += angle;
angleVer = (angleVer >= maxVerAngle) ? maxVerAngle : angleVer;
verSer.write(angleVer);
}
if (y > PIXY_MAX_Y / 2 + RANGE) {
angle = map(y, PIXY_MAX_Y / 2, PIXY_MAX_Y, 0, 5);
angleVer -= angle;
angleVer = (angleVer <= minVerAngle) ? minVerAngle : angleVer;
verSer.write(angleVer);
}
}
// }
// else {
// Serial.println("no found");
// }
}
}
//******************************
// 功能：小车静止，头部找球
// 参数：无
// 返回值：全局变量dist 距离值cm
//*******************************
void Ball_Scan_Stop() {
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
// grab blocks!
blocks = pixy.getBlocks();
// If there are detect blocks, print them!
if (blocks)
{
i++;
// do this (print) every 50 frames because printing every
// frame would bog down the Arduino
// if (i % 2 == 0) {
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
for (j = 0; j < 1; j++) //blocks
{
int x = pixy.blocks[j].x;
int y = pixy.blocks[j].y;
int width = pixy.blocks[j].width;
int height = pixy.blocks[j].height;
int angle = 0;
if (x < PIXY_MAX_X / 2 - RANGE) {
angle = map(x, PIXY_MAX_X / 2, 0, 0, 9);
angleHor -= angle;
angleHor = (angleHor <= 0) ? 0 : angleHor;
horSer.write(angleHor);
}
if (x > PIXY_MAX_X / 2 + RANGE) {
angle = map(x, PIXY_MAX_X / 2, PIXY_MAX_X, 0, 9);
angleHor += angle;
angleHor = (angleHor >= 180) ? 180 : angleHor;
horSer.write(angleHor);
}
if (y < PIXY_MAX_Y / 2 - RANGE) {
angle = map(y, PIXY_MAX_Y / 2, 0, 0, 5);
angleVer += angle;
angleVer = (angleVer >= maxVerAngle) ? maxVerAngle : angleVer;
verSer.write(angleVer);
}
if (y > PIXY_MAX_Y / 2 + RANGE) {
angle = map(y, PIXY_MAX_Y / 2, PIXY_MAX_Y, 0, 5);
angleVer -= angle;
angleVer = (angleVer <= minVerAngle) ? minVerAngle : angleVer;
verSer.write(angleVer);
}
}
// }
// else {
// Serial.println("no found");
// }
}
}
void TurnLeft() {
if (StopMove) {
Serial.print('G');
delay(200);
}
}
void TurnRight() {
if (StopMove) {
Serial.print('C');
delay(200);
}
}
void Forward() {
if (StopMove) {
Serial.print('A');
Serial.print('A');
delay(100);
}
}
void Backward() {
if (StopMove) {
Serial.print('E');
delay(200);
}
}
void getDistance() //分别返回左右超声波传感器的距离 当no=0 左侧 n=1 右侧
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
dist = pulseIn(echoPin, HIGH) / 58;
}
//******************************
// 功能：播报当前的距离值
// 参数：无
// 返回值：全局变量dist 距离值cm
//*******************************
void broadDistance() //
{
getDistance();
Serial.println(dist);
voice.VoiceWord(15); //播报地址为15的语音，内容为：前方
voice.VoiceNum(dist); //播报距离值distance
voice.VoiceWord(16); //播报地址为15的语音，内容为：厘米
voice.VoiceWord(67); //播报地址为33的语音，内容为：障碍物
// delay(10);
}
//******************************
// 功能：跟随模式
// 参数：无
// 返回值：dist 距离值cm
//*******************************
void followDriveUltraSonic() {
getDistance();
if ((dist >= followDist - followBalance) && (dist <= followDist + followBalance)) {
if (dist >= followDist + 5) {
Forward();
delay(80);
}
if (dist <= followDist - 5) {
Backward();
delay(80);
}
}
}
//******************************
// 功能：welcom
// 参数：无
// 返回值：全局变量dist 距离值cm
//*******************************
void welcom() {
getDistance(); //获取当前超声波返回的距离值
if ((dist <= distMax) && (dist >= distMin)) { // ❷
if (preDist < dist) { // ❸
nearOrLeft = 1;
}
else {
nearOrLeft = 0;
}
if ((dist == distMin) && (nearOrLeft == 0)) { // ❹
voice.VoiceWord(21);
voice.VoiceWord(26);
verSer.write(maxVerAngle);
delay(500);
verSer.write(verAngleDefault);
}
else if ((dist == distMax) && (nearOrLeft == 1)) { // ❺
voice.VoiceWord(59);
}
preDist = dist;
}
}
