/*這段程式包含了讀取三軸加速度感測模組ADXL-335 xyz軸的類比輸出、超音波測具模組HC-SR04*2的輸出控制及輸入接收與距離換算，在程式內我們定義HC-SR04有效偵測距離為2.5m以下。
我們將Sensor所偵測的資料透過判斷式轉換成訊號事件表signal[3][3]並定義什麼樣的訊號組合該做什麼樣的回饋，並具有優先判斷碰撞的功能。*/
/*****************靈敏度調適*****************/
const int sr_ran=30; //超音波警示距離30cm
const int ad_ran=20; //三軸20個單位
/*****************環境參數******************/
int first=1;//宣告第一次讀取感測試值旗標
unsigned long time=0; //宣告晶片開啟時間暫存器
unsigned long previoustime=0; //宣告前一次晶片開啟時間暫存器
const unsigned int delaytime=1000; //宣告自定義延遲時間ms
const unsigned long timeout=15000;
/*宣告HC-SR04偵測之有效距離換算式為（2*距離/音速）秒＝
29.41（公分/微秒）
我們所使用之宣告為微秒（μs）
沒有設定的話 pulseIn（）的timeout預設為1秒，
注意timeout設定值要適宜、太高的話在超音波任何一邊沒有收到echo訊號的情況下
會嚴重影響到讀取感測器數值的速度！；太低則讀取不到echo訊號！*/
/**************** 超音波測距模組****************/
const int f_TrigPin = 4; //宣告前方超音波發送腳位f_TrigPin為PWM I/O 4
const int f_EchoPin = 5; //宣告前方超音波接收腳位f_EchoPin為PWM I/O 5
const int b_TrigPin = 6; //宣告後方超音波發送腳位b_TrigPin為PWM I/O 6
const int b_EchoPin = 7; //宣告後方超音波接收腳位b_EchoPin為PWM I/O 7
/****************超音波感測值暫存器****************/
long f_duration,b_duration, f_cm,b_cm;
//前方傳回時間,後方傳回時間,前方換算距離,後方換算距離
/****************三軸加速度感測器****************/
const int x = 0; //設定x軸讀取腳位 0
const int y = 1; //設定y軸讀取腳位 1
const int z = 2;//設定z軸類讀取腳位 2
/****宣告x、y、z_in暫存器用來放置三軸加速度數據****/
int x_in;
int y_in;
int z_in;
/****宣告x、y、z_bef暫存器用來放置前一次的三軸加速度數據****/
int x_bef;
int y_bef;
int z_bef;
/*****定義數位腳位47,49,51,53為與Arm6410L溝通之GPIO****/
int sig0=47;
int sig1=49;
int sig2=51;
int sig3=53;
/*****訊號事件列表*****/
int front=0,back=0,treaix=0;//宣告訊號暫存器
const char signal[3][3]={0,2,0,1,0,1,0,2,0};
//0代表安全狀態,1代表前方距離警示,2代表後方距離警示
/****輸入、輸出與鲍率設定****/
void setup()
{
/*******設定鲍率********/
Serial.begin(9600);
/*******設定sig0,1,2,3為GPIO輸出********/
pinMode(sig0,OUTPUT);
pinMode(sig1,OUTPUT);
pinMode(sig2,OUTPUT);
pinMode(sig3,OUTPUT);
/*******設定超音波測具模組I/O*******/
pinMode(f_TrigPin,OUTPUT);
pinMode(f_EchoPin,INPUT);
pinMode(b_TrigPin,OUTPUT);
pinMode(b_EchoPin,INPUT);
/*******設定三軸加速度感測模組I/O*******/
pinMode(x,INPUT);
pinMode(y,INPUT);
pinMode(z,INPUT);
}
/****主程式****/
void loop()
{
sigcache:
/**** 讀取三軸加速器數值 ***/
x_in = analogRead(x);
y_in = analogRead(y);
z_in = analogRead(z);
/*********** 超音波前方 *********/
digitalWrite(f_TrigPin, HIGH);
delayMicroseconds(10); //延遲10微秒
digitalWrite(f_TrigPin, LOW);
f_duration = pulseIn(f_EchoPin, HIGH,timeout); //f_duration＝傳回時間
f_cm = microsecondsToCentimeters(f_duration); //前方距離換算
/*********** 超音波後方 *********/
digitalWrite(b_TrigPin, HIGH);
delayMicroseconds(10); //延遲10微秒
digitalWrite(b_TrigPin, LOW);
b_duration = pulseIn(b_EchoPin, HIGH,timeout); //b_duration＝傳回時間
b_cm = microsecondsToCentimeters(b_duration); //後方距離換算
/************前方測距結果印出************/
Serial.print(“Front distance:");
Serial.print(f_cm);
Serial.println(" cm");
/************後方測距結果印出************/
Serial.print(“Back distance:");
Serial.print(b_cm);
Serial.println(" cm");
/************判斷式回饋區************/
if(( f_cm > sr_ran ))
front=0;
if(( f_cm!=0 )&&( f_cm < sr_ran))
front=1;
if(f_cm==0)
front=2;
if(( b_cm > sr_ran ))
back=0;
if(( b_cm!=0 )&&(b_cm < sr_ran))
back=1;
if(b_cm==0)
back=2;
treaix=(z_in < ( z_bef – ad_ran ) ) || ( z_in > ( z_bef + ad_ran ));
/***********事件回饋區***********/
if(first==0)
{
if(treaix) //碰撞事件
{
digitalWrite(sig3, HIGH),digitalWrite(sig2, LOW),digitalWrite(sig1, LOW),digitalWrite(sig0, LOW);
//GPIO碰撞事件訊號
Serial.println(“================================================================CRASH================================================================");
delay(1000); //等一秒
digitalWrite(sig3, LOW),digitalWrite(sig2, LOW),digitalWrite(sig1, LOW),digitalWrite(sig0, LOW);
//GPIO碰撞事件訊號還原
}
}
if(first==1) //剛開機沒有辦法取得前一次x、y、z數值，故需設定此旗標
{
first=0;
x_bef=x_in; //將前一次x軸數質放入x_bef儲存
y_bef=y_in; //將前一次y軸數質放入y_bef儲存
z_bef=z_in; //將前一次z軸數質放入x_bef儲存
}
switch(signal[front][back])
{
case 0:
goto sigcache; //如果為安全狀態、重新取的感測數值
case 1:
x_bef=x_in; //將前一次x軸數質放入x_bef儲存
y_bef=y_in; //將前一次y軸數質放入y_bef儲存
z_bef=z_in; //將前一次z軸數質放入z_bef儲存
digitalWrite(sig0, HIGH);//GPIO事件1訊號
Serial.println(“=========================================================Front BOMB=========================================================");
previoustime=millis();
while((time=millis()-previoustime) < delaytime) //自定義延時令我們在延時過程中還能讀取三軸資料做判斷運算
{
x_in = analogRead(x); //讀取x軸加速器數值
y_in = analogRead(y); //讀取y軸加速器數值
z_in = analogRead(z); //讀取z軸加速器數值
Serial.println(“=========================================================delay=========================================================");
if((z_in < ( z_bef – ad_ran ) ) || ( z_in > ( z_bef + ad_ran )))
{
digitalWrite(sig3, HIGH),digitalWrite(sig2, LOW),digitalWrite(sig1, LOW),digitalWrite(sig0, LOW);//GPIO碰撞事件訊號
Serial.println(“================================================================CRASH================================================================");
delay(1000); //等一秒
digitalWrite(sig3, LOW),digitalWrite(sig2, LOW),digitalWrite(sig1, LOW),digitalWrite(sig0, LOW);//GPIO碰撞事件訊號還原
}
}
digitalWrite(sig0, LOW); //還原GPIO事件訊號
break;
case 2:
x_bef=x_in; //將前一次x軸數質放入x_bef儲存
y_bef=y_in; //將前一次y軸數質放入y_bef儲存
z_bef=z_in; //將前一次z軸數質放入z_bef儲存
digitalWrite(sig1, HIGH);//GPIO事件2訊號
Serial.println(“=========================================================Back BOMB=========================================================");
previoustime=millis();
while((time=millis()-previoustime) < delaytime) //自定義延時令我們在延時過程中還能讀取三軸資料做判斷運算
{
x_in = analogRead(x);
y_in = analogRead(y);
z_in = analogRead(z);
Serial.println(“=========================================================delay=========================================================");
if((z_in < ( z_bef – ad_ran ) ) || ( z_in > ( z_bef + ad_ran )))
{
digitalWrite(sig3, HIGH),digitalWrite(sig2, LOW),digitalWrite(sig1, LOW),digitalWrite(sig0, LOW);//GPIO事件2訊號
Serial.println(“================================================================CRASH================================================================");
delay(1000); //等一秒
digitalWrite(sig3, LOW),digitalWrite(sig2, LOW),digitalWrite(sig1, LOW),digitalWrite(sig0, LOW);//GPIO事件2訊號還原
}
}
digitalWrite(sig1, LOW);//還原GPIO事件2訊號
break;
}
}
/** 將音速轉換成距離 『物體距離（公分） =回應時間÷29.41（公分/微秒）÷2（來回）』**/
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29.41 / 2;
}
