#include "Wire.h"
#include <MPU6050_light.h>
#include <Servo.h>
#include <BH1750.h>
#define ECHOPIN 3
#define TRIGPIN 4
MPU6050 mpu(Wire);
BH1750 lightMeter(0x23); //I2C Address
Servo myservoX, myservoY; // Roll/Pitch
int trimpitch = 90, trimyaw = 90; //Kondisi Awal
int pitchout, yawout, yawout1;
uint8_t jarak;
char inByte = "";
byte IN4 = 12, IN3 = 11, IN2 = 10, IN1 = 9, ENA = 9, ENB = 10;
bool FlagPing = false, FlagMaju = false;
int FlagIn = 0;
unsigned long previousMillis = 0;
const long interval = 50;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
//Serial.println("test1234");
Wire.begin();
lightMeter.begin();
myservoY.attach(6); // Attach Y servo to pin 9
myservoX.attach(5); // Attach X servo to pin 10
myservoY.write(90); //Kalibrasi
myservoX.write(90); //Kalibrasi
byte status = mpu.begin();
delay(1000);
mpu.calcOffsets(); // gyro and accelero(Zero)
//init1();
pinMode(IN4, OUTPUT); // in4 12
pinMode(IN3, OUTPUT); // in3 11
pinMode(IN2, OUTPUT); // in2 10
pinMode(IN1, OUTPUT); // in1 9
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(13, OUTPUT); // in1 9
pinMode(ECHOPIN, INPUT);
pinMode(TRIGPIN, OUTPUT);
}
void loop() {
// put your main code here, to run repeatedly:
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
//directionControl();
readMPU();
actServo();
//readBH1750();
readPing();
}
if (Serial.available()) {
char ch = Serial.read();
actMotor(ch);
}
}