兩軸橡皮筋砲台

#include<Servo.h>

#include<Wire.h>

#include<LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x3f,16,2);

Servo servo1,servo2,servo3;

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

int angle_lunch=180; //設定觸發時servo的角度

int angle_ready=120; //設定預備時servo的角度

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

int rl0=90 , ud0=90 ;

int rl1,ud1,x,y;

long x_avg,x_set;

long y_avg,y_set;

unsigned long t1,t2;

void lcd_disp(){

t2=millis();

if(t2>t1+50){

lcd.clear();

lcd.setCursor(0,0);

lcd.print(rl1);

lcd.print("(");

if(x>=0){

lcd.print("+");

}

lcd.print(x);

lcd.print(")");

lcd.setCursor(8,0);

lcd.print(ud1);

lcd.print("(");

if(y>=0){

lcd.print("+");

}

lcd.print(y);

lcd.print(")");

t1=t2;

}

}

void setup(){

Serial.begin(9600);

servo1.attach(8);

servo2.attach(9);

servo3.attach(10);

pinMode(16, INPUT_PULLUP);

lcd.init();

lcd.backlight();

lcd.clear();

lcd.setCursor(0,0);

lcd.print("Servo in D8");

lcd.setCursor(0,1);

lcd.print("Don't move control");

delay(1000);

for(int i=0 ; i<50 ; i++){ //這一段程式碼是對香菇頭的讀數做歸零調整，取平均

x_set += analogRead(A0);

y_set += analogRead(A1);

}

x_set=x_set/50;

y_set=y_set/50;

t1=millis();

}

void loop() {

for(int i=0 ; i<50 ; i++){ //這一段程式碼是對香菇頭的讀數變化取平均，作濾波，可以濾掉不穩定的變動值

x_avg += analogRead(A0);

y_avg += analogRead(A1);

}

x_avg=x_avg/50;

y_avg=y_avg/50;

x =(x_set-x_avg)/100;

y =(y_set-y_avg)/100;

rl1=rl0+x;

if(rl1>180)

{

rl1=180;

}

if(rl1<0)

{

rl1=0;

}

servo1.write(180-rl1);

rl0=rl1;

ud1=ud0+y;

if(ud1>180)

{

ud1=180;

}

if(ud1<0)

{

ud1=0;

}

servo2.write(180-ud1);

ud0=ud1;

if(digitalRead(16)==0)

{

servo3.write(angle_lunch);

lcd.setCursor(0,1);

lcd.print("Lunch!!");

delay(200);

servo3.write(angle_ready);

lcd.setCursor(0,1);

lcd.print("ready...");

delay(300);

}

else

{

servo3.write(angle_ready);

lcd.setCursor(0,1);

lcd.print("ready...");

}

lcd_disp();

delay(5);

}