#include <Wire.h>
#include "BaseControl.h"
#include "ArmControl.h"
#include "SerialControl.h"

// Pin definitions
const int MOTOR1_IN1_PIN = 3;
const int MOTOR1_IN2_PIN = 4;
const int MOTOR2_IN3_PIN = 5;
const int MOTOR2_IN4_PIN = 6;
const int MOTOR1_EN_PIN = 9;
const int MOTOR2_EN_PIN = 10;

const int BASE_SERVO_PIN = 13;
const int SHOULDER_SERVO_PIN = 7;
const int ELBOW_SERVO_PIN = 8;
const int GRIPPER_SERVO_PIN = 11;

const int TRIG_PIN = 12;
const int ECHO_PIN = 2;

// Create instances of our control classes
BaseControl base(MOTOR1_IN1_PIN, MOTOR1_IN2_PIN, MOTOR2_IN3_PIN, MOTOR2_IN4_PIN, MOTOR1_EN_PIN, MOTOR2_EN_PIN, TRIG_PIN, ECHO_PIN);
ArmControl arm(BASE_SERVO_PIN, SHOULDER_SERVO_PIN, ELBOW_SERVO_PIN, GRIPPER_SERVO_PIN);
SerialControl serialControl(base, arm);
void setup() {
    Serial.begin(115200);
    arm.moveToHome(); // Initialize the arm to its home position
    base.setObstacleAvoidance(true);  // Enable obstacle avoidance by default
    Serial.println("Robot Control System Initialized");
    Serial.println("Robot is in standby mode. Obstacle avoidance is enabled.");
}

void loop() {
    base.update();
    arm.update();
    serialControl.update();
}