控制手套

PCA9685 函式庫

手套端

#include <Wire.h>

#include <HMC5883L.h>

#include <ADXL345.h>

#include <LiquidCrystal_I2C.h>

#include <SoftwareSerial.h>

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

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

SoftwareSerial BT(RX,TX);

#define TrigX 4

#define EchoX 5

#define TrigZ 6

#define EchoZ 7

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

HMC5883L compass;

ADXL345 accelerometer;

float heading0;

float heading1;

float heading2;

float x0,x,x1,x2,z0,z,z1,z2;

int now,old,A,B;

int output;

int Temp=30;

unsigned long t,t1,t2,t3,t4,t5,t6;

double pitch, roll, yaw;

int sw=1;

//無傾斜補償

float noTiltCompensate(Vector mag){

float heading = atan2(mag.YAxis, mag.XAxis);

return heading;

}

// 傾斜補償

float tiltCompensate(Vector mag, Vector normAccel){

roll = asin(normAccel.YAxis);

pitch = asin(-normAccel.XAxis);

if (roll > 0.78 || roll < -0.78 || pitch > 0.78 || pitch < -0.78){return -1000; }

float cosRoll = cos(roll);

float sinRoll = sin(roll);

float cosPitch = cos(pitch);

float sinPitch = sin(pitch);

float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;

float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

float heading = atan2(Yh, Xh);

return heading;

}

//角度校正

float correctAngle(float heading){

if (heading < 0) { heading += 2 * PI; }

if (heading > 2 * PI) { heading -= 2 * PI; }

return heading;

}

//得到角度

void getdata(){

// 讀取向量值

Vector mag = compass.readNormalize();

Vector acc = accelerometer.readScaled();

// Calculate headings

heading1 = noTiltCompensate(mag);

heading2 = tiltCompensate(mag, acc);

if (heading2 == -1000){

heading2 = heading1;

}

// Set declination angle on your location and fix heading

// You can find your declination on: http://magnetic-declination.com/

// (+) Positive or (-) for negative

// 台北地區的磁偏角是 4'4 WEST (negative)

// Formula: (deg + (min / 60.0)) / (180 / M_PI);

float declinationAngle = (-4.0 - (4.0 / 60.0)) / (180 / M_PI);

heading1 += declinationAngle;

heading2 += declinationAngle;

// Correct for heading < 0deg and heading > 360deg

heading1 = correctAngle(heading1);

heading2 = correctAngle(heading2);

// Convert to degrees

heading1 = heading1 * 180/M_PI;

heading2 = heading2 * 180/M_PI;

}

void get_dist(){

digitalWrite(TrigX, LOW);

delayMicroseconds(2);

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

digitalWrite(TrigX, LOW);

t = pulseIn(EchoX, HIGH,50000);

x1 = (331+0.6*Temp)*t/20000 ;

if(x1>3&&x1<500){x2=x1;}

digitalWrite(TrigZ, LOW);

delayMicroseconds(2);

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

digitalWrite(TrigZ, LOW);

t = pulseIn(EchoZ, HIGH,50000);

z1 = (331+0.6*Temp)*t/20000 ;

if(z1>3&&z1<500){z2=z1;}

}

void lcddisp(){

t2 = millis();

if(t2 >t1+100){

lcd.setCursor(0,0);

lcd.print(x1);

lcd.print(" ");

lcd.setCursor(8,0);

lcd.print(z1);

lcd.print(" ");

lcd.setCursor(0,1);

lcd.print(heading0);

lcd.print(" ");

lcd.setCursor(8,1);

lcd.print("op=");

lcd.print(output);

lcd.print(" ");

t1=t2;

}}

