/* LCD Library */
#include <LiquidCrystal_I2C.h> //include library LCD
LiquidCrystal_I2C lcd(0x27,20,4); //set the LCD address to 0x27 for a 16 chars and 2 line display
/* Servo Library */
#include <Servo.h> //include library servo
Servo servo_depan, servo_belakang; //buat objek untuk servo depan dan belakang
/* EzButton Library */
#include <ezButton.h>
ezButton button_mode(A8); //pin Button mode
ezButton button_kiri(A11); //pin Button kiri
ezButton button_maju(A9); //pin Button maju
ezButton button_mundur(A10); //pin Button mundur
ezButton button_kanan(A12); //pin Button kanan
/* LED Pinout */
const int pin_led_mode1 = 53; //pin LED mode 1 (merah)
const int pin_led_mode2 = 52; //pin LED mode 2 (kuning)
const int pin_led_mode3 = 51; //pin LED mode 3 (hijau)
/* Joystick Pinout */
const int pin_joy_vert = A7; //pin joystick vertikal
const int pin_joy_horz = A6; //pin joystick horizontal
/* Encoder Pinout */
const int pin_ENA_depan = 2; //pin encoder ENA depan
const int pin_ENB_depan = 3; //pin encoder ENB depan
const int pin_ENA_belakang = 18; //pin encoder ENA belakang
const int pin_ENB_belakang = 19; //pin encoder ENB belakang
/* Motor Pinout */
const int pin_PWM_depan = 4; //pin PWM motor depan
const int pin_RPWM_depan = 10; //pin RPWM motor depan
const int pin_LPWM_depan = 9; //pin LPWM motor depan
const int pin_PWM_belakang = 7; //pin PWM motor belakang
const int pin_RPWM_belakang = 8; //pin RPWM motor belakang
const int pin_LPWM_belakang = 6; //pin LPWM motor belakang
/* Button Variable */
int readbt_forward = LOW; //Variabel state button maju
int readbt_backward = LOW; //Variabel state button mundur
int readbt_left = LOW; //Variabel state button kiri
int readbt_right = LOW; //Variabel state button kanan
int readbt_mode = LOW; //Variabel state button mode
/* PID Depan */
int pos_depan, posPrev_depan; //Variabel pulse sekarang dan sebelumnya Depan
volatile int pos_depan_i = 0; //Variabel pulse mentah Depan
float v_depan, vPrev_depan; //Variabel RPM sebelum difilter Depan
float kp_depan, ki_depan, kd_depan; //Variabel Kp Kd Ki Motor Roda Depan
float error_depan, e_integral_depan, e_dot_depan, prev_error_depan; //Variabel error PID (error asli, error integral, error derivatif) Depan
float Setpoint_depan, Sensor_depan, Output_depan; //Variabel I/O PID (Setpoint, Input, Output) Depan
int speed_depan = 0; //Variabel PWM untuk Gerak Motor Depan
/* PID Belakang */
int pos_belakang, posPrev_belakang; //Variabel pulse sekarang dan sebelumnya Belakang
volatile int pos_belakang_i = 0; //Variabel pulse mentah Belakang
float v_belakang, vPrev_belakang; //Variabel RPM sebelum difilter Belakang
float kp_belakang, ki_belakang, kd_belakang; //Variabel Kp, Ki, Kd Motor Roda Belakang
float error_belakang, e_integral_belakang, e_dot_belakang, prev_error_belakang; //Variabel error PID (error asli, error integral, error derivatif) Belakang
float Setpoint_belakang, Sensor_belakang, Output_belakang; //Variabel I/O PID (Setpoint, Input, Output) Belakang
int speed_belakang = 0; //Variabel PWM untuk Gerak Motor Belakang
/* Button Hold */
int speed_hold = 0; //Variabel Tingkat Speed Hold
bool button_indicator1 = false; //Variabel Indikator Speed Hold
int direction_hold = 0; //Variabel Tingkat Turning Hold
bool button_indicator2 = false; //Variabel Indikator Speed Hold
/* Horizontal Joystick Variable */
float horz_read = 0; //Variabel reading joystick horizontal
float pothorzPrev = 512; //Variabel posisi joystick horz sebelumnya
float pothorzSmoothed = 512; //Variabel posisi joystick horz yang dismoothing
/* Vertical Joystick Variable */
float vert_read = 0; //Variabel reading joystick vertikal
float potvertPrev = 512; //Variabel posisi joystick vert sebelumnya
float potvertSmoothed = 512; //Variabel posisi joystick vert yang dismoothing
/* Other Variable */
int MODE = 1; //Variabel Indikator Mode
int degree = 90; //Variabel Sudut servo
float deltaT, prevT; //Variabel Waktu
void setup() { //SETUP
Serial.begin(115200);
setupled();
setupLCD();
setupbutton();
setupjoystick();
setupservo();
setupencoder();
setupmotor();
}
void loop() { //LOOP
button_mode.loop(); //Start Button Mode
button_maju.loop(); //Start Button Maju
button_mundur.loop(); //Start Button Mundur
button_kiri.loop(); //Start Button Kiri
button_kanan.loop(); //Start Button Kanan
if (button_indicator1 == false && button_indicator2 == false){ //Cek apakah mode button hold aktif, jika off lakukan perubahan mode saat tombol ditekan
MODE = button_mode.getCount(); //Fungsi pergantian mode dengan counting pressed button
if (MODE > 2){ //Cek jika counting bernilai diatas 2, reset counter
button_mode.resetCount(); //Fungsi Reset counter
}
}
mode_selector(); //Fungsi pergantian Mode
//debugger();
}
void mode_selector(){
switch(MODE){
case 0:
mode1(); //Fungsi Mode 1
lcd.setCursor(7,3); //Update tulisan mode 1 di LCD
lcd.print("MODE: 1");
digitalWrite(pin_led_mode1, HIGH); //Set LED 1 on, other off
digitalWrite(pin_led_mode2, LOW);
digitalWrite(pin_led_mode3, LOW);
break;
case 1:
mode2(); //Fungsi Mode 2
lcd.setCursor(7,3); //Update tulisan mode 2 di LCD
lcd.print("MODE: 2");
digitalWrite(pin_led_mode1, LOW); //Set LED 2 on, other off
digitalWrite(pin_led_mode2, HIGH);
digitalWrite(pin_led_mode3, LOW);
break;
case 2:
mode3(); //Fungsi Mode 3
lcd.setCursor(7,3); //Update tulisan mode 3 di LCD
lcd.print("MODE: 3");
digitalWrite(pin_led_mode1, LOW); //Set LED 3 on, other off
digitalWrite(pin_led_mode2, LOW);
digitalWrite(pin_led_mode3, HIGH);
break;
}
}
void speed_button_control(){
if(button_maju.isPressed()){ //Fungsi speed hold naik
if (speed_hold < 4){
speed_hold++;
}
}
else if(button_mundur.isPressed()){ //Fungsi speed hold turun
if (speed_hold > -4){
speed_hold--;
}
}
if(speed_hold != 0){ //Indikator jika mode speed hold aktif, joystick maju mundur akan off
button_indicator1 = true;
}
else{
button_indicator1 = false;
}
switch(speed_hold){ //Fungsi Tampilan LCD sesuai tingkatan speed hold
case 0:
// lcd.setCursor(0,1);
// lcd.print(" ");
// lcd.setCursor(0,2);
// lcd.print(" ");
STOP();
break;
case 1:
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,2);
lcd.print("\1 ");
SPEED1();
break;
case 2:
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,2);
lcd.print("\1\2 ");
SPEED2();
break;
case 3:
lcd.setCursor(0,1);
lcd.print(" \1 ");
lcd.setCursor(0,2);
lcd.print("\1\2\2 ");
SPEED3();
break;
case 4:
lcd.setCursor(0,1);
lcd.print(" \1\2");
lcd.setCursor(0,2);
lcd.print("\1\2\2\2");
SPEED4();
break;
case -1:
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,2);
lcd.print("\3 ");
SPEED1R();
break;
case -2:
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,2);
lcd.print("\3\4 ");
SPEED2R();
break;
case -3:
lcd.setCursor(0,1);
lcd.print(" \3 ");
lcd.setCursor(0,2);
lcd.print("\3\4\2 ");
SPEED3R();
break;
case -4:
lcd.setCursor(0,1);
lcd.print(" \3\4");
lcd.setCursor(0,2);
lcd.print("\3\4\2\2");
SPEED4R();
break;
}
}
void direction_button_control(){
if(button_kanan.isPressed()){ //Fungsi direction hold naik
if (direction_hold < 4){
direction_hold++;
}
}
else if(button_kiri.isPressed()){ //Fungsi direction hold turun
if (direction_hold > -4){
direction_hold--;
}
}
if(direction_hold != 0){ //Indikator jika mode direction hold aktif, joystick kiri kanan akan off
button_indicator2 = true;
}
else{
button_indicator2 = false;
}
switch(direction_hold){ //Fungsi tampilan LCD sesuai tingkatan direction hold
case 0:
degree = 90;
lcd.setCursor(15,1);
break;
case 1:
degree = 120;
lcd.setCursor(15,1);
lcd.print(degree);
break;
case 2:
degree = 135;
lcd.setCursor(15,1);
lcd.print(degree);
break;
case 3:
degree = 150;
lcd.setCursor(15,1);
lcd.print(degree);
break;
case 4:
degree = 180;
lcd.setCursor(15,1);
lcd.print(degree);
break;
case -1:
degree = 60;
lcd.setCursor(15,1);
lcd.print(" ");
lcd.print(degree);
break;
case -2:
degree = 45;
lcd.setCursor(15,1);
lcd.print(" ");
lcd.print(degree);
break;
case -3:
degree = 30;
lcd.setCursor(15,1);
lcd.print(" ");
lcd.print(degree);
break;
case -4:
degree = 0;
lcd.setCursor(15,1);
lcd.print(" ");
lcd.print(degree);
break;
}
}
void countPulse_depan(){
// Read encoder B when ENCA rises
int b1 = digitalRead(pin_ENB_depan);
int increment1 = 0;
if(b1>0){
// jika b positif, value terbaca nambah 1 ketika encoder ngedetect
increment1 = 1;
}
else{
// Otherwise, increment backward
increment1 = -1;
}
pos_depan_i = pos_depan_i + increment1;
}
void countPulse_belakang(){
// Read encoder B when ENCA rises
int b2 = digitalRead(pin_ENB_belakang);
int increment2 = 0;
if(b2>0){
// If B is high, increment forward
increment2 = 1;
}
else{
// Otherwise, increment backward
increment2 = -1;
}
pos_belakang_i = pos_belakang_i + increment2;
}
void debugger(){
if (button_mode.isPressed()){
Serial.println(MODE);
}
if (button_maju.isPressed()){
Serial.println("MAJU");
}
if (button_mundur.isPressed()){
Serial.println("MUNDUR");
}
if (button_kiri.isPressed()){
Serial.println("KIRI");
}
if (button_kanan.isPressed()){
Serial.println("KANAN");
}
}