Kod:
#include <Servo.h>
#include<SoftwareSerial.h>
#include<math.h>
#include <PCF8574.h>
#include <Wire.h>
#include<LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x20,16,2);
PCF8574 expander;
Servo myservo;
int odleglosc;
int pozycja = 130;
int b;
void setup() {
Serial.begin(9600);
expander.begin(0x20);
lcd.begin(16, 2); //Deklaracja typu
lcd.setCursor(0, 0); //Ustawienie kursora
lcd.print("Start"); //Wyświetlenie tekstu
lcd.setCursor(0, 1); //Ustawienie kursora
lcd.print("Start"); //Wyświetlenie tekstu
// put your setup code here, to run once:
pinMode(2,OUTPUT);
pinMode(3,OUTPUT);
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
pinMode(13,OUTPUT);
digitalWrite(2,LOW);
digitalWrite(3,LOW);
digitalWrite(4,LOW);
digitalWrite(13,LOW);
pinMode(A0,OUTPUT);
pinMode(12,OUTPUT);
pinMode(7,INPUT);
pinMode(10,INPUT);
analogWrite(6,150);
analogWrite(5,150);
myservo.attach(11);
Serial.begin(9600);
myservo.write(130);
}
void loop() {
// put your main code here, to run repeatedly:
odleglosc = pomiar2();
if(odleglosc<30){
zatrzymaj();
delay(100);
b=kierunek();
delay(100);
tyl();
delay(400);
zatrzymaj();
delay(100);
if(b==1) {
lewo();
delay(1500);
analogWrite(6,150);
analogWrite(5,150);
}
if(b==2){
prawo();
delay(1500);
analogWrite(6,150);
analogWrite(5,150);
}
b=0;
}
else przod();
delay(100);
}
int pomiar1() {
long czas,dystans;
digitalWrite(A0, LOW);
delayMicroseconds(2);
digitalWrite(A0, HIGH);
delayMicroseconds(10);
digitalWrite(A0, LOW);
czas = pulseIn(7, HIGH);
dystans = czas / 58;
return dystans;
}
int pomiar2() {
long czas,dystans;
digitalWrite(12, LOW);
delayMicroseconds(2);
digitalWrite(12, HIGH);
delayMicroseconds(10);
digitalWrite(12, LOW);
czas = pulseIn(10, HIGH);
dystans = czas / 58;
return dystans;
}
void przod(){
digitalWrite(2,HIGH);
digitalWrite(3,LOW);
digitalWrite(4,HIGH);
digitalWrite(13,LOW);
}
void tyl(){
digitalWrite(2,LOW);
digitalWrite(3,HIGH);
digitalWrite(4,LOW);
digitalWrite(13,HIGH);
}
void prawo(){
analogWrite(6,220);
analogWrite(5,220);
digitalWrite(2,HIGH);
digitalWrite(3,LOW);
digitalWrite(4,LOW);
digitalWrite(13,HIGH);
}
void lewo(){
analogWrite(6,220);
analogWrite(5,220);
digitalWrite(2,LOW);
digitalWrite(3,HIGH);
digitalWrite(4,HIGH);
digitalWrite(13,LOW);
}
void zatrzymaj(){
digitalWrite(2,LOW);
digitalWrite(3,LOW);
digitalWrite(4,LOW);
digitalWrite(13,LOW);
}
int kierunek (){
double szerokosc;
double dystans;
double alfa=0;
double beta=0;
double alfaPI;
double betaPI;
double gammaPI;
double x=0;
double y=0;
int a;
int lewy,prawy;
int odleglosc2, odleglosc3;
odleglosc2 = pomiar1();
while(odleglosc2<40 && pozycja<180){
myservo.write(pozycja);
pozycja++;
delay(50);
odleglosc2 = pomiar1();
alfa++;
Serial.print("alfa : ");
Serial.print(alfa);
Serial.println();
delay(100);
}
delay(300);
lewy = pomiar1();
delay(200);
Serial.print("lewy : ");
Serial.print(lewy);
Serial.println();
pozycja = 130;
myservo.write(pozycja);
delay(1500);
odleglosc3 = pomiar1();
Serial.print("odleglosc3 : ");
Serial.print(odleglosc3);
Serial.println();
while(odleglosc3<40 && pozycja>80){
myservo.write(pozycja);
pozycja--;
delay(50);
odleglosc3 = pomiar1();
beta++;
Serial.print("beta : ");
Serial.print(beta);
Serial.println();
delay(100);
}
delay(300);
prawy = pomiar1();
delay(200);
Serial.print("prawy : ");
Serial.print(prawy);
Serial.println();
pozycja =130;
myservo.write(pozycja);
delay(1500);
dystans = pomiar2();
Serial.print("dystans : ");
Serial.print(dystans);
Serial.println();
delay(200);
Serial.print("alfa : ");
Serial.print(alfa);
Serial.println();
Serial.print("beta : ");
Serial.print(beta);
Serial.println();
alfaPI = alfa/180;
Serial.print("alfaPI : ");
Serial.print(alfaPI);
Serial.println();
betaPI = beta/180;
Serial.print("betaPI : ");
Serial.print(betaPI);
Serial.println();
gammaPI=alfaPI+betaPI;
Serial.print("gammaPI : ");
Serial.print(gammaPI);
Serial.println();
x=dystans/cos(alfaPI);
Serial.print("x : ");
Serial.print(x);
Serial.println();
y=dystans/cos(alfaPI);
Serial.print("y : ");
Serial.print(y);
Serial.println();
szerokosc = x*x+y*y - 2*x*y*cos(gammaPI);
Serial.print("szerokosc : ");
Serial.print(szerokosc);
Serial.println();
lcd.clear();
lcd.setCursor(0, 0); //Ustawienie kursora
lcd.print("Szerokosc: "); //Wyświetlenie tekstu
lcd.print(szerokosc);
if(lewy>prawy) a=1;
else a=2;
delay(100);
Serial.print("a : ");
Serial.print(a);
Serial.println();
delay(100);
return a;
}Aż po oczach daje taki post jeszcze raz bez "code" i będzie nagroda