void setup(){

Serial.begin(38400);

BT.begin(38400);

lcd.init();

lcd.backlight();

lcd.clear();

pinMode(TrigX, OUTPUT);

pinMode(TrigZ, OUTPUT);

if (!accelerometer.begin()){delay(500);} // 初始化 ADXL345

accelerometer.setRange(ADXL345_RANGE_2G);

while (!compass.begin()) { delay(500);} // 初始化電子羅盤 HMC5883L

compass.setRange(HMC5883L_RANGE_1_3GA); //設定測量範圍

compass.setMeasurementMode(HMC5883L_CONTINOUS); // 設定測量模式

compass.setDataRate(HMC5883L_DATARATE_30HZ); // 設定資料傳輸速率

compass.setSamples(HMC5883L_SAMPLES_8); //設樣本平均數的組數

compass.setOffset(0, 0); //設定檢量線的數值

delay(100);

getdata();

heading0=heading2;

old=90;

get_dist();

x0=x2;

z0=z2;

}

void loop(){

getdata();

if(heading0>=0&&heading0<90){

if(heading2<180&&heading2>=0){

now = 90 + heading2 - heading0 ;

}else if(heading2<360&&heading2>=270){

now = 90 + heading2 - heading0 - 360 ;

}

if(heading0>=90&&heading0<180){

if(heading2<270&&heading2>=0){

now = 90 + heading2 - heading0 ;

}}

if(heading0>=180&&heading0<270){

if(heading2<360&&heading2>=90){

now = 90 + heading2 - heading0 ;

}}

if(heading0>=270&&heading0<360){

if(heading2<360&&heading2>=180){

now = 90 + heading2 - heading0 ;

}else if(heading2<90&&heading2>=0){

now = 90 + heading2 - heading0 + 360 ;

}

if(analogRead(A0)<100){

sw=sw*(-1);

delay(100);}

now=constrain(now,0,180);

output=4180-now;

t4=millis();

if(t4>t3+50){

BT.println(output);

BT.println(3003+sw);

t3=t4;

}

get_dist();

x=1000 + 500 + x0 - x2;

z=2000 + 500 + z0 - z2;

t6=millis();

if(t6>t5+50){

BT.println(x);

BT.println(z);

t5=t6;

}

lcddisp();

}

第二版

手套端

機械手臂端

#include <Wire.h>

#include <LiquidCrystal_I2C.h>

#include <SoftwareSerial.h>

#include <Adafruit_PWMServoDriver.h>

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

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

SoftwareSerial BT(RX,TX);

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

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)

String data_A="90";

String data_B="90";

String data_C="90";

String data_D="90";

int A , B ;

int a0,a1,a2,b0,b1,b2,c0,c1,c2,d0,d1,d2;

unsigned long t1,t2,t3,t4;

int p,q,y;

void servoA_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(8, 0, x);

}

void servoB_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(10, 0, x);

}

void servoC_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(9, 0, x);

}

void servoD_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(11, 0, x);

}

void setup(){

Serial.begin(38400);

BT.begin(38400);

pwm.begin();

pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates

yield();

BT.setTimeout(10); // 設定為每10毫秒結束一次讀取(數字愈小愈快)

lcd.init();

lcd.backlight();

lcd.clear();

servoA_write(90);

servoB_write(90);

servoC_write(90);

servoD_write(90);

a0=90,b0=90,c0=90,d0=90;

a2=90,b2=90,c2=90,d2=90;

a1=90,b1=90,c1=90,d1=90;

}

