Академический Документы
Профессиональный Документы
Культура Документы
#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 Fitur======================//
boolean lkas=0;
boolean absrcas=0;
boolean acc=0;
boolean manual=0;
//==================================================//
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();
// 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 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);
fuzzy->addFuzzyOutput(Rem);
fuzzy->addFuzzyOutput(SudutBelok);
fuzzy->addFuzzyOutput(KecepatanACC);
//Fuzzy Rule1
FuzzyRuleAntecedent* ifKecepatanLambatAndJarakDekat = new FuzzyRuleAntecedent();
ifKecepatanLambatAndJarakDekat->joinWithAND(Lambat, Dekat);
//Fuzzy Rule2
FuzzyRuleAntecedent* ifKecepatanLambatAndJarakAman = new FuzzyRuleAntecedent();
ifKecepatanLambatAndJarakAman->joinWithAND(Lambat, Aman);
//Fuzzy Rule3
FuzzyRuleAntecedent* ifKecepatanLambatAndJarakJauh = new FuzzyRuleAntecedent();
ifKecepatanLambatAndJarakJauh->joinWithAND(Lambat, Jauh);
//Fuzzy Rule4
FuzzyRuleAntecedent* ifKecepatanSedangAndJarakDekat = new FuzzyRuleAntecedent();
ifKecepatanSedangAndJarakDekat->joinWithAND(Sedang, Dekat);
//Fuzzy Rule5
FuzzyRuleAntecedent* ifKecepatanSedangAndJarakAman = new FuzzyRuleAntecedent();
ifKecepatanSedangAndJarakAman->joinWithAND(Sedang, Aman);
//Fuzzy Rule6
FuzzyRuleAntecedent* ifKecepatanSedangAndJarakJauh = new FuzzyRuleAntecedent();
ifKecepatanSedangAndJarakJauh->joinWithAND(Sedang, Jauh);
//Fuzzy Rule7
FuzzyRuleAntecedent* ifKecepatanCepatAndJarakDekat = new FuzzyRuleAntecedent();
ifKecepatanCepatAndJarakDekat ->joinWithAND(Cepat, Dekat);
//Fuzzy Rule8
FuzzyRuleAntecedent* ifKecepatanCepatAndJarakAman = new FuzzyRuleAntecedent();
ifKecepatanCepatAndJarakAman ->joinWithAND(Cepat, Aman);
//Fuzzy Rule9
FuzzyRuleAntecedent* ifKecepatanCepatAndJarakJauh = new FuzzyRuleAntecedent();
ifKecepatanCepatAndJarakJauh ->joinWithAND(Cepat, Jauh);
// Building FuzzyRule 11
FuzzyRuleAntecedent* ifDeviasiTengahAndKecepatanLambat = new FuzzyRuleAntecedent();
ifDeviasiTengahAndKecepatanLambat->joinWithAND(Tengah, Lambat);
// Building FuzzyRule 12
FuzzyRuleAntecedent* ifDeviasiKananAndKecepatanLambat = new FuzzyRuleAntecedent();
ifDeviasiKananAndKecepatanLambat->joinWithAND(Kanan, Lambat);
// Building FuzzyRule 13
FuzzyRuleAntecedent* ifDeviasiKiriAndKecepatanSedang = new FuzzyRuleAntecedent();
ifDeviasiKiriAndKecepatanSedang->joinWithAND(Kiri, Sedang);
// Building FuzzyRule 14
FuzzyRuleAntecedent* ifDeviasiTengahAndKecepatanSedang = new FuzzyRuleAntecedent();
ifDeviasiTengahAndKecepatanSedang->joinWithAND(Tengah, Sedang);
// 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);
// Building FuzzyRule 17
FuzzyRuleAntecedent* ifDeviasiTengahAndKecepatanCepat = new FuzzyRuleAntecedent();
ifDeviasiTengahAndKecepatanCepat->joinWithAND(Tengah, Cepat);
// Building FuzzyRule 18
FuzzyRuleAntecedent* ifDeviasiKananAndKecepatanCepat = new FuzzyRuleAntecedent();
ifDeviasiKananAndKecepatanCepat->joinWithAND(Kanan, Cepat);
//Fuzzy Rule19
FuzzyRuleAntecedent* ifJarakACCDekatACCAndErrorKecil = new FuzzyRuleAntecedent();
ifJarakACCDekatACCAndErrorKecil->joinWithAND(DekatACC, Kecil);
//Fuzzy Rule20
FuzzyRuleAntecedent* ifJarakACCAmanACCAndErrorLumayan = new
FuzzyRuleAntecedent();
ifJarakACCAmanACCAndErrorLumayan->joinWithAND(AmanACC, Lumayan);
//Fuzzy Rule21
FuzzyRuleAntecedent* ifJarakACCJauhACCAndErrorBesar = new FuzzyRuleAntecedent();
ifJarakACCJauhACCAndErrorBesar->joinWithAND(JauhACC, Besar);
//===========================================================//
//====================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);
Serial.print("Initializing SD Card...");
if (!SD.begin(chipSelect))
{ Serial.println("Card Failed, Or Not Present");
return;
}
Serial.println("card initialized.");
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;
}
}
//=======================================================//
//----------------------------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);
}
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);
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;
}
//===================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