Вы находитесь на странице: 1из 18

Listing Program Arduino

#include <Fuzzy.h>
#include <FuzzyComposition.h>
#include <FuzzyInput.h>
#include <FuzzyIO.h>
#include <FuzzyOutput.h>
#include <FuzzyRule.h>
#include <FuzzyRuleAntecedent.h>
#include <FuzzyRuleConsequent.h>
#include <FuzzySet.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <NewPing.h>
#include <SPI.h>
#include <SD.h>

LiquidCrystal_I2C lcd(0x27,20,4);
NewPing range(42, 42, 400);
const int chipSelect=53;
long id=1;

// Inisialisasi Line Tracer Sensor //


int LT1=A12;
int LT2=A13;
int LT3=A14;
int LT4=A10;
int LT5=A9;
int LT6=A8;
//================================//
boolean LLL,LL,L,R,RR,RRR;
boolean LastKanan = 0;
boolean LastKiri = 0;

int VRX = A7;


int VRY = A6;
int VRX_, VRY_;
float Deviasi;

Servo spd, steer;


int Ping1, Ping2, Ping3, PingLast;
int kalibrasiESC = 1500;
int val_spd = 12;
int val_spd1, pengereman;
int mulai = 0;
int POT=A15;
int valPOT;
int start = 0;
int changespeed;
int hard=0;
int errorACC;
float val_spd__;
int change_ = 0;
int tekan =0;
int mundur=0;

//=================Inisialisasi Fitur======================//
boolean lkas=0;
boolean absrcas=0;
boolean acc=0;
boolean manual=0;
//==================================================//

//============Inisialisasi Nilai Fuzzy Input=================//


Fuzzy* fuzzy = new Fuzzy();

FuzzySet* Lambat = new FuzzySet(0, 0, 13, 20);


FuzzySet* Sedang = new FuzzySet(15, 25, 25, 35);
FuzzySet* Cepat = new FuzzySet(30, 40, 50, 50);

FuzzySet* Dekat = new FuzzySet(0, 0, 60, 125);


FuzzySet* Aman = new FuzzySet(60, 150, 150, 240);
FuzzySet* Jauh = new FuzzySet(175, 240, 350, 350);

FuzzySet* Kiri = new FuzzySet(1, 1, 1, 3);


FuzzySet* Tengah = new FuzzySet(1, 3, 3, 5);
FuzzySet* Kanan = new FuzzySet(3, 5, 5, 5);

FuzzySet* DekatACC = new FuzzySet(0, 0, 45, 85);


FuzzySet* AmanACC = new FuzzySet(45, 120, 120, 195);
FuzzySet* JauhACC = new FuzzySet(130, 195, 350, 350);

FuzzySet* Kecil = new FuzzySet(0, 0, 0, 50);


FuzzySet* Lumayan = new FuzzySet(10, 60, 60, 110);
FuzzySet* Besar = new FuzzySet(70, 110, 130, 130);

