Heavy-Lift Tracked Drone Platform

#include <Arduino.h>
// ============================================================
//  Amphibious Tracked UGV — Main Controller Firmware
//  Board  : ESP32-S3-DevKitC-1
//  Author : Schematik
//  Purpose: Long-range 4G-telemetry cargo delivery UGV
//           with dual-track drive, ICE throttle, GPS nav,
//           9-DoF IMU, waterproof ultrasonics, bilge pump,
//           and engine-temp monitoring.
// ============================================================

// ---- Modem selection (must come before TinyGSM) -----------
#define TINY_GSM_MODEM_SIM7600
#include <TinyGsmClient.h>

#include <Wire.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_PWMServoDriver.h>
#include <OneWire.h>
#include <DallasTemperature.h>
#include <NewPing.h>
#include <TinyGPSPlus.h>
#include <HardwareSerial.h>

// ---- Pin definitions (match set_wiring exactly) -----------
#define IBT2_L_RPWM   1
#define IBT2_L_LPWM   2
#define IBT2_L_R_EN   3
#define IBT2_L_L_EN   4
#define IBT2_R_RPWM   5
#define IBT2_R_LPWM   6
#define IBT2_R_R_EN   7
#define IBT2_R_L_EN   14
#define SIM_PWRKEY    21
#define SIM_RESET     38
#define GPS_RX_PIN    39   // UART2 RX ← GPS TX
#define GPS_TX_PIN    40   // UART2 TX → GPS RX
#define IMU_INT       42
#define SONAR_F_TRIG  15
#define SONAR_F_ECHO  16
#define SONAR_L_TRIG  17
#define SONAR_L_ECHO  18
#define SONAR_R_TRIG  41
#define SONAR_R_ECHO  47
#define BILGE_RELAY   10
#define ONEWIRE_BUS   48

// ---- UART assignments ------------------------------------
#define SIM_SERIAL_NUM  1   // UART1 → SIM7600 (GPIO43/44)
#define GPS_SERIAL_NUM  2   // UART2 → NEO-M8N (GPIO39/40)

// ---- Obstacle thresholds (cm) ----------------------------
#define OBSTACLE_STOP_CM    40
#define OBSTACLE_STEER_CM   80
#define SONAR_MAX_CM       400

// ---- IBT-2 PWM channel parameters -----------------------
#define PWM_FREQ_HZ      15000  // 15 kHz motor PWM
#define PWM_RESOLUTION     8    // 8-bit: 0-255

// ---- PCA9685 servo channels (ICE throttle) --------------
#define PCA_THROTTLE_CH   0
#define SERVO_MIN_US    500
#define SERVO_MAX_US   2500
#define SERVO_IDLE_US   700   // ~idle throttle
#define SERVO_MAX_THR  2200   // full throttle

// ---- APN for SIM7600 (update for your carrier) ----------
#define MODEM_APN       "internet"
#define MODEM_USER      ""
#define MODEM_PASS      ""
#define TELEMETRY_HOST  "ugv.local"   // Replace with your server IP / MQTT broker
#define TELEMETRY_PORT  1883

// ---- Temperature safety ---------------------------------
#define ENGINE_TEMP_MAX_C  95.0f   // shut throttle above this
#define BILGE_TEMP_TRIG_C  70.0f   // not used for bilge; reserved

// ====== Object instantiation ============================

// UART ports

// Hoisted type definitions
struct Waypoint { double lat; double lon; };


// Forward declarations
void setTrack(int rpwmPin, int lpwmPin, int renPin, int lenPin, int speed);
void driveForward(int spd);
void driveReverse(int spd);
void driveStop();
void turnLeft(int spd);
void turnRight(int spd);
void setThrottle(int microseconds);
bool modemInit();
void sendTelemetry();
void updateGPS();
void updateIMU();
bool obstacleCheck();
float bearingTo(double lat2, double lon2);
float distanceTo(double lat2, double lon2);
void navigateWaypoints();
void updateBilge();

