meArm 專題(一)

感測端

#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050.h"

#include <LiquidCrystal_I2C.h>

#include <Kalman.h>

#include <SoftwareSerial.h>

#define RX 3 //HC-05 TX接3

#define TX 2 //HC-05 RX接2

#define X_Trig 7

#define X_Echo 6

#define L_Trig 9

#define L_Echo 8

#define H_Trig 11

#define H_Echo 10

SoftwareSerial BT(RX,TX);

LiquidCrystal_I2C lcd(0x3F,16,2); // 若LCD 無法啟用，請將0X3F 改為0X27

Kalman kalmanX , kalmanY, kalmanZ;

MPU6050 accelgyro;

int16_t ax, ay, az, gx, gy, gz;

double accX, accY, accZ, gyroX, gyroY, gyroZ;

uint32_t timer , t1 , t2 , t3, t4;

double pitch, roll, yaw, AngleX, AngleY, AngleZ, Xrate, Yrate, Zrate;

int H,L,X,H0,L0,X0,roll_0,pitch_0,sw_state=1;

int l,h,a,c;

unsigned long sw;

// 中位值濾波法

#define FILTER_N 3

int filter_buf[FILTER_N];

int i, j;

int filter_temp;

void dist_X(){

for(int i=0; i<3 ; i++){

digitalWrite(X_Trig, LOW);

delayMicroseconds(2);

digitalWrite(X_Trig, HIGH); delayMicroseconds(10);

digitalWrite(X_Trig, LOW);

double t = pulseIn(X_Echo, HIGH,50000);

filter_buf[i] = int(340*t/20000) - X0 ;

}

for(j = 0; j < FILTER_N - 1; j++) {

for(i = 0; i < FILTER_N - 1 - j; i++) {

if(filter_buf[i] > filter_buf[i + 1]) {

filter_temp = filter_buf[i];

filter_buf[i] = filter_buf[i + 1];

filter_buf[i + 1] = filter_temp;

}}}

X=filter_buf[(FILTER_N - 1) / 2];

if(X<0){X=0;}else if(X>60){X=60;}

}

void dist_L(){

for(int i=0; i<3 ; i++){

digitalWrite(L_Trig, LOW);

delayMicroseconds(2);

digitalWrite(L_Trig, HIGH); delayMicroseconds(10);

digitalWrite(L_Trig, LOW);

double t = pulseIn(L_Echo, HIGH,50000);

filter_buf[i] = int(340*t/20000) ;

}

for(j = 0; j < FILTER_N - 1; j++) {

for(i = 0; i < FILTER_N - 1 - j; i++) {

if(filter_buf[i] > filter_buf[i + 1]) {

filter_temp = filter_buf[i];

filter_buf[i] = filter_buf[i + 1];

filter_buf[i + 1] = filter_temp;

}}}

L=filter_buf[(FILTER_N - 1) / 2]- L0;

if(L<0){L=0;}else if(L>45){L=45;}

lcd.setCursor(0,0);

lcd.print("L=");

lcd.print(L);

lcd.print(" ");

}

void dist_H(){

for(int i=0; i<3 ; i++){

digitalWrite(H_Trig, LOW);

delayMicroseconds(2);

digitalWrite(H_Trig, HIGH); delayMicroseconds(10);

digitalWrite(H_Trig, LOW);

double t = pulseIn(H_Echo, HIGH,50000);

filter_buf[i] = int(340*t/20000) ;

}

for(j = 0; j < FILTER_N - 1; j++) {

for(i = 0; i < FILTER_N - 1 - j; i++) {

if(filter_buf[i] > filter_buf[i + 1]) {

filter_temp = filter_buf[i];

filter_buf[i] = filter_buf[i + 1];

filter_buf[i + 1] = filter_temp;

}}}

H=filter_buf[(FILTER_N - 1) / 2]- H0;

if(H<0){H=0;}else if(H>75){H=75;}

lcd.setCursor(7,0);

lcd.print("H=");

lcd.print(H);

lcd.print(" ");

}

void get_angle(){

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accX = ax;

accY = ay;

accZ = az;

gyroX = gx;

gyroY = gy;

gyroZ = gz;

double dt = (double)(micros() - timer) / 1000000; // Calculate delta time

timer = micros();

pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

roll = atan2(accY,accZ) * RAD_TO_DEG;

yaw = atan2(accX , accY) * RAD_TO_DEG;

Yrate = gyroY / 131.0; // Convert to deg/s

Xrate = gyroX / 131.0;

Zrate = gyroZ / 131.0;

AngleY = kalmanY.getAngle(pitch, Yrate, dt) - roll_0;

AngleX = kalmanX.getAngle(roll, Xrate, dt) - pitch_0;

}

void tx(int cmd_l , int cmd_h , int cmd_a , int cmd_c , int t){

if(cmd_c>0){cmd_c=1;}else if(cmd_c<0){cmd_c=-1;}

if(millis()>t1+t){

long msg = cmd_c * (cmd_a*10000 + cmd_h*100 + cmd_l);

BT.print(msg);

t1=millis();

}}

void setup() {

Wire.begin();

lcd.init();

lcd.backlight();

lcd.clear();

Serial.begin(9600);

BT.begin(38400);

pinMode(X_Trig, OUTPUT);

pinMode(L_Trig, OUTPUT);

pinMode(H_Trig, OUTPUT);

pinMode(13, OUTPUT);

pinMode(14, INPUT_PULLUP);

accelgyro.initialize();

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accX = ax; accY = ay; accZ = az; gyroX = gx; gyroY = gy; gyroZ = gz;

pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

roll = atan2(accY , accZ) * RAD_TO_DEG;

yaw = atan2(accX , accY) * RAD_TO_DEG;

kalmanY.setAngle(pitch);

kalmanX.setAngle(roll);

kalmanZ.setAngle(yaw);

do{

sw = digitalRead(14);

lcd.setCursor(0,0);

lcd.print("Push the Button");

get_angle();

}while(sw==1);

dist_X();

dist_L();

dist_H();

get_angle();

X0 = X;

L0 = L;

H0 = H;

roll_0 = AngleY;

pitch_0 = AngleX;

lcd.clear();

}

void loop() {

get_angle();

if(AngleX<10&&AngleX>-10&&AngleY<10&&AngleY>-10){

sw = digitalRead(A0);

Serial.println(sw);

t4=millis();

if(sw==0&&t4>t3+500){

delay(20);

sw = digitalRead(A0);

if(sw==0){

sw_state=-1* sw_state;

t3=t4;

}

}

if(sw_state==1){

c=1;

lcd.setCursor(11,1);

lcd.print("open ");

}else if(sw_state==-1){

c=-1;

lcd.setCursor(11,1);

lcd.print("close");

}

digitalWrite(13,1);

dist_L();

dist_H();

dist_X();

if(X<8){

a=1;

lcd.setCursor(15,0);

lcd.print("R");

}else if(X>=8&&X<25){

a=2;

lcd.setCursor(15,0);

lcd.print("S");

}else if(X>=25&&X<50){

a=3;

lcd.setCursor(15,0);

lcd.print("L");

}

tx(L,H,a,c,100),

lcd.setCursor(0,1);

lcd.print(int(AngleX));

lcd.print(" ");

lcd.print(int(AngleY));

lcd.print(" ");

}else{

digitalWrite(13,0);

lcd.setCursor(0,0);

lcd.print(" ");

lcd.setCursor(0,1);

lcd.print("Keep horizontal");

lcd.print(" ");

}}