|
|
#include<IRremote.h>//包含紅外庫 關(guān)鍵點(diǎn)
#include "VarSpeedServo.h" //包含 VarSpeedServo library
int RECV_PIN = A4;//端口聲明
IRrecv irrecv(RECV_PIN);
decode_results results;//結(jié)構(gòu)聲明
int on = 0;//標(biāo)志位
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;//定義蜂鳴器 數(shù)字A3 接口
int Echo = A1; // Echo回聲腳(P2.0)
int Trig =A0; // Trig 觸發(fā)腳(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; // 定義數(shù)組編號為5。
/* 當(dāng)遇到障礙時(shí)，轉(zhuǎn)過身來。每一步都需要5組數(shù)據(jù)。每組4個(gè)伺服系統(tǒng)的角度改變*/
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個(gè)數(shù)組，每個(gè)數(shù)組包含4個(gè)元素來存儲伺服的當(dāng)前角度值。
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); // 給觸發(fā)腳低電平2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // 給觸發(fā)腳高電平10μs，這里至少是10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // 持續(xù)給觸發(fā)腳低電
float Fdistance = pulseIn(Echo, HIGH); // 讀取高電平時(shí)間(單位：微秒)
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(); // 開始啟動(dòng)紅外遙控
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)) //調(diào)用庫函數(shù)：解碼
{
// If it's been at least 1/4 second since the last
// IR received, toggle the relay
if (millis() - last > 250) //確定接收到信號
{
on = !on;//標(biāo)志位置反
digitalWrite(13, on ? HIGH : LOW);//板子上接收到信號閃爍一下led
dump(&results);//解碼紅外信號
}
if (results.value == ir_run_car )//按鍵CH
{
n_Forward(4,250); //前進(jìn)
}
if (results.value == ir_back_car )//按鍵+
{
n_Backward(2,650);//后退
}
if (results.value == ir_left_car )//按鍵<<
{
n_left_turn(1,650);//左轉(zhuǎn)
}
if (results.value == ir_right_car )//按鍵>||
{
n_right_turn(1,650);//右轉(zhuǎn)
}
if (results.value == ir_stop_car )//按鍵>>|
{
stop();//停車
}
last = millis();
irrecv.resume(); // Receive the next value
}
}
|
|
