Mój pierwszy robot Arduino

Kod źródłowy:

//ROBOT jeżdżący
//LCD I2C; 2x silniki, sonarr HC-SR04, tsop4836 RC5
//Piny:
/*
12: Silnik prawy kierunek;
11: Silnik prawy PWM;
4: Silnik lewy kierunek;
5: Silnik lewy PWM;
8: Sonar Echo;
9: Sonar trig;
10: TSOP4836;
SDA/SCL:  LCD 16x2;

*/
#include <Wire.h>  // Comes with Arduino IDE
#include <FastIO.h>
#include <I2CIO.h>
#include <LCD.h>
#include <LiquidCrystal_I2C.h>
#include <LiquidCrystal_SR.h>
#include <LiquidCrystal_SR2W.h>
#include <LiquidCrystal_SR3W.h>



LiquidCrystal_I2C lcd(0x20, 4, 5, 6, 0, 1, 2, 3, 7, POSITIVE);  // Set the LCD I2C address
const int echo = 8;
const int trig = 9;
long duration, cm;

void setup(){
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(8, INPUT);
  pinMode(9, OUTPUT);
  pinMode(10, INPUT);

  analogWrite(6,0);
  analogWrite(5,0);
  digitalWrite(7,LOW);
  digitalWrite(4,LOW);
  digitalWrite(9,LOW);
  
  lcd.begin(16,2);         // initialize the lcd for 20 chars 4 lines]
  lcd.setBacklightPin(7, NEGATIVE);
  lcd.setBacklight(0);
// NOTE: Cursor Position: CHAR, LINE) start at 0  
  lcd.setCursor(0,0); //Start at character 4 on line 0
  lcd.print("ROBOT! 4");


}
void loop(){
int wynik;
lcd.setCursor(0,1);
lcd.print("L:           ");
lcd.setCursor(3,1);
wynik = pomiar();
if(wynik<3000){
  lcd.print(wynik);
  
    if(wynik<30)
    {
      digitalWrite(7,LOW);
      digitalWrite(4,LOW);
      analogWrite(6,255);
      analogWrite(5,255);
    }
    if((wynik>30) && (wynik<80))
    {
      digitalWrite(7,LOW);
      digitalWrite(4,LOW);
      analogWrite(6,0);
      analogWrite(5,0);
    }

    if(wynik>50)
    {
      digitalWrite(7,HIGH);
      digitalWrite(4,HIGH);
      analogWrite(6,0);
      analogWrite(5,0);
    }


}
delay(10);


}
int pomiar(void)
{
  pinMode(trig, OUTPUT);
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
   
  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echo, INPUT);
  duration = pulseIn(echo, HIGH);
  return microsecondsToCentimeters(duration);
}

int microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}