#include <Wire.h>
#include <MPU6050.h>
#include <ESP32Servo.h>
#include <U8g2lib.h>
// ========= OLED SH1107 =========
U8G2_SH1107_SEEED_128X128_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);
// ========= MPU =========
MPU6050 mpu;
// ========= Servos =========
Servo rudderServo;
Servo shipServo;
Servo indicatorServo;
// ========= Pins =========
#define JOY_PIN 34
#define POT_PIN 35
#define MODE_PIN 26
#define AUTO_BTN 33
#define RESET_BTN 25
#define LED_LEFT 4
#define LED_RIGHT 5
#define BUZZER 13
// ========= Settings =========
int centerAngle = 90;
int rudderLimit = 35;
int autoLimit = 20;
float courseHeading = 0;
bool autoMode = false;
// ========= JOYSTICK RATE CONTROL =========
float commandedAngle = 90;
unsigned long lastMoveTime = 0;
const unsigned long moveInterval = 200; // 1 degree every 0.2 sec
// ======================================
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
rudderServo.attach(18);
shipServo.attach(19);
indicatorServo.attach(23);
pinMode(MODE_PIN, INPUT_PULLUP);
pinMode(AUTO_BTN, INPUT_PULLUP);
pinMode(RESET_BTN, INPUT_PULLUP);
pinMode(LED_LEFT, OUTPUT);
pinMode(LED_RIGHT, OUTPUT);
pinMode(BUZZER, OUTPUT);
// OLED init
u8g2.begin();
u8g2.clearBuffer();
u8g2.setFont(u8g2_font_6x12_tr);
u8g2.drawStr(0,15,"Ship Control System");
u8g2.drawStr(0,30,"Booting...");
u8g2.sendBuffer();
delay(1500);
}
// ======================================
void loop() {
bool useJoystick = digitalRead(MODE_PIN);
bool autoButtonPressed = !digitalRead(AUTO_BTN);
// ----- SET AUTO COURSE -----
if (autoButtonPressed) {
courseHeading = getHeading();
autoMode = true;
delay(400);
}
// ----- MANUAL CONTROL -----
if (!autoMode) {
// -------- JOYSTICK MODE --------
if (useJoystick == LOW) {
int joyValue = analogRead(JOY_PIN);
int deadZoneLow = 1800;
int deadZoneHigh = 2300;
unsigned long currentTime = millis();
if (currentTime - lastMoveTime >= moveInterval) {
if (joyValue < deadZoneLow) {
commandedAngle -= 1;
lastMoveTime = currentTime;
}
else if (joyValue > deadZoneHigh) {
commandedAngle += 1;
lastMoveTime = currentTime;
}
commandedAngle = constrain(commandedAngle,
centerAngle - rudderLimit,
centerAngle + rudderLimit);
}
}
// -------- POT MODE --------
else {
int potValue = analogRead(POT_PIN);
commandedAngle = map(potValue,
0, 4095,
centerAngle - rudderLimit,
centerAngle + rudderLimit);
}
}
bool resetPressed = !digitalRead(RESET_BTN);
if (!autoMode && resetPressed) {
// Smooth return to center
while (commandedAngle != centerAngle) {
if (commandedAngle > centerAngle)
commandedAngle--;
else if (commandedAngle < centerAngle)
commandedAngle++;
rudderServo.write(commandedAngle);
indicatorServo.write(commandedAngle);
delay(15); // smooth movement
}
}
// ----- AUTO MODE -----
else if (autoMode) {
float currentHeading = getHeading();
float error = courseHeading - currentHeading;
error = constrain(error, -autoLimit, autoLimit);
commandedAngle = centerAngle + error;
if (abs(error) > 60) {
tone(BUZZER, 2000);
} else {
noTone(BUZZER);
}
}
// Final safety constrain
commandedAngle = constrain(commandedAngle,
centerAngle - rudderLimit,
centerAngle + rudderLimit);
rudderServo.write(commandedAngle);
indicatorServo.write(commandedAngle);
simulateShip(commandedAngle);
updateLEDs(commandedAngle);
updateDisplay(getHeading(), commandedAngle, autoMode, useJoystick);
delay(20);
}
// ======================================
float getHeading() {
int16_t gx, gy, gz;
mpu.getRotation(&gx, &gy, &gz);
return gz / 131.0;
}
// ======================================
void simulateShip(int rudderAngle) {
static float shipAngle = 90;
float turnRate = (rudderAngle - 90) * 0.02;
shipAngle += turnRate;
shipAngle = constrain(shipAngle, 0, 180);
shipServo.write(shipAngle);
}
// ======================================
void updateLEDs(int commandedAngle) {
if (commandedAngle < centerAngle) {
digitalWrite(LED_RIGHT, HIGH);
digitalWrite(LED_LEFT, LOW);
}
else if (commandedAngle > centerAngle) {
digitalWrite(LED_LEFT, HIGH);
digitalWrite(LED_RIGHT, LOW);
}
else {
digitalWrite(LED_LEFT, LOW);
digitalWrite(LED_RIGHT, LOW);
}
}
// ======================================
void updateDisplay(float heading, int rudderAngle, bool autoMode, bool useJoystick) {
u8g2.clearBuffer();
u8g2.setFont(u8g2_font_6x12_tr);
char buffer[30];
sprintf(buffer, "Heading: %.1f", heading);
u8g2.drawStr(0,15,buffer);
sprintf(buffer, "Course : %.1f", courseHeading);
u8g2.drawStr(0,30,buffer);
sprintf(buffer, "Rudder : %d", rudderAngle - 90);
u8g2.drawStr(0,45,buffer);
if(autoMode)
u8g2.drawStr(0,60,"Mode: AUTO");
else if(useJoystick == LOW)
u8g2.drawStr(0,60,"Mode: JOYSTICK");
else
u8g2.drawStr(0,60,"Mode: POT");
u8g2.sendBuffer();
}-35
35
SHIP
Rudder
reset