#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <ESP32Servo.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
Servo Rservo;
Servo Lservo;
int serval_R;
int serval_L;
float GyroX;
float GyroY;
float GyroZ;
int horzval;
int vertval;
int duration;
int initial_state;
int SensPeriod;
unsigned long CurrMill;
unsigned long LastMill;
int countP;
float Pitch;
int countR;
float Roll;
int countY;
float Yaw;
#define SERPIN 2
#define TRIGPIN 18
#define ECHOPIN 5
#define HORZPIN 34
#define VERTPIN 35
void setup() {
Serial.begin(115200);
pinMode(TRIGPIN, OUTPUT);
pinMode(ECHOPIN, INPUT);
mpu.begin();
Rservo.setPeriodHertz(50);
Rservo.attach(SERPIN, 544, 2400);
Lservo.setPeriodHertz(50);
Lservo.attach(SERPIN, 544, 2400);
Rservo.write(90);
Lservo.write(90);
mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
initial_state = 0;
SensPeriod = 100;
LastMill = 0;
Pitch = 0;
Roll = 0;
Yaw = 0;
}
void loop() {
//getDistance();
getPitch();
//getRoll();
//getYaw();
//getJoyHorz();
//getJoyVert();
//delay(1000);
}
int getDistance() {
digitalWrite(TRIGPIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGPIN, LOW);
duration = pulseIn(ECHOPIN, HIGH);
Serial.print("Ultrasonic Distance: ");
Serial.println(duration / 58);
return duration;
}
int getPitch() {
sensors_event_t accel, gyro, temp;
mpu.getEvent(&accel, &gyro, &temp);
for(countP = 0; countP < 10; countP++) {
CurrMill = millis();
if(CurrMill - LastMill >= SensPeriod) {
GyroX += gyro.gyro.x;
LastMill = CurrMill;
}
Serial.println(countP);
if(countP == 9) {
GyroX /= 10;
GyroX *= 57.2958;
Pitch += GyroX;
if(Pitch > 359) {
Pitch -= 360;
}
else if(Pitch < 0) {
Pitch += 360;
}
Serial.print("GyroX: ");
Serial.print(GyroX);
Serial.print("\tPitch: ");
Serial.println(Pitch);
GyroX = 0;
}
}
return Pitch;
}
int getYaw() {
sensors_event_t accel, gyro, temp;
mpu.getEvent(&accel, &gyro, &temp);
for(countY = 0; countY < 10; countY++) {
GyroY += gyro.gyro.y;
delay(100);
if(countY == 9) {
GyroY /= 10;
GyroY *= 57.2958;
Yaw += GyroY;
if(Yaw > 359) {
Yaw -= 360;
}
else if(Yaw < 0) {
Yaw += 360;
}
Serial.print("GyroY: ");
Serial.print(GyroY);
Serial.print("\tYaw: ");
Serial.println(Yaw);
GyroY = 0;
}
}
return Yaw;
}
int getRoll() {
sensors_event_t accel, gyro, temp;
mpu.getEvent(&accel, &gyro, &temp);
for(countR = 0; countR < 10; countR++) {
GyroZ += gyro.gyro.z;
delay(100);
if(countR == 9) {
GyroZ /= 10;
GyroZ *= 57.2958;
Roll += GyroZ;
if(Roll > 359) {
Roll -= 360;
}
else if(Roll < 0) {
Roll += 360;
}
Serial.print("GyroZ: ");
Serial.print(GyroZ);
Serial.print("\tRoll: ");
Serial.println(Roll);
GyroZ = 0;
}
}
return Roll;
}
int getJoyHorz() {
horzval = analogRead(HORZPIN);
Serial.print("JoyHorz: ");
Serial.println(horzval);
return horzval;
}
int getJoyVert() {
vertval = analogRead(VERTPIN);
Serial.print("JoyVert: ");
Serial.println(vertval);
return vertval;
}