void loop(){

if(BT.available()>0){

y = BT.parseInt();

}

p=y/1000;

q=y%1000;

if(p==1){A=q; }

if(p==2){B=q; }

if(p==3){c2=q; }

if(p==4){d2=q; }

a2= 83 + 3*(A-500);

b2= 132-6*(B-500);

if(a2+b2>=270){

b2=270-a2;

}else if(a2+b2<150){

b2=150-a2;

}

if(a2<0){a2=0;}else if(a2>180){a2=180;}

if(b2>150){b2=150;}else if(b2<0){b2=0;}

//servoD_write(d2);

if(d2>d1){

for(int i=d1;i<d2;i++){

servoD_write(i);

delay(10);

}

d1=d2;

}else if(d2<d1){

for(int i=d1; i>d2 ;i--){

servoD_write(i);

delay(10);

}

d1=d2;

}else if(d2==d1){

servoD_write(d2);

}

t4=millis();

if(t4>t3+10){

int s = 1;

if(a2>a1){a1=a0+s;}else if(a2==a1){a1=a0;}else if(a2<a1){a1=a0-s;}

servoA_write(a1);

a0=a1;

if(b2>b1){b1=b0+s;}else if(b2==b1){b1=b0;}else if(b2<b1){b1=b0-s;}

servoB_write(b1);

b0=b1;

if(c2==4){servoC_write(50);}else if(c2==2){servoC_write(120);}

if(d2>d1){d1=d0+s;}else if(d2==d1){d1=d0;}else if(d2<d1){d1=d0-s;}

servoD_write(d1);

d0=d1;

t3=t4;

}

t2=millis();

if(t2>t1+300){

lcd.setCursor(0,0);

lcd.print(" ");

lcd.setCursor(0,0);

lcd.print("A=");

lcd.print(A-500);

lcd.setCursor(8,0);

lcd.print("B=");

lcd.print(B-500);

lcd.setCursor(0,1);

lcd.print(" ");

lcd.setCursor(0,1);

lcd.print("D=");

lcd.print(d2);

lcd.setCursor(8,1);

lcd.print("C=");

lcd.print(c2);

t1=t2;

}

第二版

手臂端

#include <Wire.h>

#include <LiquidCrystal_I2C.h>

#include <SoftwareSerial.h>

#include <Adafruit_PWMServoDriver.h>

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

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

SoftwareSerial BT(RX,TX);

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

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)

int A , B ;

int a0,a1,a2,b0,b1,b2,c0,c1,c2,d0,d1,d2;

unsigned long t1,t2,t3,t4;

int p,q,y;

int fb0,fb1,ud0,ud1,z;

void servoA_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(8, 0, x);

}

void servoB_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(10, 0, x);

}

void servoC_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(9, 0, x);

}

