cvratovic_projekt

int NN;
int LD;

int NaprijedD = 3;
int NazadD = 5;
const int xos = A0; // x os
const int yos = A3; // y os

const int NaprijedL = 6;
const int NazadL = 9;

int SrednjaVrijednostNN = 0;
int SrednjaVrijednostLD = 0;
int SrednjaVrijednostL = 0;
int SrednjaVrijednostD = 0;

int maxSpeed = 180;
int skretanje = 100;

const int numReadings = 20;
const int numReadings1 = 20;

int readings[numReadings];
int readIndex = 0;
int total = 0;
int average = 0;

int readings1[numReadings1];
int readIndex1 = 0;
int total1 = 0;
int average1 = 0;


void setup() {

  pinMode(NaprijedD, OUTPUT);
  pinMode(NazadD, OUTPUT);
  pinMode(NaprijedL, OUTPUT);
  pinMode(NazadL, OUTPUT);

  //Serial.begin(9600);
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    readings[thisReading] = 0;
  }
  for (int thisReading1 = 0; thisReading1 < numReadings1; thisReading1++) {
    readings1[thisReading1] = 0;
  }
}

void loop() {

  NN = analogRead(xos);
  LD = analogRead(yos);

  SrednjaVrijednostNN = gladenje(NN);
  SrednjaVrijednostLD = gladenje1(LD);

  // IspisNN();
  // IspisLD();
  SkalirajNN();
  SkalirajLD();

  Kretanje();
}

void SkalirajNN() {
  if (SrednjaVrijednostNN >= 360 && SrednjaVrijednostNN < 375) {
    SrednjaVrijednostNN = 0;
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostNN);

  } else if (SrednjaVrijednostNN >= 375 && SrednjaVrijednostNN < 530) {
    SrednjaVrijednostNN = map(SrednjaVrijednostNN, 375, 530, 0, maxSpeed * 1.25);
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostNN);

  } else if (SrednjaVrijednostNN >= 530 ) {
    SrednjaVrijednostNN = maxSpeed * 1.25;
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostNN);

  } else if (SrednjaVrijednostNN >= 250 && SrednjaVrijednostNN < 360) {
    SrednjaVrijednostNN = map(SrednjaVrijednostNN, 250, 360, maxSpeed * (-1), 0);
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostNN);

  } else if (SrednjaVrijednostNN < 250 ) {
    SrednjaVrijednostNN = maxSpeed * (-1);
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostNN);

  } else Serial.println();
}

void SkalirajLD() {
  if (SrednjaVrijednostLD >= 380 && SrednjaVrijednostLD < 420) {
    SrednjaVrijednostLD = 0;
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostL);

  } else if (SrednjaVrijednostLD >= 420 && SrednjaVrijednostLD < 510) {
    SrednjaVrijednostLD = map(SrednjaVrijednostLD, 420, 510, 0, skretanje);
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostL);

  } else if (SrednjaVrijednostLD >= 510 ) {
    SrednjaVrijednostLD = skretanje;
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostL);

  } else if (SrednjaVrijednostLD >= 220 && SrednjaVrijednostLD < 380) {
    SrednjaVrijednostLD = map(SrednjaVrijednostLD, 220, 380, skretanje * (-1), 0);
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostD);

  } else if (SrednjaVrijednostLD < 220 ) {
    SrednjaVrijednostLD = skretanje * (-1);
    Serial.print( "   Nakon " );
    Serial.println(SrednjaVrijednostD);

  } else Serial.println();
}

void Kretanje() {

  if (SrednjaVrijednostNN >= 0) {
    int  calc1 = SrednjaVrijednostNN + SrednjaVrijednostLD;
    if (calc1 < 0) {
      calc1 = 0;
    }
    analogWrite(NaprijedL, calc1);
    analogWrite(NazadL, 0);

    int calc2 = SrednjaVrijednostNN - SrednjaVrijednostLD;
    if (calc2 < 0) {
      calc2 = 0;
    }
    analogWrite(NaprijedD, calc2);
    analogWrite(NazadD, 0);

  } else if (SrednjaVrijednostNN < 0) {
    int calc3 = SrednjaVrijednostNN * (-1) + SrednjaVrijednostLD;
    if (calc3 < 0) {
      calc3 = 0;
    }
    analogWrite(NaprijedL, 0);
    analogWrite(NazadL, calc3);

    int calc4 = SrednjaVrijednostNN * (-1) - SrednjaVrijednostLD;
    if (calc4 < 0) {
      calc4 = 0;
    }
    analogWrite(NaprijedD, 0);
    analogWrite(NazadD, calc4);
  }
}

int gladenje(int ulaz) {
  // subtract the last reading:
  total = total - readings[readIndex];
  // read from the sensor:
  readings[readIndex] = ulaz;
  // add the reading to the total:
  total = total + readings[readIndex];
  // advance to the next position in the array:
  readIndex = readIndex + 1;

  // if we're at the end of the array...
  if (readIndex >= numReadings) {
    // ...wrap around to the beginning:
    readIndex = 0;
  }

  // calculate the average:
  average = total / numReadings;
  return average;
}

int gladenje1(int ulaz1) {
  // subtract the last reading:
  total1 = total1 - readings1[readIndex1];
  // read from the sensor:
  readings1[readIndex1] = ulaz1;
  // add the reading to the total:
  total1 = total1 + readings1[readIndex1];
  // advance to the next position in the array:
  readIndex1 = readIndex1 + 1;

  // if we're at the end of the array...
  if (readIndex1 >= numReadings1) {
    // ...wrap around to the beginning:
    readIndex1 = 0;
  }

  // calculate the average:
  average1 = total1 / numReadings1;
  return average1;
}

void IspisNN() {
  Serial.print( "  PV = " ); // prvobitna vrijednost

  Serial.print(NN);

  Serial.print( "   IV = " ); // izgladjena vrijednost

  Serial.print(SrednjaVrijednostNN);

  Serial.print( "    PV = " );
}

void IspisLD() {
  Serial.print( "    PV = " );

  Serial.print(LD);

  Serial.print( "   IV = " );

  Serial.print(SrednjaVrijednostLD);
}