Untitled

avatar
unknown
plain_text
a year ago
12 kB
9
Indexable
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);  // Set the LCD I2C address

int sensor[6];
int ref[6] = { 367, 360, 285, 337, 283, 308 };
//int ref[6];
int button1 = 10;
int button2 = 11;
int i, xsensor, var, start1, start2;
int s0, s1, s2, s3, s4, s5, flag;
unsigned char pwm_ka, pwm_ki;
int j;
const int in1 = 8;
const int in2 = 9;
const int in3 = 7;
const int in4 = 4;
const int ena_a = 5;
const int ena_b = 6;

int laju, kecepatan;
int data_putih[6], data_gelap[6], data_sensor_depan[6];
int address = 0, address_speed = 10;

void stopped() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}
void maju(unsigned char kiri, unsigned char kanan) {
  pwm_ki = kiri;
  pwm_ka = kanan;
  analogWrite(ena_a, pwm_ka);
  analogWrite(ena_b, pwm_ki);
  digitalWrite(in1, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
}
void mundur(unsigned char kiri, unsigned char kanan) {
  pwm_ki = kiri;
  pwm_ka = kanan;
  analogWrite(ena_a, pwm_ka);
  analogWrite(ena_b, pwm_ki);
  digitalWrite(in1, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, HIGH);
}
void bel_ka(unsigned char kiri, unsigned char kanan) {
  pwm_ki = kiri;
  pwm_ka = kanan;
  analogWrite(ena_a, pwm_ka);
  analogWrite(ena_b, pwm_ki);
  digitalWrite(in1, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
}
void bel_ki(unsigned char kiri, unsigned char kanan) {
  pwm_ki = kiri;
  pwm_ka = kanan;
  analogWrite(ena_a, pwm_ka);
  analogWrite(ena_b, pwm_ki);
  digitalWrite(in1, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, HIGH);
}

void baca_button() {
  start1 = digitalRead(button1);
  start2 = digitalRead(button2);
}
void baca_sensor() {
  sensor[0] = analogRead(A0);
  sensor[1] = analogRead(A1);
  sensor[2] = analogRead(A2);
  sensor[3] = analogRead(A3);
  sensor[4] = analogRead(A6);
  sensor[5] = analogRead(A7);
}
void convert_sensor() {
  baca_sensor();
  /*for(i=0;i<=5;i++){
    ref[i]=EEPROM.read(i);
  }*/
  xsensor = 63;
  xsensor &= 63;
  if (sensor[0] < ref[0]) {
    s0 = 0;
    var = 1;
  } else {
    s0 = 1;
    var = 0;
    flag = 1;
  }
  xsensor &= ~var;
  if (sensor[1] < ref[1]) {
    s1 = 0;
    var = 2;
  } else {
    s1 = 1;
    var = 0;
  }
  xsensor &= ~var;
  if (sensor[2] < ref[2]) {
    s2 = 0;
    var = 4;
  } else {
    s2 = 1;
    var = 0;
  }
  xsensor &= ~var;
  if (sensor[3] < ref[3]) {
    s3 = 0;
    var = 8;
  } else {
    s3 = 1;
    var = 0;
  }
  xsensor &= ~var;
  if (sensor[4] < ref[4]) {
    s4 = 0;
    var = 16;
  } else {
    s4 = 1;
    var = 0;
  }
  xsensor &= ~var;
  if (sensor[5] < ref[5]) {
    s5 = 0;
    var = 32;
  } else {
    s5 = 1;
    var = 0;
    flag = 0;
  }
  xsensor &= ~var;
}
void tampil_sensor() {
  convert_sensor();
  lcd.setCursor(0, 1);
  lcd.print(s5);
  lcd.print(s4);
  lcd.print(s3);
  lcd.print(s2);
  lcd.print(s1);
  lcd.print(s0);
}
void tes_sensor() {
  baca_sensor();
  lcd.setCursor(0, 0);
  lcd.print(sensor[5]);
  lcd.setCursor(4, 0);
  lcd.print(sensor[4]);
  lcd.setCursor(8, 0);
  lcd.print(sensor[3]);
  lcd.setCursor(12, 0);
  lcd.print(sensor[2]);
  lcd.setCursor(0, 1);
  lcd.print(sensor[1]);
  lcd.setCursor(4, 1);
  lcd.print(sensor[0]);
  delay(400);
  lcd.clear();
}
void kalibrasi() {
  lcd.clear();
  lcd.setCursor(5, 1);
  lcd.print("PUTIH");

  delay(80);
  baca_button();
  while (start1) { baca_button(); }
  while (!start1) {
    baca_button();
    lcd.clear();
  }
  baca_sensor();
  data_putih[0] = sensor[0];
  data_putih[1] = sensor[1];
  data_putih[2] = sensor[2];
  data_putih[3] = sensor[3];
  data_putih[4] = sensor[4];
  data_putih[5] = sensor[5];

  lcd.clear();
  lcd.setCursor(5, 1);
  lcd.print("HITAM");

  delay(80);

  baca_button();
  while (start1) { baca_button(); }
  while (!start1) {
    baca_button();
    lcd.clear();
  }
  baca_sensor();
  data_gelap[0] = sensor[0];
  data_gelap[1] = sensor[1];
  data_gelap[2] = sensor[2];
  data_gelap[3] = sensor[3];
  data_gelap[4] = sensor[4];
  data_gelap[5] = sensor[5];

  delay(5);
  for (i = 0; i <= 5; i++) {
    ref[i] = ((data_gelap[i] - data_putih[i]) / 2) + data_putih[i];
    EEPROM.write(address, ref[i]);
    address++;
    if (address == 6) {
      address = 0;
    }
  }

  lcd.clear();
  lcd.setCursor(6, 1);
  lcd.print("DONE");

  delay(1000);
  lcd.clear();
}
void pid(unsigned char simpan) {
  unsigned int kec_maks = 175;
  unsigned int kec_min = 100;
  float kp = 2.1;               //6
  float kd = 1.2;               //10
  static unsigned char ki = 0;  //146.5              //300        1130       1730    750
  static int error, lastError, Error, LastError, SumError, right_speed, left_speed;

  convert_sensor();
  switch (xsensor) {
    case 7: error = -7; break;
    case 1: error = -6; break;
    case 3: error = -5; break;
    case 2: error = -4; break;
    case 6: error = -3; break;
    case 4: error = -2; break;
    case 28: error = 1; break;
    case 0b001100: error = 0; break;
    case 14: error = 1; break;
    case 8: error = 2; break;
    case 24: error = 3; break;
    case 16: error = 4; break;
    case 48: error = 5; break;
    case 32: error = 6; break;
    case 56: error = 7; break;
    case 0:
      if (flag == 0) {
        error = 8;
      } else {
        error = -8;
      }
      break;
  }

  int SetPoint = 0;  // Setpoint yang diinginkan
  Error = SetPoint - error;
  int outPID = kp * 0.1 * Error + kd * (Error - lastError) + ki * 0.1;
  lastError = Error;

  double motorKi = simpan - outPID;  // Motor Kiri
  double motorKa = simpan + outPID;  // Motor Kanan

  tampil_sensor();
  /*** Pembatasan kecepatan ***/
  if (motorKi > kec_maks) motorKi = kec_maks;
  if (motorKi < kec_min) motorKi = kec_min;
  if (motorKa > kec_maks) motorKa = kec_maks;
  if (motorKa < kec_min) motorKa = kec_min;
  if (motorKi == motorKa) {
    maju(simpan, simpan);
    lcd.setCursor(10, 0);
    lcd.print("F ");
  } else if (motorKi > motorKa) {
    bel_ki(motorKi, motorKa);
    lcd.setCursor(10, 0);
    lcd.print("L ");
  } else if (motorKa > motorKi) {
    bel_ka(motorKi, motorKa);
    lcd.setCursor(10, 0);
    lcd.print("R ");
  }
}
void pid_mundur(unsigned char simpan) {
  unsigned int kec_maks = 130;
  unsigned int kec_min = 80;
  float kp = 2.1;               //6
  float kd = 1.2;               //10
  static unsigned char ki = 0;  //146.5              //300        1130       1730    750
  static int error, lastError, Error, LastError, SumError, right_speed, left_speed;

  convert_sensor();
  switch (xsensor) {
    case 7: error = -7; break;
    case 1: error = -6; break;
    case 3: error = -5; break;
    case 2: error = -4; break;
    case 6: error = -3; break;
    case 4: error = -2; break;
    case 28: error = 1; break;
    case 12: error = 0; break;
    case 14: error = 1; break;
    case 8: error = 2; break;
    case 24: error = 3; break;
    case 16: error = 4; break;
    case 48: error = 5; break;
    case 32: error = 6; break;
    case 56: error = 7; break;
    case 0:
      if (flag == 0) {
        error = 8;
      } else {
        error = -8;
      }
      break;
  }

  int SetPoint = 0;  // Setpoint yang diinginkan
  Error = SetPoint - error;
  int outPID = kp * 0.1 * Error + kd * (Error - lastError) + ki * 0.1;
  lastError = Error;

  double motorKi = simpan - outPID;  // Motor Kiri
  double motorKa = simpan + outPID;  // Motor Kanan

  tampil_sensor();
  /*** Pembatasan kecepatan ***/
  if (motorKi > kec_maks) motorKi = kec_maks;
  if (motorKi < kec_min) motorKi = kec_min;
  if (motorKa > kec_maks) motorKa = kec_maks;
  if (motorKa < kec_min) motorKa = kec_min;
  if (motorKi == motorKa) {
    mundur(simpan, simpan);
    lcd.setCursor(10, 0);
    lcd.print("RF ");
  } else if (motorKi > motorKa) {
    bel_ki(motorKi, motorKa);
    lcd.setCursor(10, 0);
    lcd.print("L ");
  } else if (motorKa > motorKi) {
    bel_ka(motorKi, motorKa);
    lcd.setCursor(10, 0);
    lcd.print("R ");
  }
}
void kanan() {
  while (1) {
    pid(110);
    if (s5 && s0)
      break;
  }
  rem();
  maju(100, 100);
  delay(50);  //120
  while (1) {
    bel_ka(110, 140);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5) break;
  }
  while (1) {
    bel_ka(80, 120);
    convert_sensor();
    if (s1 || s2) break;
  }
}
void kiri() {
  while (1) {
    pid(110);
    if (s5 && s0)
      break;
  }
  rem();
  maju(100, 100);
  delay(50);  //120
  while (1) {
    bel_ki(140, 120);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5) break;
  }
  while (1) {
    bel_ki(150, 110);
    convert_sensor();
    if (s4 || s3) break;
  }
}
void siku_kanan() {
  while (1) {
    pid(110);
    if ((s3 && s2 && s1 && s0) || (s2 && s1 && s0) || (s2 && s1 && s3))
      break;
  }
  rem();
  maju(100, 100);
  delay(50);  //120
  while (1) {
    bel_ka(110, 140);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5) break;
  }
  while (1) {
    bel_ka(80, 120);
    convert_sensor();
    if (s1 || s2) break;
  }
}
void siku_kiri() {
  while (1) {
    pid(110);
    if ((s5 && s4 && s3 && s2) || (s4 && s3 && s2) || (s5 && s4 && s3))
      break;
  }
  rem();
  maju(100, 100);
  delay(30);  //120
  while (1) {
    bel_ki(140, 120);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5) break;
  }
  while (1) {
    bel_ki(150, 110);
    convert_sensor();
    if (s4 || s3) break;
  }
}
void pertigaan_kiri() {
  while (1) {
    pid(100);
    if (s5 && s4 && s3)
      break;
  }
  maju(100, 100);
  delay(70);
  stopped();
  delay(100);
  while (1) {
    bel_ki(190, 130);
    convert_sensor();
    if (s5) break;
  }
}
void pertigaan_kanan() {
  while (1) {
    pid(120);
    if ((s3 && s2 && s1 && s0) || (s2 && s1 && s0) || (s2 && s1 && s3))
      break;
  }
  rem();
  maju(100, 100);
  delay(50);
  while (1) {
    bel_ka(110, 140);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5) break;
  }
  while (1) {
    bel_ka(80, 120);
    convert_sensor();
    if (s1 || s2) break;
  }
}
void lurus() {
  while (1) {
    pid(120);
    if (s1 && s5 || s1 || s5)
      break;
  }
  while (1) {
    maju(100, 100);
    delay(100);
    convert_sensor();
    if (s1 || s2 || s3 || s4) break;
  }
}
void berhenti_putih() {
  while (1) {
    pid(120);
    if (!s0 && !s1 && !s2 && !s3 && !s5 && !s4)
      break;
  }
  while (1) {
    stopped();
    break;
  }
}
void berhenti_hitam() {
  while (1) {
    pid(110);
    if (s0 && s5)
      break;
  }
  while (1) {
    stopped();
    break;
  }
  rem();
}
void putar_balik() {
  while (1) {
    bel_ka(140, 140);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5)
      break;
  }
  rem();
  while (1) {
    convert_sensor();
    bel_ka(100, 110);
    if (s0 || s1) break;
  }
}
void putar_balik2() {
  while (1) {
    bel_ka(100, 170);
    delay(850);
    break;
  }
  while (1) {
    convert_sensor();
    bel_ka(100, 170);
    if (s0 || s1) {
      break;
    }
  }
}
void mundur_perempatan_kiri() {
  while (1) {
    pid_mundur(110);
    if (s5 || s0)
      break;
  }
  rem();
  maju(100, 100);
  delay(90);  //120
  while (1) {
    bel_ki(110, 140);
    convert_sensor();
    if (!s0 && !s1 && !s2 && !s3 && !s4 && !s5) break;
  }
  while (1) {
    bel_ki(80, 120);
    convert_sensor();
    if (s3 || s4) break;
  }
}

void rem() {
  mundur(150, 150);
  delay(50);
  stopped();
}
void setup() {
  lcd.begin();
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(ena_a, OUTPUT);
  pinMode(ena_b, OUTPUT);
  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);

  lcd.setCursor(0, 0);
  lcd.print("   Project UAS      ");
  lcd.setCursor(0, 1);
  lcd.print("Line Follower ");
  delay(1000);
  lcd.clear();
}

void loop() {
  lcd.setCursor(0, 0);
  lcd.print("1.Sensor");
  lcd.setCursor(0, 1);
  lcd.print("2.Start");
  baca_button();
  while (start1 && start2) { baca_button(); }
  if (!start1) {
    while (!start1) {
      tes_sensor();
    }
  } else if (!start2) {
    delay(100);
    lcd.clear();
    while (!start2) { baca_button(); }
    while (1) {
      pid(150);

      pertigaan_kanan();
      pertigaan_kiri();
      kanan();
      delay(40);
      pertigaan_kiri();
      kanan();
      delay(50);
      kiri();
      kanan();
      kiri();
      berhenti_hitam();


      while (1) {
        stopped();
      }
    }
  }
}
Editor is loading...
Leave a Comment