void servoD_write(int d){

int x = map(d,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(11, 0, x);

}

void lcd_display(int time_interval){

t2=millis();

if(t2>t1+time_interval){

lcd.setCursor(0,0);

lcd.print("A=");

lcd.print(p);

lcd.setCursor(8,0);

lcd.print("B=");

lcd.print(q);

lcd.setCursor(0,1);

lcd.print("D=");

lcd.print(d1);

lcd.setCursor(8,1);

lcd.print("C=");

lcd.print(y);

t1=t2;

}}

void setup(){

Serial.begin(38400);

BT.begin(38400);

pwm.begin();

pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates

yield();

BT.setTimeout(10); // 設定為每10毫秒結束一次讀取(數字愈小愈快)

lcd.init();

lcd.backlight();

lcd.clear();

servoA_write(85);

servoB_write(50);

servoC_write(60);

servoD_write(90);

d0=90;

a1=85;

b1=50;

fb0=85;

fb1=85;

}

void loop(){

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

lcd.setCursor(0,1);

lcd.print(q);

//決定左右

while(p==1){

lcd.setCursor(0,0);

lcd.print("Left / Right 1 ");

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

lcd.setCursor(0,0);

lcd.print("Left / Right 1 ");

p=y/10;

q=y%10;

}

if(p==1&&q==6){ //向右

d1=d0-1;

if(d1<0){d1=0;}

servoD_write(d1);

lcd.setCursor(0,1);

lcd.print("Right...");

lcd.setCursor(13,1);

lcd.print(d1);

lcd.print(" ");

d0=d1;

}else if(p==1&&q==4){ //向左

d1=d0+1;

if(d1>180){d1=180;}

servoD_write(d1);

lcd.setCursor(0,1);

lcd.print("Left... ");

lcd.setCursor(13,1);

lcd.print(d1);

lcd.print(" ");

d0=d1;

}else if(p==1&&q==5){ //不動

servoD_write(d0);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(d0);

lcd.print(" ");

}

delay(5); //這個delay會決定 servo 速度

}

//////////////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定前後

while(p==2){

lcd.setCursor(0,0);

lcd.print("Back / Forward 2 ");

if(BT.available()>0){ y = BT.parseInt();}

if(y!=0){

p=y/10;

q=y%10;

lcd.setCursor(0,0);

lcd.print("Back / Forward 2 ");

}

//伸長

if(p==2&&q==6){

fb1=fb0+1;

if(fb1>170){fb1=170;}

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1=fb1-70; }else{ b1=135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Forward...");

lcd.setCursor(13,1);

lcd.print(fb1);

lcd.print(" ");

fb0=fb1;

}

//縮回

if(p==2&&q==4){

fb1=fb0-1;

if(fb1<0){fb1=0;}

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1=fb1-70; }else{ b1=135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Back...");

lcd.setCursor(13,1);

lcd.print(fb1);

lcd.print(" ");

fb0=fb1;

}

//不動

if(p==2&&q==5){

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1 = fb1-70 ; }else{ b1= 135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(fb0);

lcd.print(" ");

fb0=fb1;}

delay(2);

ud0=b1;

ud1=b1;

z=a1+b1;

}

///////////////////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定上下

while(p==3){

lcd.setCursor(0,0);

lcd.print("High / Low 3 ");

if(BT.available()>0){ y = BT.parseInt();}

if(y!=0){ p=y/10 , q=y%10;

lcd.setCursor(0,0);

lcd.print("High / Low 3");

}

//抬高

if(p==3&&q==6){

ud1=ud0+1;

if(ud1>165){ud1=165;}

b1=ud1;

servoB_write(b1);

a1= z-ud1;

servoA_write(a1);

lcd.setCursor(0,1);

lcd.print("UP.....");

lcd.setCursor(13,1);

lcd.print(ud1);

lcd.print(" ");

ud0=ud1;

}

//降低

if(p==3&&q==4){

ud1=ud0-1;

if(z-ud1>170){ud1=z-170;}

b1=ud1;

servoB_write(b1);

a1= z-ud1;

servoA_write(a1);

lcd.setCursor(0,1);

lcd.print("Down...");

lcd.setCursor(13,1);

lcd.print(ud1);

lcd.print(" ");

ud0=ud1;}

//不動

if(p==3&&q==5){

b1=ud1;

a1= z-ud1;

servoA_write(a1);

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(ud0);

lcd.print(" ");

ud0=ud1;}

delay(2);

ud0=b1;

ud1=b1;

z=a1+b1;

}

///////////////////////////////////////////////////////////////////////////

if(BT.available()>0){y = BT.parseInt();}

if(y!=0){

p=y/10;

q=y%10;

}

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

lcd.setCursor(0,1);

lcd.print(q);

//決定左右

while(p==4){

lcd.setCursor(0,0);

lcd.print("Left / Right 4 ");

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

lcd.setCursor(0,0);

lcd.print("Left / Right 4 ");

p=y/10;

q=y%10;

}

if(p==4&&q==6){ //向右

d1=d0-1;

if(d1<0){d1=0;}

servoD_write(d1);

lcd.setCursor(0,1);

lcd.print("Right...");

lcd.setCursor(13,1);

lcd.print(d1);

lcd.print(" ");

d0=d1;

}else if(p==4&&q==4){ //向左

d1=d0+1;

if(d1>180){d1=180;}

servoD_write(d1);

lcd.setCursor(0,1);

lcd.print("Left... ");

lcd.setCursor(13,1);

lcd.print(d1);

lcd.print(" ");

d0=d1;

}else if(p==4&&q==5){ //不動

servoD_write(d0);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(d0);

lcd.print(" ");

}

delay(5); //這個delay會決定 servo 速度

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){y = BT.parseInt();}

if(y!=0){

p=y/10;

q=y%10;

}

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){y = BT.parseInt();}

if(y!=0){

p=y/10;

q=y%10;

}

//決定上下