HardwareSerial SimSerial(SIM_SERIAL_NUM);
HardwareSerial GpsSerial(GPS_SERIAL_NUM);

// Modem & GSM client
TinyGsm       modem(SimSerial);
TinyGsmClient gsmClient(modem);

// GPS
TinyGPSPlus gps;

// IMU
Adafruit_ICM20948 icm;

// PCA9685
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

// OneWire / DS18B20
OneWire           oneWire(ONEWIRE_BUS);
DallasTemperature tempSensor(&oneWire);

// Ultrasonics
NewPing sonarFront(SONAR_F_TRIG, SONAR_F_ECHO, SONAR_MAX_CM);
NewPing sonarLeft (SONAR_L_TRIG, SONAR_L_ECHO, SONAR_MAX_CM);
NewPing sonarRight(SONAR_R_TRIG, SONAR_R_ECHO, SONAR_MAX_CM);

// ====== State variables ==================================
float  engineTempC     = 0.0f;
float  imuRoll         = 0.0f;
float  imuPitch        = 0.0f;
double gpsLat          = 0.0;
double gpsLon          = 0.0;
float  gpsSpeedKmh     = 0.0f;
int    distFront       = SONAR_MAX_CM;
int    distLeft        = SONAR_MAX_CM;
int    distRight       = SONAR_MAX_CM;
bool   bilgeActive     = false;
bool   modemConnected  = false;

// Waypoint list (lat/lon pairs — extend as needed)

const Waypoint WAYPOINTS[] = {
    {48.8588443,  2.2943506},   // Example: Eiffel Tower
    {48.8606111,  2.3354553},   // Example: Louvre
};
const int   NUM_WAYPOINTS = sizeof(WAYPOINTS) / sizeof(WAYPOINTS[0]);
int         currentWaypoint = 0;
bool        autonomousMode  = false;

// ====== IBT-2 helper functions ===========================

// Set track speed: speed -255 (full reverse) to +255 (full forward)
void setTrack(int rpwmPin, int lpwmPin, int renPin, int lenPin, int speed) {
    digitalWrite(renPin, HIGH);
    digitalWrite(lenPin, HIGH);
    if (speed > 0) {
        ledcWrite(rpwmPin, speed);
        ledcWrite(lpwmPin, 0);
    } else if (speed < 0) {
        ledcWrite(rpwmPin, 0);
        ledcWrite(lpwmPin, -speed);
    } else {
        ledcWrite(rpwmPin, 0);
        ledcWrite(lpwmPin, 0);
    }
}

void driveForward(int spd)  { setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN,  spd);
                               setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN,  spd); }
void driveReverse(int spd)  { setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN, -spd);
                               setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN, -spd); }
void driveStop()            { setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN,  0);
                               setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN,  0); }
void turnLeft(int spd)      { setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN, -spd);
                               setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN,  spd); }
void turnRight(int spd)     { setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN,  spd);
                               setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN, -spd); }

// ====== ICE Throttle via PCA9685 =========================
void setThrottle(int microseconds) {
    microseconds = constrain(microseconds, SERVO_MIN_US, SERVO_MAX_US);
    int pulse = map(microseconds, 0, 20000, 0, 4096);
    pwm.setPWM(PCA_THROTTLE_CH, 0, pulse);
}

