#include<IRremote.h>//包含红外库 关键点
#include "VarSpeedServo.h" //包含 VarSpeedServo library
int RECV_PIN = A4;//端口声明
IRrecv irrecv(RECV_PIN);
decode_results results;//结构声明
int on = 0;//标志位
unsigned long last = millis();
long ir_run_car = 0x00FF629D;//按键CH
long ir_back_car = 0x00FFA857;//按键+
long ir_left_car = 0x00FF22DD;//按键<<
long ir_right_car = 0x00FFC23D;//按键>||
long ir_stop_car = 0x00FF02FD;//按键>>|
long ir_left_turn = 0x00ffE01F;//按键-
long ir_right_turn = 0x00FF906F;//按键EQ
//==============================
VarSpeedServo RU; //Right Upper
VarSpeedServo RL; //Right Lower
VarSpeedServo LU; //Left Upper
VarSpeedServo LL; //Left Lower
int beep=A3;//定义蜂鸣器 数字A3 接口
int Echo = A1; // Echo回声脚(P2.0)
int Trig =A0; // Trig 触发脚(P2.1)
int Distance = 0;
//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL )
const int delay_tim = 300; //Delay 750ms
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,40,-20},
{30,0,40,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
};
const int num3 = 5; // 定义数组编号为5。
/* 当遇到障碍时，转过身来。每一步都需要5组数据。每组4个伺服系统的角度改变*/
const int array_left[num3][4] =
{
{-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees.
{-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
}; //定义5个数组，每个数组包含4个元素来存储伺服的当前角度值。
const int num4 = 5;
const int array_right[num4][4] =
{
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
{-30,0,-40,0},
{0,0,0,0},
};
//#define INSTALL
//#define CALIBRATION
#define RUN
void Servo_Init()
{
RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9
RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10
LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11
LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12
}
void Adjust() // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree); // in steps of 1 degree
LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
delay(15); // waits 15ms for the servo to reach the RU_Degreeition
}
}
void TooClose()
{
digitalWrite(Trig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // 给触发脚高电平10μs，这里至少是10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // 持续给触发脚低电
float Fdistance = pulseIn(Echo, HIGH); // 读取高电平时间(单位：微秒)
Fdistance= Fdistance/58; //为什么除以58等于厘米， Y米=（X秒*344）/2
// X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
Serial.print("Distance:"); //输出距离（单位：厘米）
Serial.println(Fdistance); //显示距离
Distance = Fdistance;
}
void Forward(unsigned int n_delay)
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(n_delay); //delay_Forward :700
}
}
void stop()
{
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(delay_tim);
}
void Backward(unsigned int n_delay)
{
for(int y=0; y<num2; y++) {
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(n_delay); //delay_Forward:700
}
}
void left_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num3; y++)
{
RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void right_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num4; y++)
{
RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void n_Forward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Forward(n_delay);
}
}
void n_Backward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Backward(n_delay);
}
}
void n_left_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
left_turn(n_delay);
}
}
void n_right_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
right_turn(n_delay);
}
}
void setup()
{
#ifdef INSTALL
Servo_Init();
RU.slowmove (90 , vel);
RL.slowmove (90 , vel);
LU.slowmove (90 , vel);
LL.slowmove (90 , vel);
while(1);
#endif
#ifdef CALIBRATION
Servo_Init();
RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo.
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
while(1);
#endif
#ifdef RUN
Servo_Init();
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
#endif
irrecv.enableIRIn(); // 开始启动红外遥控
pinMode(Echo, INPUT); // 定义超声波输入脚
pinMode(Trig, OUTPUT); // 定义超声波输出脚
pinMode(beep,OUTPUT); //蜂鸣器
}
void dump(decode_results *results)
{
int count = results->rawlen;
if (results->decode_type == UNKNOWN)
{
//Serial.println("Could not decode message");
// brake();
}
}
void loop()
{
if (irrecv.decode(&results)) //调用库函数：解码
{
// If it's been at least 1/4 second since the last
// IR received, toggle the relay
if (millis() - last > 250) //确定接收到信号
{
on = !on;//标志位置反
digitalWrite(13, on ? HIGH : LOW);//板子上接收到信号闪烁一下led
dump(&results);//解码红外信号
}
if (results.value == ir_run_car )//按键CH
{
n_Forward(4,250); //前进
}
if (results.value == ir_back_car )//按键+
{
n_Backward(2,650);//后退
}
if (results.value == ir_left_car )//按键<<
{
n_left_turn(1,650);//左转
}
if (results.value == ir_right_car )//按键>||
{
n_right_turn(1,650);//右转
}
if (results.value == ir_stop_car )//按键>>|
{
stop();//停车
}
last = millis();
irrecv.resume(); // Receive the next value
}
}
|