while(p==5){

lcd.setCursor(0,0);

lcd.print("High / Low 5 ");

if(BT.available()>0){ y = BT.parseInt();}

if(y!=0){ p=y/10 , q=y%10;

lcd.setCursor(0,0);

lcd.print("High / Low 5");

}

//抬高

if(p==5&&q==6){

ud1=ud0+1;

if(ud1>165){ud1=165;}

b1=ud1;

servoB_write(b1);

a1= z-ud1;

servoA_write(a1);

lcd.setCursor(0,1);

lcd.print("UP.....");

lcd.setCursor(13,1);

lcd.print(ud1);

lcd.print(" ");

ud0=ud1;

}

//降低

if(p==5&&q==4){

ud1=ud0-1;

if(z-ud1>170){ud1=z-170;}

b1=ud1;

servoB_write(b1);

a1= z-ud1;

servoA_write(a1);

lcd.setCursor(0,1);

lcd.print("Down...");

lcd.setCursor(13,1);

lcd.print(ud1);

lcd.print(" ");

ud0=ud1;}

//不動

if(p==5&&q==5){

b1=ud1;

a1= z-ud1;

servoA_write(a1);

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(ud0);

lcd.print(" ");

ud0=ud1;}

delay(2);

fb0=a1;

fb1=a1;

}

//////////////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定前後

while(p==6){

lcd.setCursor(0,0);

lcd.print("Back / Forward 6");

if(BT.available()>0){ y = BT.parseInt();}

if(y!=0){

p=y/10;

q=y%10;

lcd.setCursor(0,0);

lcd.print("Back / Forward 6 ");

}

//伸長

if(p==6&&q==6){

fb1=fb0+1;

if(fb1>170){fb1=170;}

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1=fb1-70; }else{ b1=135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Forward...");

lcd.setCursor(13,1);

lcd.print(fb1);

lcd.print(" ");

fb0=fb1;

}

//縮回

if(p==6&&q==4){

fb1=fb0-1;

if(fb1<0){fb1=0;}

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1=fb1-70; }else{ b1=135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Back...");

lcd.setCursor(13,1);

lcd.print(fb1);

lcd.print(" ");

fb0=fb1;

}

//不動

if(p==6&&q==5){

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1 = fb1-70 ; }else{ b1= 135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(fb0);

lcd.print(" ");

fb0=fb1;}

delay(2);

fb0=a1;

fb1=a1;

}

//////////////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定夾夾

while(p==9){

if(q==1){servoC_write(120);

lcd.setCursor(0,0);

lcd.print("Release...");

}

if(q==2){servoC_write(60);

lcd.setCursor(0,0);

lcd.print("Holding...");

}

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//////////////////////////////////////////////////////////////

if(BT.available()>0){

y = BT.parseInt();

}

if(y!=0){

p=y/10;

q=y%10;

}

//決定前後

while(p==7){

lcd.setCursor(0,0);

lcd.print("Back / Forward 7");

if(BT.available()>0){ y = BT.parseInt();}

if(y!=0){

p=y/10;

q=y%10;

lcd.setCursor(0,0);

lcd.print("Back / Forward 7 ");

}

//伸長

if(p==7&&q==6){

fb1=fb0+1;

if(fb1>170){fb1=170;}

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1=fb1-70; }else{ b1=135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Forward...");

lcd.setCursor(13,1);

lcd.print(fb1);

lcd.print(" ");

fb0=fb1;

}

//縮回

if(p==7&&q==4){

fb1=fb0-1;

if(fb1<0){fb1=0;}

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1=fb1-70; }else{ b1=135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Back...");

lcd.setCursor(13,1);

lcd.print(fb1);

lcd.print(" ");

fb0=fb1;

}

//不動

if(p==7&&q==5){

a1=fb1;

servoA_write(a1);

if(fb1>=100){ b1 = fb1-70 ; }else{ b1= 135-fb1; }

servoB_write(b1);

lcd.setCursor(0,1);

lcd.print("Frezze...");

lcd.setCursor(13,1);

lcd.print(fb0);

lcd.print(" ");

fb0=fb1;}

delay(2);

fb0=a1;

fb1=a1;

}

///////////////////////////////////////////////////////////////////////

}

#include <Wire.h>

#include <HMC5883L.h>

#include <ADXL345.h>

#include <LiquidCrystal_I2C.h>

