#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
LiquidCrystal_I2C lcd(0x3F,16,2); // 若LCD 無法啟用，請將0X3F 改為0X27
unsigned long x0,x1,x2,x3;
int a1,a2,b1,b2;
int limit=50;
int t=50;
int n=100;
Servo servox,servoy;
void dispLcd(){
lcd.setCursor(0,0);
lcd.print(x1);
lcd.print(" ");
lcd.setCursor(6,0);
lcd.print("(");
lcd.print(a2);
lcd.print(")");
lcd.print(" ");
lcd.setCursor(12,0);
lcd.print(x2);
lcd.print(" ");
lcd.setCursor(0,1);
lcd.print(x0);
lcd.print(" ");
lcd.setCursor(6,1);
lcd.print("(");
lcd.print(b2);
lcd.print(")");
lcd.setCursor(12,1);
lcd.print(x3);
lcd.print(" ");
}
void detect(){
for(int i=0 ; i<n ;i++){
x0 += analogRead(A0);
x1 += analogRead(A1);
x2 += analogRead(A2);
x3 += analogRead(A3);
}
x0=x0/n;
x1=x1/n;
x2=x2/n;
x3=x3/n;
dispLcd();
}
void work(){
//////////////////////////////////////////
detect();
while(x0+x1>x2+x3+limit){
if(b2>=90){
a2=a1+1;
if(a2>180){a2=180;}
}else{
a2=a1-1;
if(a2<0){
a2=0;
}}
if(a2==180)break;
if(a2==0)break;
servox.write(a2);
Serial.print("x=");
Serial.println(a2);
a1=a2;
delay(t);
detect();
}
/////////////////////////////////////////
detect();
while(x2+x3>x0+x1+limit){
if(b2>=90){
a2=a1-1;
if(a2<0){
a2=0;
}}else{
a2=a1+1;
if(a2>180){
a2=180;
}}
if(a2==180)break;
if(a2==0)break;
servox.write(a2);
Serial.print("x=");
Serial.println(a2);
a1=a2;
delay(t);
detect();
}
/////////////////////////////////////////////////
detect();
while(x1+x2>x0+x3+limit){
b2=b1+1;
if(b2>180){
b2=180;
}
if(b2==180)break;
if(b2==0)break;
servoy.write(b2);
Serial.print("y=");
Serial.println(b2);
b1=b2;
delay(t);
detect();
}
//////////////////////////////////////////////////
detect();
while(x0+x3>x1+x2+limit){
b2=b1-1;
if(b2<0){
b2=0;
}
if(b2==180)break;
if(b2==0)break;
servoy.write(b2);
Serial.print("y=");
Serial.println(b2);
b1=b2;
delay(t);
detect();
}
}
void setup() {
Serial.begin(9600);
lcd.init();
lcd.backlight();
lcd.clear();
servox.attach(8);
servoy.attach(11);
servox.write(90);
servoy.write(90);
a1=90;
b1=90;
a2=90;
b2=90;
}
void loop() {
work();
}
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
LiquidCrystal_I2C lcd(0x3F,16,2); // 若LCD 無法啟用，請將0X3F 改為0X27
unsigned long x0,x1,x2,x3;
int a1,a2,b1,b2,a3,a4;
int limit=50;
int t=1;
int n=100;
int mode;
Servo servox,servoy;
void debug(){
Serial.print(x0+x1);
Serial.print(",");
Serial.print(x2+x3);
Serial.print(",");
Serial.println(a2);
}
void dispLcd(){
lcd.setCursor(0,0);
lcd.print(x1);
lcd.print(" ");
lcd.setCursor(6,0);
lcd.print("(");
lcd.print(a2);
lcd.print(")");
lcd.print(" ");
lcd.setCursor(12,0);
lcd.print(x2);
lcd.print(" ");
lcd.setCursor(0,1);
lcd.print(x0);
lcd.print(" ");
lcd.setCursor(6,1);
lcd.print("(");
lcd.print(b2);
lcd.print(")");
lcd.setCursor(12,1);
lcd.print(x3);
lcd.print(" ");
//debug();
}
void detect(){
for(int i=0 ; i<n ;i++){
x0 += analogRead(A0);
x1 += analogRead(A1);
x2 += analogRead(A2);
x3 += analogRead(A3);
}
x0=x0/n;
x1=x1/n;
x2=x2/n;
x3=x3/n;
dispLcd();
}
void work(){
//////////////////////////////////////////
detect();
while(x0+x1>x2+x3+limit&&mode==0){
if(b2>=90){
a2=a1+1;
if(a2>180){a2=180;}
}else{
a2=a1-1;
if(a2<0){
a2=0;
}}
if(a2==180)break;
if(a2==0)break;
servox.write(a2);
a1=a2;
delay(t);
detect();
}
a3=a2;
/////////////////////////////////////////
detect();
while(x2+x3>x0+x1+limit&&mode==0){
if(b2>=90){
a2=a1-1;
if(a2<0){
a2=0;
}}else{
a2=a1+1;
if(a2>180){
a2=180;
}}
if(a2==180)break;
if(a2==0)break;
servox.write(a2);
a1=a2;
delay(t);
detect();
}
a4=a2;
mode++;
servox.write((a3+a4)/2);
if(x2+x3>x0+x1+limit+100){mode=0;}
if(x0+x1>x2+x3+limit+100){mode=0;}
/////////////////////////////////////////////////
detect();
while(x1+x2>x0+x3+limit){
b2=b1+1;
if(b2>180){
b2=180;
}
if(b2==180)break;
if(b2==0)break;
servoy.write(b2);
b1=b2;
delay(t);
detect();
}
//////////////////////////////////////////////////
detect();
while(x0+x3>x1+x2+limit){
b2=b1-1;
if(b2<0){
b2=0;
}
if(b2==180)break;
if(b2==0)break;
servoy.write(b2);
b1=b2;
delay(t);
detect();
}
}
void setup() {
Serial.begin(9600);
lcd.init();
lcd.backlight();
lcd.clear();
servox.attach(8);
servoy.attach(11);
servox.write(90);
servoy.write(90);
a1=90;
b1=90;
a2=90;
b2=90;
}
void loop() {
work();
}
#include <Stepper.h>
#include <Servo.h>
#include <Wire.h> //載入I2C介面函式庫
#include <LiquidCrystal_I2C.h> //載入I2C液晶控制函式庫
LiquidCrystal_I2C lcd(0x27,16,2); // 若LCD 無法啟用，請將0X3F 改為0X27
const int stepsPerRevolution = 1024;
Servo servo;
Stepper myStepper(stepsPerRevolution, 4, 6, 7, 5);
/*
pin腳接法
IN1=4
IN2=6
IN3=7
IN4=5
*/
int x,x1,y,y1,degX,degY;
int UL,UR,DL,DR;
int w=5; //容忍值
int mode=1;
void setup() {
lcd.init(); //LCD初始化
lcd.backlight(); //LCD 開啟背光
lcd.clear(); // LCD清除畫面
pinMode(12,INPUT);
pinMode(13,OUTPUT);
servo.attach(8);
myStepper.setSpeed(5);
lcd.setCursor(0,0); //設定LCD游標至第0行，第0列的位置
lcd.print("Stepper_X");
lcd.setCursor(0,1); //設定LCD游標至第0行，第0列的位置
lcd.print("initial...");
do{
myStepper.step(4);
x=digitalRead(12);
}while(x==1);
lcd.setCursor(0,0); //設定LCD游標至第0行，第0列的位置
lcd.print("Servo_Y");
lcd.setCursor(0,1); //設定LCD游標至第0行，第0列的位置
lcd.print("initial...");
servo.write(90);
y=90;
lcd.setCursor(0,0); //設定LCD游標至第0行，第0列的位置
lcd.print("Stepper_X");
lcd.setCursor(0,1); //設定LCD游標至第0行，第0列的位置
lcd.print("setting...");
myStepper.step(-512);
lcd.clear();
}
void LCD_Display(){
lcd.setCursor(0,0); //設定LCD游標至第0行，第0列的位置
lcd.print(UL); //LCD在現在的游標顯示變數a的值
if(UL<100){
lcd.print(" ");
}
lcd.setCursor(13,0); //設定LCD游標至第8行，第0列的位置
lcd.print(UR); //LCD在現在的游標顯示變數c的值
if(UR<100){
lcd.print(" ");
}
lcd.setCursor(0,1); //設定LCD游標至第0行，第1列的位置
lcd.print(DL); //LCD在現在的游標顯示變數b的值
if(DL<100){
lcd.print(" ");
}
lcd.setCursor(13,1); //設定LCD游標至第8行，第1列的位置
lcd.print(DR); //LCD在現在的游標顯示變數d的值
if(DR<100){
lcd.print(" ");
}}
void loop() {
while(mode==1){
DL=analogRead(A1);
DR=analogRead(A3);
UL=analogRead(A0);
UR=analogRead(A2);
if(UL>UR+w && UL>DR+w && UL>DL+w){ //左上最亮
myStepper.step(4);
}
if(DL>UR+w && DL>DR+w && DL>UL+w){ //左下最亮
myStepper.step(4);
}
if(UR>UL+w && UR>DR+w && UR>DL+w){ //右上最亮
myStepper.step(-4);
}
if(DR>UL+w && DR>DL+w && DR>UR+w){ //右下最亮
myStepper.step(-4);
}
LCD_Display();
if(abs(UL-UR)<w && UL>DL && UR>DR){
mode=2;
}
if(abs(DL-DR)<w && DL>UL && DR>UR){
mode=2;
}
}
while(mode==2){
digitalWrite(13,1);
DL=analogRead(A1);
DR=analogRead(A3);
UL=analogRead(A0);
UR=analogRead(A2);
if(UL>UR+w && UL>DR+w && UL>DL+w){ //左上最亮
y=y+1;
if(y>130){y=130; }
servo.write(y);
}
if(UR>UL+w && UR>DR+w && UR>DL+w){ //右上最亮
y=y+1;
if(y>130){y=130; }
servo.write(y);
}
if(DL>UR+w && DL>DR+w && DL>UL+w){ //左下最亮
y=y-1;
if(y<5){y=5; }
servo.write(y);
}
if(DR>UL+w && DR>DL+w && DR>UR+w){ //右下最亮
y=y-1;
if(y<5){y=5; }
servo.write(y);
}
LCD_Display();
if(abs(UL-DL)<w && UL>UR && DL>DR){
mode=1;
}
if(abs(UR-DR)<w && UR>UL && DR>DL){
mode=1;
}
}
digitalWrite(13,0);
}