// ====== Modem setup =====================================
bool modemInit() {
    Serial.println("[LTE] Powering modem...");
    pinMode(SIM_PWRKEY, OUTPUT);
    pinMode(SIM_RESET,  OUTPUT);
    digitalWrite(SIM_RESET, LOW);
    digitalWrite(SIM_PWRKEY, LOW);
    delay(1000);
    digitalWrite(SIM_PWRKEY, HIGH);
    delay(2000);
    digitalWrite(SIM_PWRKEY, LOW);
    delay(3000);

    SimSerial.begin(115200, SERIAL_8N1, 44, 43); // RX=GPIO44, TX=GPIO43
    delay(500);
    if (!modem.restart()) {
        Serial.println("[LTE] Modem restart failed");
        return false;
    }
    Serial.print("[LTE] Modem: ");
    Serial.println(modem.getModemInfo());

    if (!modem.waitForNetwork(30000)) {
        Serial.println("[LTE] Network timeout");
        return false;
    }
    if (!modem.gprsConnect(MODEM_APN, MODEM_USER, MODEM_PASS)) {
        Serial.println("[LTE] GPRS connect failed");
        return false;
    }
    Serial.print("[LTE] IP: ");
    Serial.println(modem.localIP());
    return true;
}

// ====== Telemetry JSON uplink ===========================
void sendTelemetry() {
    if (!modemConnected) return;
    if (!gsmClient.connect(TELEMETRY_HOST, TELEMETRY_PORT)) {
        Serial.println("[TEL] Server connect failed");
        return;
    }
    char payload[320];
    snprintf(payload, sizeof(payload),
        "{\"lat\":%.6f,\"lon\":%.6f,\"spd\":%.1f,"
        "\"roll\":%.1f,\"pitch\":%.1f,"
        "\"distF\":%d,\"distL\":%d,\"distR\":%d,"
        "\"tempC\":%.1f,\"bilge\":%d,\"wp\":%d}",
        gpsLat, gpsLon, gpsSpeedKmh,
        imuRoll, imuPitch,
        distFront, distLeft, distRight,
        engineTempC, bilgeActive ? 1 : 0, currentWaypoint
    );
    // Simple raw TCP JSON push (replace with MQTT publish for production)
    gsmClient.print(payload);
    gsmClient.stop();
    Serial.print("[TEL] Sent: "); Serial.println(payload);
}

// ====== GPS update loop =================================
void updateGPS() {
    while (GpsSerial.available()) {
        if (gps.encode(GpsSerial.read())) {
            if (gps.location.isValid()) {
                gpsLat      = gps.location.lat();
                gpsLon      = gps.location.lng();
                gpsSpeedKmh = gps.speed.kmph();
            }
        }
    }
}

// ====== IMU update ======================================
void updateIMU() {
    sensors_event_t accel, gyro, mag, temp;
    icm.getEvent(&accel, &gyro, &temp, &mag);
    // Simple roll/pitch from accelerometer
    imuRoll  = atan2(accel.acceleration.y, accel.acceleration.z) * 180.0f / PI;
    imuPitch = atan2(-accel.acceleration.x,
                     sqrt(accel.acceleration.y * accel.acceleration.y +
                          accel.acceleration.z * accel.acceleration.z)) * 180.0f / PI;
}

// ====== Obstacle avoidance ==============================
bool obstacleCheck() {
    distFront = sonarFront.ping_cm();
    distLeft  = sonarLeft.ping_cm();
    distRight = sonarRight.ping_cm();
    if (distFront == 0) distFront = SONAR_MAX_CM; // 0 means out of range
    if (distLeft  == 0) distLeft  = SONAR_MAX_CM;
    if (distRight == 0) distRight = SONAR_MAX_CM;

    if (distFront < OBSTACLE_STOP_CM) {
        driveStop();
        Serial.printf("[OBS] STOP — front=%dcm\n", distFront);
        delay(300);
        driveReverse(120);
        delay(600);
        driveStop();
        // Steer around obstacle
        if (distLeft > distRight) turnLeft(150);
        else                      turnRight(150);
        delay(800);
        driveStop();
        return true;
    }
    if (distFront < OBSTACLE_STEER_CM) {
        if (distLeft > distRight) {
            setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN, 180);
            setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN, 100);
        } else {
            setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN, 100);
            setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN, 180);
        }
        return true;
    }
    return false;
}