#include <SoftwareSerial.h>

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

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

SoftwareSerial BT(RX,TX);

#define TrigX 4

#define EchoX 5

#define TrigZ 6

#define EchoZ 7

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

HMC5883L compass;

ADXL345 accelerometer;

float heading0;

float heading1;

float heading2;

float x0,x,x1,x2,z0,z,z1,z2;

int now,old,A,B;

int output,fb,ud;

int Temp=30;

unsigned long t,t1,t2,t3,t4,t5,t6;

double pitch, roll, yaw;

boolean sw,sw1;

//無傾斜補償

float noTiltCompensate(Vector mag){

float heading = atan2(mag.YAxis, mag.XAxis);

return heading;

}

// 傾斜補償

float tiltCompensate(Vector mag, Vector normAccel){

roll = asin(normAccel.YAxis);

pitch = asin(-normAccel.XAxis);

if (roll > 0.78 || roll < -0.78 || pitch > 0.78 || pitch < -0.78){return -1000; }

float cosRoll = cos(roll);

float sinRoll = sin(roll);

float cosPitch = cos(pitch);

float sinPitch = sin(pitch);

float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;

float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

float heading = atan2(Yh, Xh);

return heading;

}

//角度校正

float correctAngle(float heading){

if (heading < 0) { heading += 2 * PI; }

if (heading > 2 * PI) { heading -= 2 * PI; }

return heading;

}

//得到角度

void get_direction(){

// 讀取向量值

Vector mag = compass.readNormalize();

Vector acc = accelerometer.readScaled();

// Calculate headings

heading1 = noTiltCompensate(mag);

heading2 = tiltCompensate(mag, acc);

if (heading2 == -1000){

heading2 = heading1;

}

float declinationAngle = (-4.0 - (4.0 / 60.0)) / (180 / M_PI);

heading1 += declinationAngle;

heading2 += declinationAngle;

heading1 = correctAngle(heading1);

heading2 = correctAngle(heading2);

heading1 = heading1 * 180/M_PI;

heading2 = heading2 * 180/M_PI;

if(heading0>=0&&heading0<90){

if(heading2<180&&heading2>=0){

now = 90 + heading2 - heading0 ;

}else if(heading2<360&&heading2>=270){

now = 90 + heading2 - heading0 - 360 ;

}}

if(heading0>=90&&heading0<180){

if(heading2<270&&heading2>=0){

now = 90 + heading2 - heading0 ;

}}

if(heading0>=180&&heading0<270){

if(heading2<360&&heading2>=90){

now = 90 + heading2 - heading0 ;

}}

if(heading0>=270&&heading0<360){

if(heading2<360&&heading2>=180){

now = 90 + heading2 - heading0 ;

}else if(heading2<90&&heading2>=0){

now = 90 + heading2 - heading0 + 360 ;

}}

now=constrain(now,0,180);

return now,heading2;

}

void get_distX(){

digitalWrite(TrigX, LOW);

delayMicroseconds(2);

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

digitalWrite(TrigX, LOW);

t = pulseIn(EchoX, HIGH,50000);

x1 = (331+0.6*Temp)*t/20000;

if(x1>3&&x1<500){x2=x1;}

}

void get_distZ(){

digitalWrite(TrigZ, LOW);

delayMicroseconds(2);

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

digitalWrite(TrigZ, LOW);

t = pulseIn(EchoZ, HIGH,50000);

z1 = (331+0.6*Temp)*t/20000 ;

if(z1>3&&z1<500){z2=z1;}

}

void lcddisp(){

t2 = millis();

if(t2 >t1+100){

lcd.setCursor(0,0);

lcd.print(x1);

lcd.print(" ");

lcd.setCursor(8,0);

lcd.print(z1);

lcd.print(" ");

lcd.setCursor(0,1);

lcd.print(now);

lcd.print(" ");

t1=t2;

}}

