babcia_v1.0

const int czujodl= A4; //konf. czujnika odleglosci
const int pinkier1= 4; //pin kierunku lewego
const int pinpwm1 = 5; //pin predkosci lewej
const int pinkier2= 7; //pin kierunku prawego
const int pinpwm2 = 6; // pin predkosci prawej
const int przyc= 2; //pin przycisku dotykowego

//konfriguracja czujnikow odbicia
const int l2 = A5;
const int l1 = A0;
const int sr = A1;
const int p1 = A2;
const int p2 = A3;

//zmienne wartosci czujnikow odbicia
int ol2 = 0;
int ol1 = 0;
int osr = 0;
int op1 = 0;
int op2 = 0;

double k = 0.6; //mnoznik (regulator P)
double p = 35.0; //liczba pomocnicza

int odl= 0; //zmienna odległosci
int pwm1=0; //zmienna wartości pwm lewego koła
int pwm2=0; //zmienna wartosci pwm prawego kola
boolean onoff = 0; //stan wlaczenia/wylaczenia
void setup() {

pinMode(pinkier1, OUTPUT);
pinMode(pinpwm1, OUTPUT);
pinMode(pinkier2, OUTPUT);
pinMode(pinpwm2, OUTPUT);
pinMode(przyc, INPUT);
Serial.begin(9600);
}

void loop(){

//odczyt wskazań czujnikow odbicia

  ol2 = analogRead(l2);
   ol1 = analogRead(l1);
    osr = analogRead(sr);
     op1 = analogRead(p1);
      op2 = analogRead(p2);

      Serial.println(" ");

//wypisywanie wskazan na monitor
      Serial.println(ol2);
      Serial.println(ol1);
      Serial.println(osr);
      Serial.println(op1);
      Serial.println(op2);
      Serial.println(" ");

//konfiguracja wlaczenia i wylaczenia robota
if(digitalRead(przyc)==LOW && onoff == 0)
{ onoff = 1;
//zabezpieczenie przed wielokrotną zmianą stanu
  while(digitalRead(przyc) == LOW)
  {delay(100);
  }
}
if(digitalRead(przyc)==LOW && onoff == 1)
{ onoff = 0;

 //wylaczenie silnikow
digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, 0);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, 0);

//zabezpieczenie przed wielokrotną zmianą stanu
  while(digitalRead(przyc) == LOW)
  {delay(100);
  }
}

 if(onoff == 1){

 //9 możliwych warunków na dzialania silnikow w zaleznosci od odczytow z czujnikow
 if(ol2<500 && ol1<500 && osr>500 && op1<500 && op2<500) // czujnik sr
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, 220*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, 220*k);
 Serial.println("sr");
 }

   if(ol2<500 && ol1>500 && osr<500 && op1<500 && op2<500) // czujnik l1 i sr
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, (220-1*p)*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, 220*k);
  Serial.println("l1");
 }

  if(ol2<500 && ol1>500 && osr<500 && op1<500 && op2<500) // czujnik l1
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, (220-2*p)*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, 220*k);
  Serial.println("l1");
 }

 if(ol2>500 && ol1>500 && osr<500 && op1<500 && op2<500) // czujnik l1 i l2
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, (220-4*p)*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, 220*k);
  Serial.println("l1");
 }

 if(ol2>500 && ol1<500 && osr<500 && op1<500 && op2<500) // czujnik l2
 {
 digitalWrite(pinkier1,LOW);
 analogWrite(pinpwm1, (220-2*p)*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, (220-2*p)*k);
  Serial.println("l2");
 }

 if(ol2<500 && ol1<500 && osr>500 && op1>500 && op2<500) // czujnik p1 i sr
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, 220*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, (220-1*p)*k);
  Serial.println("p1");
 }

  if(ol2<500 && ol1<500 && osr<500 && op1>500 && op2<500) // czujnik p1
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, 220*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, (220-2*p)*k);
  Serial.println("p1");
 }

 if(ol2<500 && ol1<500 && osr<500 && op1>500 && op2>500) // czujnik p1 i p2
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, 220*k);
 digitalWrite(pinkier2,LOW);
 analogWrite(pinpwm2, (220-4*p)*k);
  Serial.println("p1");
 }

  if(ol2<500 && ol1<500 && osr<500 && op1<500 && op2>500) // czujnik p2
 {
 digitalWrite(pinkier1,HIGH);
 analogWrite(pinpwm1, (220-2*p)*k);
 digitalWrite(pinkier2,HIGH);
 analogWrite(pinpwm2, (220-2*p)*k);
  Serial.println("p2");
 }

 }
}