// ====== Waypoint navigation =============================
// Returns bearing in degrees from current position to target
float bearingTo(double lat2, double lon2) {
    double dLon = (lon2 - gpsLon) * DEG_TO_RAD;
    double lat1r = gpsLat * DEG_TO_RAD;
    double lat2r = lat2  * DEG_TO_RAD;
    double x = sin(dLon) * cos(lat2r);
    double y = cos(lat1r) * sin(lat2r) -
               sin(lat1r) * cos(lat2r) * cos(dLon);
    float bearing = atan2(x, y) * RAD_TO_DEG;
    return fmod(bearing + 360.0f, 360.0f);
}

float distanceTo(double lat2, double lon2) {
    // Haversine distance in metres
    const float R = 6371000.0f;
    float dlat = (lat2 - gpsLat) * DEG_TO_RAD;
    float dlon = (lon2 - gpsLon) * DEG_TO_RAD;
    float a = sin(dlat/2)*sin(dlat/2) +
              cos(gpsLat*DEG_TO_RAD)*cos(lat2*DEG_TO_RAD)*
              sin(dlon/2)*sin(dlon/2);
    return R * 2.0f * atan2(sqrt(a), sqrt(1.0f-a));
}

void navigateWaypoints() {
    if (currentWaypoint >= NUM_WAYPOINTS) {
        driveStop();
        setThrottle(SERVO_IDLE_US);
        Serial.println("[NAV] All waypoints reached.");
        return;
    }
    float dist = distanceTo(WAYPOINTS[currentWaypoint].lat,
                            WAYPOINTS[currentWaypoint].lon);
    if (dist < 2.0f) {
        Serial.printf("[NAV] Waypoint %d reached\n", currentWaypoint);
        currentWaypoint++;
        return;
    }
    if (!obstacleCheck()) {
        float hdg = gps.course.isValid() ? gps.course.deg() : 0;
        float target = bearingTo(WAYPOINTS[currentWaypoint].lat,
                                 WAYPOINTS[currentWaypoint].lon);
        float err = target - hdg;
        if (err > 180) err -= 360;
        if (err < -180) err += 360;

        int spd = 200;
        if (abs(err) > 20) {
            int steer = constrain((int)(abs(err) * 1.5f), 60, 200);
            if (err > 0) {
                setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN, spd);
                setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN, spd - steer);
            } else {
                setTrack(IBT2_L_RPWM, IBT2_L_LPWM, IBT2_L_R_EN, IBT2_L_L_EN, spd - steer);
                setTrack(IBT2_R_RPWM, IBT2_R_LPWM, IBT2_R_R_EN, IBT2_R_L_EN, spd);
            }
        } else {
            driveForward(spd);
        }
        setThrottle(SERVO_MAX_THR);
    }
}

// ====== Bilge logic =====================================
void updateBilge() {
    // Simple timer-based bilge: run pump 5s every 2 minutes during water mode
    static unsigned long lastBilge = 0;
    static bool pumping = false;
    unsigned long now = millis();
    if (!pumping && (now - lastBilge > 120000UL)) {
        pumping = true;
        lastBilge = now;
        digitalWrite(BILGE_RELAY, LOW); // Active-low relay ON
        bilgeActive = true;
        Serial.println("[BILGE] Pump ON");
    }
    if (pumping && (now - lastBilge > 5000UL)) {
        pumping = false;
        digitalWrite(BILGE_RELAY, HIGH); // Relay OFF
        bilgeActive = false;
        Serial.println("[BILGE] Pump OFF");
    }
}