void setup() {
Serial.begin(9600);
pinMode(18,OUTPUT); //Mode pin u/ I2C
pinMode(19,OUTPUT); //Mode pin u/ I2C
pinMode(3,OUTPUT);
pinMode(22,OUTPUT);
pinMode(23,OUTPUT);
digitalWrite(3,LOW); //Gnd u/ESC
digitalWrite(19,HIGH); //Vcc u/I2C
digitalWrite(18,LOW); //Gnd u/ I2C
digitalWrite(22,HIGH); //Vcc u/ FAN
digitalWrite(23,LOW); //Gnd u/ FAN

steer.attach(13);
spd.attach(4);
//spd.writeMicroseconds(kalibrasiESC);
lcd.init();
lcd.backlight();

//Fuzzy Input Kecepatan


FuzzyInput* Kecepatan = new FuzzyInput(1);
Kecepatan->addFuzzySet(Lambat);
Kecepatan->addFuzzySet(Sedang);
Kecepatan->addFuzzySet(Cepat);
fuzzy->addFuzzyInput(Kecepatan);

// FuzzyInput Jarak
FuzzyInput* Jarak= new FuzzyInput(2);
Jarak->addFuzzySet(Dekat);
Jarak->addFuzzySet(Aman);
Jarak->addFuzzySet(Jauh);
fuzzy->addFuzzyInput(Jarak);

// FuzzyInput Deviasi
FuzzyInput* Deviasi = new FuzzyInput(3);
Deviasi->addFuzzySet(Kiri);
Deviasi->addFuzzySet(Tengah);
Deviasi->addFuzzySet(Kanan);
fuzzy->addFuzzyInput(Deviasi);

// FuzzyInput Jarak ACC


FuzzyInput* JarakACC= new FuzzyInput(4);
JarakACC->addFuzzySet(DekatACC);
JarakACC->addFuzzySet(AmanACC);
JarakACC->addFuzzySet(JauhACC);
fuzzy->addFuzzyInput(JarakACC);

// FuzzyInput Error
FuzzyInput* Error= new FuzzyInput(5);
Error->addFuzzySet(Kecil);
Error->addFuzzySet(Lumayan);
Error->addFuzzySet(Besar);
fuzzy->addFuzzyInput(Error);

// FuzzyOutput Pengereman
FuzzyOutput* Rem = new FuzzyOutput(1);

FuzzySet* NoRem = new FuzzySet(0, 0, 0, 0);


Rem->addFuzzySet(NoRem);
FuzzySet* SoftBrake = new FuzzySet(4, 12, 12, 20);
Rem->addFuzzySet(SoftBrake);
FuzzySet* NormalBrake = new FuzzySet(15, 22.5, 22.5, 30);
Rem->addFuzzySet(NormalBrake);
FuzzySet* HardBrake = new FuzzySet(25, 32.5, 32.5, 40);
Rem->addFuzzySet(HardBrake);
FuzzySet* VeryHardBrake = new FuzzySet(37, 41, 50, 50);
Rem->addFuzzySet(VeryHardBrake);

fuzzy->addFuzzyOutput(Rem);

// FuzzyOutput Sudut Belok


FuzzyOutput* SudutBelok = new FuzzyOutput(2);

FuzzySet* Left = new FuzzySet(60, 60, 60, 90);


SudutBelok->addFuzzySet(Left);
FuzzySet* Lurus = new FuzzySet(60, 90, 90, 120);
SudutBelok->addFuzzySet(Lurus);
FuzzySet* Right = new FuzzySet(90, 120, 120, 120);
SudutBelok->addFuzzySet(Right);

fuzzy->addFuzzyOutput(SudutBelok);

// FuzzyOutput Kecepatan ACC


FuzzyOutput* KecepatanACC = new FuzzyOutput(3);

FuzzySet* LambatACC = new FuzzySet(0, 0, 0, 5);


KecepatanACC->addFuzzySet(LambatACC);
FuzzySet* SedangACC = new FuzzySet(0, 25, 25, 50);
KecepatanACC->addFuzzySet(SedangACC);
FuzzySet* CepatACC = new FuzzySet(25, 40, 50, 50);
KecepatanACC->addFuzzySet(CepatACC);

fuzzy->addFuzzyOutput(KecepatanACC);

//=================Fuzzy Rule ABS==========================//

//Fuzzy Rule1
FuzzyRuleAntecedent* ifKecepatanLambatAndJarakDekat = new FuzzyRuleAntecedent();
ifKecepatanLambatAndJarakDekat->joinWithAND(Lambat, Dekat);

FuzzyRuleConsequent* thenRemSoftBrake = new FuzzyRuleConsequent();


thenRemSoftBrake->addOutput(SoftBrake);

FuzzyRuleConsequent* thenRemNormalBrake = new FuzzyRuleConsequent();


thenRemNormalBrake->addOutput(NormalBrake);

FuzzyRule* fuzzyRule1 = new FuzzyRule(1, ifKecepatanLambatAndJarakDekat,


thenRemSoftBrake);
fuzzy->addFuzzyRule(fuzzyRule1);

//Fuzzy Rule2
FuzzyRuleAntecedent* ifKecepatanLambatAndJarakAman = new FuzzyRuleAntecedent();
ifKecepatanLambatAndJarakAman->joinWithAND(Lambat, Aman);

FuzzyRuleConsequent* thenRemNoRem = new FuzzyRuleConsequent();


thenRemNoRem->addOutput(NoRem);

FuzzyRule* fuzzyRule2 = new FuzzyRule(2, ifKecepatanLambatAndJarakAman,


thenRemNoRem);
fuzzy->addFuzzyRule(fuzzyRule2);

//Fuzzy Rule3
FuzzyRuleAntecedent* ifKecepatanLambatAndJarakJauh = new FuzzyRuleAntecedent();
ifKecepatanLambatAndJarakJauh->joinWithAND(Lambat, Jauh);

FuzzyRule* fuzzyRule3 = new FuzzyRule(3, ifKecepatanLambatAndJarakJauh,


thenRemNoRem);
fuzzy->addFuzzyRule(fuzzyRule3);

//Fuzzy Rule4
FuzzyRuleAntecedent* ifKecepatanSedangAndJarakDekat = new FuzzyRuleAntecedent();
ifKecepatanSedangAndJarakDekat->joinWithAND(Sedang, Dekat);

FuzzyRuleConsequent* thenRemHardBrake = new FuzzyRuleConsequent();


thenRemHardBrake->addOutput(HardBrake);

FuzzyRule* fuzzyRule4 = new FuzzyRule(4, ifKecepatanSedangAndJarakDekat,


thenRemHardBrake);
fuzzy->addFuzzyRule(fuzzyRule4);

//Fuzzy Rule5
FuzzyRuleAntecedent* ifKecepatanSedangAndJarakAman = new FuzzyRuleAntecedent();
ifKecepatanSedangAndJarakAman->joinWithAND(Sedang, Aman);

FuzzyRule* fuzzyRule5 = new FuzzyRule(5, ifKecepatanSedangAndJarakAman,


thenRemSoftBrake);
fuzzy->addFuzzyRule(fuzzyRule5);

//Fuzzy Rule6
FuzzyRuleAntecedent* ifKecepatanSedangAndJarakJauh = new FuzzyRuleAntecedent();
ifKecepatanSedangAndJarakJauh->joinWithAND(Sedang, Jauh);

FuzzyRule* fuzzyRule6 = new FuzzyRule(6, ifKecepatanSedangAndJarakJauh,


thenRemNoRem);
fuzzy->addFuzzyRule(fuzzyRule6);

//Fuzzy Rule7
FuzzyRuleAntecedent* ifKecepatanCepatAndJarakDekat = new FuzzyRuleAntecedent();
ifKecepatanCepatAndJarakDekat ->joinWithAND(Cepat, Dekat);

FuzzyRuleConsequent* thenRemVeryHardBrake = new FuzzyRuleConsequent();


thenRemVeryHardBrake->addOutput(VeryHardBrake);

FuzzyRule* fuzzyRule7 = new FuzzyRule(7, ifKecepatanCepatAndJarakDekat,


thenRemVeryHardBrake);
fuzzy->addFuzzyRule(fuzzyRule7);

//Fuzzy Rule8
FuzzyRuleAntecedent* ifKecepatanCepatAndJarakAman = new FuzzyRuleAntecedent();
ifKecepatanCepatAndJarakAman ->joinWithAND(Cepat, Aman);

FuzzyRule* fuzzyRule8 = new FuzzyRule(8, ifKecepatanCepatAndJarakAman,


thenRemNormalBrake);
fuzzy->addFuzzyRule(fuzzyRule8);

//Fuzzy Rule9
FuzzyRuleAntecedent* ifKecepatanCepatAndJarakJauh = new FuzzyRuleAntecedent();
ifKecepatanCepatAndJarakJauh ->joinWithAND(Cepat, Jauh);

FuzzyRule* fuzzyRule9 = new FuzzyRule(9, ifKecepatanCepatAndJarakJauh,


thenRemSoftBrake);
fuzzy->addFuzzyRule(fuzzyRule9);

//==================Fuzzy Rule LKAS============================//


// Building FuzzyRule 10
FuzzyRuleAntecedent* ifDeviasiKiriAndKecepatanLambat = new FuzzyRuleAntecedent();
ifDeviasiKiriAndKecepatanLambat->joinWithAND(Kiri, Lambat);

FuzzyRuleConsequent* thenSudutBelokLeft = new FuzzyRuleConsequent();


thenSudutBelokLeft->addOutput(Left);

FuzzyRule* fuzzyRule10 = new FuzzyRule(10, ifDeviasiKiriAndKecepatanLambat,


thenSudutBelokLeft);
fuzzy->addFuzzyRule(fuzzyRule10);

// Building FuzzyRule 11
FuzzyRuleAntecedent* ifDeviasiTengahAndKecepatanLambat = new FuzzyRuleAntecedent();
ifDeviasiTengahAndKecepatanLambat->joinWithAND(Tengah, Lambat);

FuzzyRuleConsequent* thenSudutBelokLurus = new FuzzyRuleConsequent();


thenSudutBelokLurus->addOutput(Lurus);

FuzzyRule* fuzzyRule11 = new FuzzyRule(11, ifDeviasiTengahAndKecepatanLambat,


thenSudutBelokLurus);
fuzzy->addFuzzyRule(fuzzyRule11);

// Building FuzzyRule 12
FuzzyRuleAntecedent* ifDeviasiKananAndKecepatanLambat = new FuzzyRuleAntecedent();
ifDeviasiKananAndKecepatanLambat->joinWithAND(Kanan, Lambat);

FuzzyRuleConsequent* thenSudutBelokRight = new FuzzyRuleConsequent();


thenSudutBelokRight->addOutput(Right);

FuzzyRule* fuzzyRule12 = new FuzzyRule(12, ifDeviasiKananAndKecepatanLambat,


thenSudutBelokRight);
fuzzy->addFuzzyRule(fuzzyRule12);

// Building FuzzyRule 13
FuzzyRuleAntecedent* ifDeviasiKiriAndKecepatanSedang = new FuzzyRuleAntecedent();
ifDeviasiKiriAndKecepatanSedang->joinWithAND(Kiri, Sedang);

FuzzyRule* fuzzyRule13 = new FuzzyRule(13, ifDeviasiKiriAndKecepatanSedang,


thenSudutBelokLeft);
fuzzy->addFuzzyRule(fuzzyRule13);

// Building FuzzyRule 14
FuzzyRuleAntecedent* ifDeviasiTengahAndKecepatanSedang = new FuzzyRuleAntecedent();
ifDeviasiTengahAndKecepatanSedang->joinWithAND(Tengah, Sedang);

FuzzyRule* fuzzyRule14= new FuzzyRule(14, ifDeviasiTengahAndKecepatanSedang,


thenSudutBelokLurus);
fuzzy->addFuzzyRule(fuzzyRule14);

// Building FuzzyRule 15
FuzzyRuleAntecedent* ifDeviasiKananAndKecepatanSedang = new FuzzyRuleAntecedent();
ifDeviasiKananAndKecepatanSedang->joinWithAND(Kanan, Sedang);
FuzzyRule* fuzzyRule15= new FuzzyRule(15, ifDeviasiKananAndKecepatanSedang,
thenSudutBelokRight);
fuzzy->addFuzzyRule(fuzzyRule15);

// Building FuzzyRule 16
FuzzyRuleAntecedent* ifDeviasiKiriAndKecepatanCepat = new FuzzyRuleAntecedent();
ifDeviasiKiriAndKecepatanCepat->joinWithAND(Kiri, Cepat);

FuzzyRule* fuzzyRule16= new FuzzyRule(16, ifDeviasiKiriAndKecepatanCepat,


thenSudutBelokLeft);
fuzzy->addFuzzyRule(fuzzyRule16);

// Building FuzzyRule 17
FuzzyRuleAntecedent* ifDeviasiTengahAndKecepatanCepat = new FuzzyRuleAntecedent();
ifDeviasiTengahAndKecepatanCepat->joinWithAND(Tengah, Cepat);

FuzzyRule* fuzzyRule17= new FuzzyRule(17, ifDeviasiTengahAndKecepatanCepat,


thenSudutBelokLurus);
fuzzy->addFuzzyRule(fuzzyRule17);

// Building FuzzyRule 18
FuzzyRuleAntecedent* ifDeviasiKananAndKecepatanCepat = new FuzzyRuleAntecedent();
ifDeviasiKananAndKecepatanCepat->joinWithAND(Kanan, Cepat);

FuzzyRule* fuzzyRule18= new FuzzyRule(18, ifDeviasiKananAndKecepatanCepat,


thenSudutBelokRight);
fuzzy->addFuzzyRule(fuzzyRule18);

//=================Fuzzy Rule Adaptive Cruise Control======================//

//Fuzzy Rule19
FuzzyRuleAntecedent* ifJarakACCDekatACCAndErrorKecil = new FuzzyRuleAntecedent();
ifJarakACCDekatACCAndErrorKecil->joinWithAND(DekatACC, Kecil);

FuzzyRuleConsequent* thenKecepatanACCLambatACC = new FuzzyRuleConsequent();


thenKecepatanACCLambatACC->addOutput(LambatACC);

FuzzyRule* fuzzyRule19 = new FuzzyRule(19, ifJarakACCDekatACCAndErrorKecil,


thenKecepatanACCLambatACC);
fuzzy->addFuzzyRule(fuzzyRule19);

//Fuzzy Rule20
FuzzyRuleAntecedent* ifJarakACCAmanACCAndErrorLumayan = new
FuzzyRuleAntecedent();
ifJarakACCAmanACCAndErrorLumayan->joinWithAND(AmanACC, Lumayan);

FuzzyRuleConsequent* thenKecepatanACCSedangACC = new FuzzyRuleConsequent();


thenKecepatanACCSedangACC->addOutput(SedangACC);

FuzzyRule* fuzzyRule20 = new FuzzyRule(20, ifJarakACCAmanACCAndErrorLumayan,


thenKecepatanACCSedangACC);
fuzzy->addFuzzyRule(fuzzyRule20);

//Fuzzy Rule21
FuzzyRuleAntecedent* ifJarakACCJauhACCAndErrorBesar = new FuzzyRuleAntecedent();
ifJarakACCJauhACCAndErrorBesar->joinWithAND(JauhACC, Besar);

FuzzyRuleConsequent* thenKecepatanACCCepatACC = new FuzzyRuleConsequent();


thenKecepatanACCCepatACC->addOutput(CepatACC);

FuzzyRule* fuzzyRule21 = new FuzzyRule(21, ifJarakACCJauhACCAndErrorBesar,


thenKecepatanACCCepatACC);
fuzzy->addFuzzyRule(fuzzyRule21);

//===========================================================//
//====================Inisialisasi Awal===========================//

while (start==0)
{
Ping1 = range.ping_cm();
delay(30);

RRR = digitalRead(LT1);
RR = digitalRead(LT2);
R = digitalRead(LT3);
L = digitalRead(LT4);
LL = digitalRead(LT5);
LLL = digitalRead(LT6);
VRX_ = analogRead(VRX);
VRY_ = analogRead(VRY);

lcd.clear();
lcd.setCursor(0,0);
lcd.print("-----Bismillah.-----");
lcd.setCursor(2,1);
lcd.print("Jrk =");
lcd.setCursor(7,1);
lcd.print(Ping1);
lcd.setCursor(12,1);
lcd.print("Spd=");
lcd.setCursor(16,1);
lcd.print(val_spd);
lcd.setCursor(2,2);
lcd.print("LKAS=");
lcd.setCursor(7,2);
lcd.print(lkas);
lcd.setCursor(12,2);
lcd.print("ABS=");
lcd.setCursor(16,2);
lcd.print(absrcas);
lcd.setCursor(2,3);
lcd.print("RCAS=");
lcd.setCursor(7,3);
lcd.print(absrcas);
lcd.setCursor(12,3);
lcd.print("ACC=");
lcd.setCursor(16,3);
lcd.print(acc);
spd.writeMicroseconds(kalibrasiESC);
delay(100);

if (Serial.available() > 0){


char bt = Serial.read();
if(bt=='X')
{
start = 1;
//SD Data Logger
pinMode(chipSelect, OUTPUT);

Serial.print("Initializing SD Card...");

if (!SD.begin(chipSelect))
{ Serial.println("Card Failed, Or Not Present");
return;
}
Serial.println("card initialized.");

//Log File Header


File logfile = SD.open("ABS_LOW1.csv", FILE_WRITE);
if(logfile)
{
logfile.println("===========================");
String header = "ID | JARAK | SPEED | REM |";
logfile.println(header);
logfile.close();
Serial.println(header);
}
else
{
Serial.println("Couldn't open log file");
}
lcd.clear();
}

else if(bt == 'A')


{
if (lkas==0)
lkas = 1;
else if (lkas==1)
lkas = 0;
}
else if(bt == 'B')
{
if (absrcas==0)
absrcas = 1;
else if (absrcas==1)
absrcas = 0;
}
else if(bt == 'C')
{
if (acc==0)
acc = 1;
else if (acc==1)
acc = 0;
}
else if(bt == 'D')
val_spd = 12;
else if(bt == 'E')
val_spd = 25;
else if(bt == 'F')
val_spd = 40;
}
}
//==============================================================//

void loop() {
//===================Komunikasi Dengan Android=====================//
if (Serial.available() > 0){
char bt = Serial.read();
if(bt == 'O')
{
tekan=0;
val_spd=0;
spd.writeMicroseconds(1500);
}
else if(bt == 'A')
{
if (lkas==0)
lkas = 1;
else if (lkas==1)
lkas = 0;
}
else if(bt == 'B')
{
if (absrcas==0)
absrcas = 1;
else if (absrcas==1)
absrcas = 0;
}
else if(bt == 'C')
{
if (acc==0)
acc = 1;
else if (acc==1)
acc = 0;
}
else if(bt == 'D')
val_spd = 12;
else if(bt == 'E')
val_spd = 25;
else if(bt == 'F')
val_spd = 40;
//====================Mode Manual==========================//
else if(bt == 'G') //Serong Atas kiri
{
tekan=1;
steer.write(115);
delay(100);
}
else if(bt == 'H')
{
tekan=0;
spd.writeMicroseconds(1500);
steer.write(85);
delay(100);
}
else if(bt == 'I') // Kiri
{
tekan=1;
steer.write(115);
delay(100);
}
else if(bt == 'J')
{
tekan=0;
steer.write(85);
delay(100);
}
else if(bt == 'K') // Serong bawah kiri
{
mundur=1;
tekan=1;
spd.writeMicroseconds(1585);
steer.write(115);
delay(100);
}
else if(bt == 'L')
{
tekan=0;
spd.writeMicroseconds(1500);
steer.write(85);
delay(100);
}
else if(bt == 'M') // Maju
{
tekan=1;
steer.write(85);
delay(100);
}
else if(bt == 'N')
{
tekan=0;
spd.writeMicroseconds(1500);
steer.write(85);
delay(100);
}
else if(bt == 'P') //Mundur
{
mundur=1;
tekan=1;
spd.writeMicroseconds(1585);
steer.write(85);
delay(100);
}
else if(bt == 'Q')
{
tekan=0;
spd.writeMicroseconds(1500);
steer.write(85);
delay(100);
}
else if(bt == 'R') //Serong kanan atas
{
tekan=1;
steer.write(40);
delay(100);
}
else if(bt == 'S')
{
tekan=0;
spd.writeMicroseconds(1500);
steer.write(85);
delay(100);
}
else if(bt == 'T') //Kanan
{
tekan=1;
steer.write(40);
delay(100);
}
else if(bt == 'U')
{
tekan=0;
steer.write(85);
delay(100);
}
else if(bt == 'V') //Serong Kanan bawah
{
mundur=1;
tekan=1;
spd.writeMicroseconds(1585);
steer.write(40);
delay(100);
}
else if(bt == 'W')
{
tekan=0;
spd.writeMicroseconds(1500);
steer.write(85);
delay(100);
}
else
{
tekan=0;
mundur=0;
}
}
//=======================================================//

//====Baca Sensor Garis ======//


RRR = digitalRead(LT1);
RR = digitalRead(LT2);
R = digitalRead(LT3);
L = digitalRead(LT4);
LL = digitalRead(LT5);
LLL = digitalRead(LT6);
VRX_ = analogRead(VRX);
VRY_ = analogRead(VRY);

//=====Baca Sensor Jarak=====//


Ping1 = range.ping_cm();
Ping2 = range.ping_cm();
Ping3 = range.ping_cm();
PingLast = (Ping1 + Ping2 + Ping3)/3;
errorACC = PingLast - 50 ;

//----------------------------Penentuan Deviasi----------------------------//
// Servo < 90 = Kanan ======= Servo > 90 = Kiri
if (LLL==1 && LL==1 && L==0 && R==1 && RR==1 && RRR==1) // 110__111
{
Deviasi = 1.578;
LastKiri = 1;
//steer.write(78);
}

else if (LLL==1 && LL==0 && L==0 && R==1 && RR==1 && RRR==1) // 100__111
{
Deviasi = 1.580;
LastKiri = 1;
//steer.write(78);
}

else if (LLL==1 && LL==0 && L==1 && R==1 && RR==1 && RRR==1) // 101__111
{
Deviasi = 1.580;
LastKiri = 1;
//steer.write(78);
}

else if (LLL==0 && LL==0 && L==1 && R==1 && RR==1 && RRR==1) // 001__111
{
Deviasi = 1.585;
LastKiri = 1;
//steer.write(78);
}
else if (LLL==0 && LL==1 && L==1 && R==1 && RR==1 && RRR==1) // 011__111
{
Deviasi = 1.578;
LastKiri = 0;
//steer.write(78);
}
else if ((LLL==0 && LL==1 && L==1 && R==1 && RR==1 && RRR==0) || (LLL==0 &&
LL==1 && L==1 && R==1 && RR==1 && RRR==0)) // 011__110
{
Deviasi = 3;
LastKiri = 0;
//steer.write(85);
}

else if (LLL==1 && LL==1 && L==1 && R==1 && RR==1 && RRR==0) // 111__110
{
Deviasi = 4.422;
LastKanan = 0;
//steer.write(92);
}

else if (LLL==1 && LL==1 && L==1 && R==1 && RR==0 && RRR==0) // 111__100
{
Deviasi = 4.415;
LastKanan = 1;
//steer.write(92);
}

else if (LLL==1 && LL==1 && L==1 && R==1 && RR==0 && RRR==1) // 111__101
{
Deviasi = 4.420;
LastKanan = 1;
//steer.write(92);
}

else if (LLL==1 && LL==1 && L==1 && R==0 && RR==0 && RRR==1) // 111__001
{
Deviasi = 4.420;
LastKanan = 1;
//steer.write(92);
}

else if (LLL==1 && LL==1 && L==1 && R==0 && RR==1 && RRR==1) // 111__011
{
Deviasi = 4.422;
LastKanan = 1;
//steer.write(92);
}

else if (LastKanan == 0 && LastKiri == 0)


{

Deviasi = 3;
steer.write(85);
}

//=============================================================//

fuzzy->setInput(1, val_spd);
fuzzy->setInput(2, PingLast);
fuzzy->setInput(3, Deviasi);
fuzzy->setInput(4, PingLast);
fuzzy->setInput(5, errorACC);
fuzzy->fuzzify();
float rem_ = fuzzy->defuzzify(1);
float steer_ = fuzzy->defuzzify(2);
float spdACC = fuzzy->defuzzify(3);

//==================Output Crisp Fuzzy===================//


float steer__ = steer_ - 5; //servo koreksi
if (absrcas==1 && acc==0 && tekan ==1)
val_spd__ = 1400 - val_spd + rem_; // out fuzzy ABS + RCAS
else if (acc==1 && absrcas==0)
val_spd__ = kalibrasiESC - 100 - spdACC; //out fuzzy ACC
else if (acc==0 && absrcas==0 && lkas==1 && tekan ==1 || acc==0 && absrcas==0 &&
lkas==0 && tekan ==1 )
{
val_spd__ = kalibrasiESC - 100 - val_spd; //Out normal tanpa pengereman
}
else
{
val_spd__ = kalibrasiESC;
tekan=0;
mundur=0;
hard = 0;
}
//==================================================//

if (val_spd__ >= 1400) //kondisi saat output Fuzzy <=0


{ val_spd__ = 1500; } //kondisi kalibrasi /Netral

if ( rem_ >= 44.9 && hard == 0 && absrcas ==1 && tekan == 1 && PingLast <20) // Very Hard
Brake
{
spd.writeMicroseconds(1600);
delay(200);
spd.writeMicroseconds(1500);
delay(100);
hard =1;
tekan = 0;
}

if(errorACC < 0 && acc ==1)


{
val_spd__ = kalibrasiESC ;
spd.writeMicroseconds(val_spd__);
}
else if (mundur==0)
spd.writeMicroseconds(val_spd__);

//===================Fuzzy LKAS=================//
if (lkas==1)
steer.write(steer__);
else if (tekan==0)
steer.write(85);
//===============================================//
//=====================================================//
//Data String for storing to SD card
String dataString = String(id) + "," + String(Deviasi) + "," + String(steer__) + "," +
String(millis()) + "," + String(tekan);
// String dataString = String(tekan) + "," + String(millis()) + "," + String(LLL) + "," + String(LL)
+ "," + String(L)+ "," + String(R) + "," + String(RR) + "," + String(RRR);
//Open File
File logFile = SD.open("ABS_LOW1.csv", FILE_WRITE);
if (logFile)
{
logFile.println(dataString);
logFile.close();
Serial.println(dataString);
}
else
{
Serial.println("Couldn't open log file");
}

//increment ID number
id++;

//===================================================//

Serial.println(dataString);
lcd.clear();
lcd.setCursor(0,0);
lcd.print(val_spd__);
lcd.setCursor(2,1);
lcd.print("Jrk =");
lcd.setCursor(7,1);
lcd.print(PingLast);
lcd.setCursor(12,1);
lcd.print("Spd=");
lcd.setCursor(16,1);
lcd.print(val_spd);
lcd.setCursor(2,2);
lcd.print("LKAS=");
lcd.setCursor(7,2);
lcd.print(lkas);
lcd.setCursor(12,2);
lcd.print("ABS=");
lcd.setCursor(16,2);
lcd.print(absrcas);
lcd.setCursor(2,3);
lcd.print("RCAS=");
lcd.setCursor(7,3);
lcd.print(rem_);
lcd.setCursor(12,3);
lcd.print("ACC=");
lcd.setCursor(16,3);
lcd.print(steer__);
}
Listing Program Android

Вам также может понравиться