#include <Servo.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,20,4);
Servo myservo[4];
#define Vert A1
#define Horz A0
#define BtnA 13
#define BtnB 12
#define BtnC 11
long WhenClick=0;
bool ispress=false;
int pinServo[]={3,5,6,9};
int angleNow[]={0,0,0,0};
int angleSaveA[]={0,0,0,0};
int angleSaveB[]={0,0,0,0};
bool BtnCClick=false;
void setup() {
lcd.init();
lcd.backlight();
lcd.print("asd");
for(int i=0;i<4;i++){
myservo[i].attach(pinServo[i]);
myservo[i].write(0);
}
pinMode(Vert, INPUT);
pinMode(Horz, INPUT);
pinMode(BtnA, INPUT_PULLUP);
pinMode(BtnB, INPUT_PULLUP);
pinMode(BtnC, INPUT_PULLUP);
}
bool issame(int A[],int B[]){
if(A[0]==B[0] && A[1]==B[1] && A[2]==B[2] && A[3]==B[3])
return true;
return false;
}
void loop() {
int TrucX=analogRead(Vert);
int TrucY=analogRead(Horz);
lcd.clear();
lcd.print("X:");
lcd.print(TrucX);
lcd.print(" ");
lcd.print("Y:");
lcd.print(TrucY);
//Tọa độ X tăng điều khiển Servo 1,2
if(TrucX>600){
angleNow[0]+=1;
angleNow[1]+=1;
if(angleNow[0]>180)
angleNow[0]=180;
if(angleNow[1]>180)
angleNow[1]=180;
}
//Tọa độ X giảm điều khiển Servo 1,2
if(TrucX<400){
angleNow[0]-=1;
angleNow[1]-=1;
if(angleNow[0]<0)
angleNow[0]=0;
if(angleNow[1]<0)
angleNow[1]=0;
}
//Tọa độ Y tăng điều khiển Servo 3,4
if(TrucY>600){
angleNow[2]+=1;
angleNow[3]+=1;
if(angleNow[2]>180)
angleNow[2]=180;
if(angleNow[3]>180)
angleNow[3]=180;
}
//Tọa độ Y giảm điều khiển Servo 3,4
if(TrucY<400){
angleNow[2]-=1;
angleNow[3]-=1;
if(angleNow[2]<0)
angleNow[2]=0;
if(angleNow[3]<0)
angleNow[3]=0;
}
//Lưu tọa độ khi bấm nút A
if(!digitalRead(BtnA)){
for(int i=0;i<4;i++){
angleSaveA[i]=angleNow[i];
}
}
//Lưu tọa độ khi bấm nút B
if(!digitalRead(BtnB)){
for(int i=0;i<4;i++){
angleSaveB[i]=angleNow[i];
}
}
//Nếu nút C đã được nhấn mà không nhấn giữ thì tiến
//hành di chuyển các Servo từ tọa độ A đến B đã lưu
if(BtnCClick){
for(int i=0;i<4;i++){
if(angleNow[i]<angleSaveB[i]){
angleNow[i]+=1;
}if(angleNow[i]>angleSaveB[i]){
angleNow[i]-=1;
}if(issame(angleNow,angleSaveB))
BtnCClick=false;
}
}
//Nếu nút C được nhấn
if(!digitalRead(BtnC)){
for(int i=0;i<4;i++){
angleNow[i]=angleSaveA[i];
}
BtnCClick=true;
//Kiểm tra có phải nhấn giữ 3s không
// nếu phải reset về 0
if(!ispress){
WhenClick=millis();
ispress=true;
}else{
if(millis()-WhenClick>=3000){
for(int i=0;i<4;i++){
angleNow[i]=0;
myservo[i].write(angleNow[i]);
BtnCClick=false;
}
}
}
}else{
ispress=false;
}
//Cho Servo quay đến góc hiện tại
for(int i=0;i<4;i++){
myservo[i].write(angleNow[i]);
}
delay(50);
}