#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;
double pitch, roll, yaw, AngleX, AngleY, AngleZ, Xrate, Yrate, Zrate;
int H,L,X,sw,H0,L0,X0,roll_0,pitch_0;
int l,h,a,c;
// 中位值濾波法
#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) - L0 ;
}
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];
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) - H0 ;
}
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];
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);
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 = analogRead(A0);
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 = analogRead(A0);
if(sw>1){
c=1;
lcd.setCursor(11,1);
lcd.print("open ");
}else{
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(" ");
}
}
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x3F,16,2); // 若LCD 無法啟用，請將0X3F 改為0X27
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 540 // this is the 'maximum' pulse length count (out of 4096)
#include <SoftwareSerial.h>
#define RX 3 //HC-05 TX接3
#define TX 2 //HC-05 RX接2
SoftwareSerial BT(RX,TX);
int R , L , B , a,b,c;
long msg,cmd;
unsigned long t1,t2;
int c1,c2,a0,b0;
int L1,L2,H1,H2;
void servoA_write(int d){
int x = map(d,0,180,SERVOMIN,SERVOMAX);
pwm.setPWM(0, 0, x);
}
void servoB_write(int d){
int x = map(d,0,180,SERVOMIN,SERVOMAX);
pwm.setPWM(1, 0, x);
}
void servoC_write(int d){
int x = map(d,0,180,SERVOMIN,SERVOMAX);
pwm.setPWM(2, 0, x);
}
void servoD_write(int d){
int x = map(d,0,180,SERVOMIN,SERVOMAX);
pwm.setPWM(3, 0, x);
}
void go_zero(){
servoA_write(135);
servoC_write(90);
servoD_write(80);
}
void forward(int arm_length , int arm_height){
arm_length = constrain(arm_length,0,45);
R= 135 + arm_length - arm_height;
L= 80 + arm_length + arm_height;
R=constrain(R,60,180);
L=constrain(L,90,162);
servoA_write(R);
servoD_write(L);
LCD_Display(100,arm_length,arm_height);
}
void LCD_Display(int t , int arm_length , int arm_height){
if(millis()-t1>t){
lcd.clear();
lcd.setCursor(0,0);
lcd.print("L=");
lcd.print(arm_length);
lcd.print(" H=");
lcd.print(arm_height);
t1=millis();
}
}
void setup(){
Serial.begin(9600);
BT.setTimeout(10); // 設定為每10毫秒結束一次讀取(數字愈小愈快)
BT.begin(38400);
lcd.init();
lcd.backlight();
pwm.begin();
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
yield();
servoA_write(135);
servoD_write(80);
}
void loop(){
if(BT.available()>0){
msg = BT.parseInt();
}
lcd.setCursor(0,1);
lcd.print(msg);
lcd.print(" ");
if(msg!=0){
if(msg>10000||msg<(-10000)){
cmd=msg;
}
if(cmd>0){
lcd.setCursor(11,1);
lcd.print("open ");
servoB_write(150);
}else{
servoB_write(115);
lcd.setCursor(11,1);
lcd.print("close");
}
if(cmd<0){cmd=-1*cmd; }
a=cmd%100;
b=cmd/100%100;
c=cmd/10000;
if(a-a0>15||a-a0<-15){
a=a0;
}
if(b-b0>15||b-b0<-15){
b=b0;
}
if(c==1){
c1=c1+2;
if(c1>90){c1=90;}
}
if(c==2){c1=c1;}
if(c==3){
c1=c1-2;
if(c1<-90){c1=-90;}
}
LCD_Display(100,45-a,b);
servoC_write(90+c1);
forward(45-a,b);
a0=a;
b0=b;
Serial.print(45-a);
Serial.print(",");
Serial.print(b);
Serial.print(",");
Serial.println(c);
}
}