// ====== Setup ===========================================
void setup() {
    Serial.begin(115200);
    Serial.println("\n=== Amphibious UGV Controller ===");

    // Track motor PWM outputs
    ledcAttach(IBT2_L_RPWM, PWM_FREQ_HZ, PWM_RESOLUTION);
    ledcAttach(IBT2_L_LPWM, PWM_FREQ_HZ, PWM_RESOLUTION);
    ledcAttach(IBT2_R_RPWM, PWM_FREQ_HZ, PWM_RESOLUTION);
    ledcAttach(IBT2_R_LPWM, PWM_FREQ_HZ, PWM_RESOLUTION);
    pinMode(IBT2_L_R_EN, OUTPUT); digitalWrite(IBT2_L_R_EN, HIGH);
    pinMode(IBT2_L_L_EN, OUTPUT); digitalWrite(IBT2_L_L_EN, HIGH);
    pinMode(IBT2_R_R_EN, OUTPUT); digitalWrite(IBT2_R_R_EN, HIGH);
    pinMode(IBT2_R_L_EN, OUTPUT); digitalWrite(IBT2_R_L_EN, HIGH);
    driveStop();

    // Bilge relay (active-low, default OFF)
    pinMode(BILGE_RELAY, OUTPUT);
    digitalWrite(BILGE_RELAY, HIGH);

    // I2C bus
    Wire.begin(8, 9); // SDA=8, SCL=9

    // IMU
    if (!icm.begin_I2C()) {
        Serial.println("[IMU] ICM-20948 not found!");
    } else {
        Serial.println("[IMU] ICM-20948 OK");
    }

    // PCA9685
    pwm.begin();
    pwm.setOscillatorFrequency(27000000);
    pwm.setPWMFreq(50); // 50Hz for servo
    setThrottle(SERVO_IDLE_US);
    Serial.println("[PCA] PCA9685 OK — throttle idle");

    // DS18B20
    tempSensor.begin();
    Serial.printf("[TEMP] Found %d sensor(s)\n", tempSensor.getDeviceCount());

    // GPS UART2
    GpsSerial.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
    Serial.println("[GPS] NEO-M8N UART started");

    // 4G modem
    modemConnected = modemInit();
    if (modemConnected) {
        Serial.println("[LTE] 4G connected");
    } else {
        Serial.println("[LTE] Running without 4G (local mode)");
    }

    // Enable autonomous navigation
    autonomousMode = true;
    Serial.println("[SYS] Ready — autonomous mode ON");
}

// ====== Main loop =======================================
void loop() {
    static unsigned long lastTelemetry  = 0;
    static unsigned long lastTempRead   = 0;
    static unsigned long lastImuRead    = 0;

    unsigned long now = millis();

    // 1. GPS feed
    updateGPS();

    // 2. IMU @ 50Hz
    if (now - lastImuRead > 20) {
        updateIMU();
        lastImuRead = now;
        // Safety: tilt kill-switch (>45° tilt = emergency stop)
        if (abs(imuRoll) > 45.0f || abs(imuPitch) > 45.0f) {
            driveStop();
            setThrottle(SERVO_IDLE_US);
            Serial.printf("[SAFETY] Tilt! roll=%.1f pitch=%.1f\n", imuRoll, imuPitch);
        }
    }

    // 3. Engine temperature @ 2Hz
    if (now - lastTempRead > 500) {
        tempSensor.requestTemperatures();
        engineTempC = tempSensor.getTempCByIndex(0);
        lastTempRead = now;
        if (engineTempC > ENGINE_TEMP_MAX_C) {
            setThrottle(SERVO_IDLE_US);
            driveStop();
            Serial.printf("[SAFETY] Overheat! %.1f°C — throttle cut\n", engineTempC);
        }
    }

    // 4. Bilge pump cycling
    updateBilge();

    // 5. Autonomous navigation
    if (autonomousMode && gps.location.isValid()) {
        navigateWaypoints();
    }

    // 6. Telemetry uplink @ 5s
    if (now - lastTelemetry > 5000) {
        sendTelemetry();
        lastTelemetry = now;
        Serial.printf("[STATUS] GPS %.6f,%.6f | Temp %.1f°C | F:%dcm L:%dcm R:%dcm\n",
                      gpsLat, gpsLon, engineTempC, distFront, distLeft, distRight);
    }
}