void button(){

sw=0;

lcd.setCursor(8,1);

lcd.print("sw=");

lcd.setCursor(11,1);

lcd.print(sw);

sw1 = analogRead(A0);

sw1=1-sw1;

if(sw1==1){

delay(20);

sw1 = analogRead(A0);

sw1=1-sw1;

if(sw1==1){

sw=1;

lcd.setCursor(11,1);

lcd.print(sw);

}else{

sw=0;

lcd.setCursor(11,1);

lcd.print(sw);

}}}

void setup(){

Serial.begin(38400);

BT.begin(38400);

lcd.init();

lcd.backlight();

lcd.clear();

pinMode(TrigX, OUTPUT);

pinMode(TrigZ, OUTPUT);

if (!accelerometer.begin()){delay(500);} // 初始化 ADXL345

accelerometer.setRange(ADXL345_RANGE_2G);

while (!compass.begin()) { delay(500);} // 初始化電子羅盤 HMC5883L

compass.setRange(HMC5883L_RANGE_1_3GA); //設定測量範圍

compass.setMeasurementMode(HMC5883L_CONTINOUS); // 設定測量模式

compass.setDataRate(HMC5883L_DATARATE_30HZ); // 設定資料傳輸速率

compass.setSamples(HMC5883L_SAMPLES_8); //設樣本平均數的組數

compass.setOffset(0, 0); //設定檢量線的數值

delay(100);

get_direction();

heading0=heading2;

get_direction();

old=now;

}

