/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: Autonomous Grabber
	- Source Code NOT compiled for: Arduino Uno
	- Source Code created on: 2025-11-24 15:25:17

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* Include a feature to calibrate the servo's range, */
	/* enabling adaptive operation for different attached */
	/* mechanisms, utilizing the existing Servo library */
	/* and Arduino Uno, with a simple start/stop */
	/* calibration interface. */
/****** END SYSTEM REQUIREMENTS *****/


/* START CODE */

#include 

// ------------------- PIN CONFIGURATION -------------------
const int LEFT_IN1  = 2;
const int LEFT_IN2  = 3;
const int LEFT_EN   = 5;
const int RIGHT_IN1 = 4;
const int RIGHT_IN2 = 7;
const int RIGHT_EN  = 6;
const int SERVO_CLAW_PIN = 9;
const int SERVO_ARM_PIN  = 10;
const int SERVO_BASE_PIN = 11;
const int TRIG_PIN = A0;
const int ECHO_PIN = A1;

// ------------------- SERVO POSITIONS (adjust/calibrate) -------------------
const int CLAW_OPEN_ANGLE   = 10;
const int CLAW_CLOSED_ANGLE = 70;
const int ARM_UP_ANGLE   = 25;
const int ARM_DOWN_ANGLE = 100;
const int BASE_HOME_ANGLE   = 90;
const int BASE_PICKUP_ANGLE = 90;

// ------------------- MOTION & SENSOR PARAMETERS -------------------
const int SPIN_SPEED     = 160;
const int APPROACH_SPEED = 200;
const int BACK_SPEED     = 200;

const float DETECTION_DISTANCE = 40.0;
const float PICKUP_DISTANCE    = 12.0;

// ------------------- GLOBAL OBJECTS -------------------
Servo servoClaw;
Servo servoArm;
Servo servoBase;

// ------------------- HELPER FUNCTIONS -------------------

long readUltrasonicCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  if (duration == 0) return -1;
  long cm = duration / 58;
  return cm;
}

// Set left motors speed: positive -> forward, negative -> backward, 0 -> stop
void leftMotor(int speed) {
  if (speed > 0) {
    digitalWrite(LEFT_IN1, HIGH);
    digitalWrite(LEFT_IN2, LOW);
    analogWrite(LEFT_EN, constrain(speed, 0, 255));
  } else if (speed < 0) {
    digitalWrite(LEFT_IN1, LOW);
    digitalWrite(LEFT_IN2, HIGH);
    analogWrite(LEFT_EN, constrain(-speed, 0, 255));
  } else {
    digitalWrite(LEFT_IN1, LOW);
    digitalWrite(LEFT_IN2, LOW);
    analogWrite(LEFT_EN, 0);
  }
}

// Set right motors speed: positive -> forward, negative -> backward, 0 -> stop
void rightMotor(int speed) {
  if (speed > 0) {
    digitalWrite(RIGHT_IN1, HIGH);
    digitalWrite(RIGHT_IN2, LOW);
    analogWrite(RIGHT_EN, constrain(speed, 0, 255));
  } else if (speed < 0) {
    digitalWrite(RIGHT_IN1, LOW);
    digitalWrite(RIGHT_IN2, HIGH);
    analogWrite(RIGHT_EN, constrain(-speed, 0, 255));
  } else {
    digitalWrite(RIGHT_IN1, LOW);
    digitalWrite(RIGHT_IN2, LOW);
    analogWrite(RIGHT_EN, 0);
  }
}

// High-level movement helpers:
void moveForward(int speed) {
  leftMotor(speed);
  rightMotor(speed);
}

void moveBackward(int speed) {
  leftMotor(-speed);
  rightMotor(-speed);
}

// Spin in place. If clockwise==true, spins clockwise, else counter-clockwise.
void spinInPlace(int speed, bool clockwise) {
  if (clockwise) {
    leftMotor(speed);
    rightMotor(-speed);
  } else {
    leftMotor(-speed);
    rightMotor(speed);
  }
}

void stopMotors() {
  leftMotor(0);
  rightMotor(0);
}

// Pickup sequence performed after robot is stopped close to object
void performPickupSequence() {
  servoBase.write(BASE_PICKUP_ANGLE);
  delay(600);
  servoArm.write(ARM_DOWN_ANGLE);
  delay(700);
  servoClaw.write(CLAW_CLOSED_ANGLE);
  delay(700);
  servoArm.write(ARM_UP_ANGLE);
  delay(700);
}

// ------------------- SETUP -------------------
void setup() {
  pinMode(LEFT_IN1, OUTPUT);
  pinMode(LEFT_IN2, OUTPUT);
  pinMode(LEFT_EN, OUTPUT);
  pinMode(RIGHT_IN1, OUTPUT);
  pinMode(RIGHT_IN2, OUTPUT);
  pinMode(RIGHT_EN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(9600);
  servoClaw.attach(SERVO_CLAW_PIN);
  servoArm.attach(SERVO_ARM_PIN);
  servoBase.attach(SERVO_BASE_PIN);
  servoBase.write(BASE_HOME_ANGLE);
  servoArm.write(ARM_UP_ANGLE);
  servoClaw.write(CLAW_OPEN_ANGLE);
  delay(500);}

// ------------------- MAIN LOOP -------------------
void loop() {
  Serial.println("Spinning and scanning for object...");
  while (true) {
    spinInPlace(SPIN_SPEED, true);
    delay(120);
    long dist = readUltrasonicCM();
    if (dist > 0) {
      Serial.print("Distance: ");
      Serial.print(dist);
      Serial.println(" cm");
      if (dist <= DETECTION_DISTANCE) {
        Serial.println("Object detected within detection range. Stopping spin.");
        stopMotors();
        delay(200);
        break;}
    } else {
      Serial.println("No echo");}
  }
  Serial.println("Approaching object...");
  while (true) {
    moveForward(APPROACH_SPEED);
    delay(150);
    long dist = readUltrasonicCM();
    if (dist > 0) {
      Serial.print("Approach distance: ");
      Serial.print(dist);
      Serial.println(" cm");
      if (dist <= PICKUP_DISTANCE) {
        Serial.println("Close enough for pickup. Stopping.");
        stopMotors();
        delay(250);
        break;}
    } else {
      Serial.println("No echo while approaching (continuing).");}
  }
  Serial.println("Performing pickup sequence...");
  performPickupSequence();
  Serial.println("Backing away...");
  moveBackward(BACK_SPEED);
  delay(800);
  stopMotors();
  Serial.println("Pickup complete. Entering idle state.");
  while (true) {
    delay(1000);}
}

/* END CODE */