void loop(){

delay(100);

BT.println(91); //放開

delay(100);

//////////////////////////////////////////////////////////////////////

//先決定左右

int new_state=4;

int old_state=4;

BT.println(40);

do{

lcd.setCursor(0,0);

lcd.print("Left / Right 1");

get_direction();

if(now-old>10){ //往右

new_state=2;

lcd.setCursor(0,1);

lcd.print(16);

if(old_state != 2 && new_state == 2 ){ BT.println(16); }

old_state = new_state;

}else if(abs(now-old)<=10){ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(15);

if(old_state != 0 && new_state == 0 ){

BT.println(15);

}

old_state = new_state;

}else if(old-now>10){ //往左

new_state=1;

lcd.setCursor(0,1);

lcd.print(14);

if(old_state != 1 && new_state == 1 ){

BT.println(14);

}

old_state = new_state;

}

button();

//lcddisp();

delay(10);

}while(sw==0);

///////////////////////////////////////////////////////////////////

delay(100);

BT.println(91); //放開

delay(100);

///////////////////////////////////////////////////////////////////

//決定前後

new_state=4;

old_state=4;

do{

lcd.setCursor(0,0);

lcd.print("Back / Forward 2");

Vector acc = accelerometer.readScaled();

fb = asin(-acc.XAxis) * 180/M_PI;

if(fb>20){ //往前

new_state=2;

lcd.setCursor(0,1);

lcd.print(26);

if(old_state != 2 && new_state == 2 ){ BT.println(26); }

old_state = new_state;

}else if(fb<-20){ //往後

new_state=1;

lcd.setCursor(0,1);

lcd.print(24);

if(old_state != 1 && new_state == 1 ){ BT.println(24); }

old_state = new_state;

}else{ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(25);

if(old_state != 0 && new_state == 0 ){BT.println(25); }

old_state = new_state;}

button();

delay(10);

}while(sw==0);

///////////////////////////////////////////////////////////////////////

delay(100);

BT.println(92); //夾緊

delay(100);

////////////////////////////////////////////////////////////////////////

//決定高低

new_state=4;

old_state=4;

do{

lcd.setCursor(0,0);

lcd.print("Up / Down 3");

Vector acc = accelerometer.readScaled();

ud = asin(acc.XAxis) * 180/M_PI;

if(ud>20){ //往上

new_state=2;

lcd.setCursor(0,1);

lcd.print(36);

if(old_state != 2 && new_state == 2 ){ BT.println(36); }

old_state = new_state;

}else if(ud<-20){ //往下

new_state=1;

lcd.setCursor(0,1);

lcd.print(34);

if(old_state != 1 && new_state == 1 ){ BT.println(34); }

old_state = new_state;

}else{ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(35);

if(old_state != 0 && new_state == 0 ){BT.println(35); }

old_state = new_state;}

button();

delay(10);

}while(sw==0);

//////////////////////////////////////////////////////////////////////////////

delay(100);

BT.println(92); //夾緊

delay(100);

///////////////////////////////////////////////////////////////////////////////////

//先決定左右

new_state=4;

old_state=4;

BT.println(40);

do{

lcd.setCursor(0,0);

lcd.print("Left or Right 4");

get_direction();

if(now-old>10){ //往右

new_state=2;

lcd.setCursor(0,1);

lcd.print(46);

if(old_state != 2 && new_state == 2 ){ BT.println(46); }

old_state = new_state;

}else if(abs(now-old)<=10){ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(45);

if(old_state != 0 && new_state == 0 ){

BT.println(45);

}

old_state = new_state;

}else if(old-now>10){ //往左

new_state=1;

lcd.setCursor(0,1);

lcd.print(44);

if(old_state != 1 && new_state == 1 ){

BT.println(44);

}

old_state = new_state;

}

button();

//lcddisp();

delay(10);

}while(sw==0);

//////////////////////////////////////////////////////////////////////////////

delay(100);

BT.println(92); //夾緊

delay(100);

///////////////////////////////////////////////////////////////////////////////////

//決定高低

new_state=4;

old_state=4;

do{

lcd.setCursor(0,0);

lcd.print("Up / Down 5");

Vector acc = accelerometer.readScaled();

ud = asin(acc.XAxis) * 180/M_PI;

if(ud>20){ //往上

new_state=2;

lcd.setCursor(0,1);

lcd.print(56);

if(old_state != 2 && new_state == 2 ){ BT.println(56); }

old_state = new_state;

}else if(ud<-20){ //往下

new_state=1;

lcd.setCursor(0,1);

lcd.print(54);

if(old_state != 1 && new_state == 1 ){ BT.println(54); }

old_state = new_state;

}else{ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(55);

if(old_state != 0 && new_state == 0 ){BT.println(55); }

old_state = new_state;}

button();

delay(10);

}while(sw==0);

//////////////////////////////////////////////////////////////////////////////////

delay(100);

BT.println(92); //夾緊

delay(100);

/////////////////////////////////////////////////////////////////////////////////

//決定前後

new_state=4;

old_state=4;

do{

lcd.print("Back / Forward 6");

Vector acc = accelerometer.readScaled();

fb = asin(-acc.XAxis) * 180/M_PI;

if(fb>20){ //往前

new_state=2;

lcd.setCursor(0,1);

lcd.print(66);

if(old_state != 2 && new_state == 2 ){ BT.println(66); }

old_state = new_state;

}else if(fb<-20){ //往後

new_state=1;

lcd.setCursor(0,1);

lcd.print(64);

if(old_state != 1 && new_state == 1 ){ BT.println(64); }

old_state = new_state;

}else{ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(65);

if(old_state != 0 && new_state == 0 ){BT.println(65); }

old_state = new_state;}

button();

delay(10);

}while(sw==0);

///////////////////////////////////////////////////////////////////////

delay(100);

BT.println(91); //放開

delay(100);

//////////////////////////////////////////////////////////////////////

//決定前後

new_state=4;

old_state=4;

do{

lcd.print("Back / Forward 7");

Vector acc = accelerometer.readScaled();

fb = asin(-acc.XAxis) * 180/M_PI;

if(fb>20){ //往前

new_state=2;

lcd.setCursor(0,1);

lcd.print(76);

if(old_state != 2 && new_state == 2 ){ BT.println(76); }

old_state = new_state;

}else if(fb<-20){ //往後

new_state=1;

lcd.setCursor(0,1);

lcd.print(74);

if(old_state != 1 && new_state == 1 ){ BT.println(74); }

old_state = new_state;

}else{ //不動

new_state=0;

lcd.setCursor(0,1);

lcd.print(75);

if(old_state != 0 && new_state == 0 ){BT.println(75); }

old_state = new_state;}

button();

delay(10);

}while(sw==0);

}