0

I am building a robot arm with 6 servo motors that works with an ultrasonic sensor. The purpose of the program is when an object is detected at 20 cm (fixed position) from the sensor, the buzzer and led turn on. After the buzzer turns on, the arm moves to that fixed position, picks up the object and places it at another fixed point.

How can I make multiple loops so that the sensor first detects an object then the arm moves to the fixed position, places the object and then returns to the original position.

I would really appreciate all the help I can get !!!

This is what I have so far in the program:

// include servo library 

#include <Servo.h>

// define pin numbers for the utltrasonic sensor 

const int trigPin = 25; // pin number for trig pin
const int echoPin = 24; // pin number for echo pin
const int buzzer = 32; // pin number for buzzer
const int ledPin = 30; // pin number for led
long duration; // duration storage
int distance; // distance storage
int safetyDistance; // safety distance storage

int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // servo positions storage


// define variable for servo positions

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;


void setup() {
  pinMode (trigPin, OUTPUT);
  pinMode(echoPin, INPUT); 
  pinMode(buzzer, OUTPUT); 
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);

  servo1.attach(6);
  //servo2.attach(7);
  //servo3.attach(8);
  //servo4.attach(9);
  //servo5.attach(10);
  //servo6.attach(11);
  delay(3000);

  // Robot initial position

  servo1Pos = 20; 
  servo2Pos = 100;
  servo3Pos = 35;
  servo4Pos = 110;
  servo5Pos = 85;
  servo6Pos = 80;

}

void loop() {

  // clears the trig pin

  digitalWrite (trigPin, LOW);
  delayMicroseconds (2);

  // sets trig pin on HIGH state for 10 microseconds

  digitalWrite (trigPin, HIGH);
  delayMicroseconds (10);
  digitalWrite (trigPin, LOW);

  // reads echo pin and stores the value in duration variable

  duration = pulseIn (echoPin, HIGH);

  // distance is calculated

  distance = duration*0.034/2;

  // distance is stored in safetyDistane variable

  safetyDistance = distance;

  if (safetyDistance <=20) {

    digitalWrite (buzzer, HIGH); // the buzzer turns on if the distance is less than 5 cm
    digitalWrite (ledPin, HIGH);
    delay (1000); 
    digitalWrite (buzzer, LOW); // the buzzer turns off after 5 sec
    delay (1000);

    for (servo1Pos = 20; servo1Pos <= 90; servo1Pos +=1)
    {
      servo1.write (servo1Pos);
      delay (500);
    }
    for (servo2Pos = 100; servo2Pos <= 180; servo2Pos +=1)
    {
      servo2.write (servo2Pos);
      delay (500);
    }
    //for (servo3Pos = 35; servo3Pos <= 90; servo3Pos +=1)
    {
      //servo3.write (servo3Pos);
      //delay (500);
    }
    //for (servo4Pos = 120; servo4Pos <= 180; servo4Pos +=1)
    {
      //servo4.write (servo4Pos);
      //delay (500);
    }
    //for (servo5Pos = 85; servo5Pos <= 90; servo5Pos +=1)
    {
      //servo5.write (servo5Pos);
      //delay (500);
    }
    //for (servo6Pos = 80; servo6Pos <= 90; servo6Pos +=1)
    {
      //servo6.write (servo6Pos);
      //delay (500);
    }

    delay (1000);

    for (servo1Pos = 20; servo1Pos >= 90; servo1Pos -=1)
    {
      servo1.write (servo1Pos);
      delay (500);
    }
    for (servo2Pos = 100; servo2Pos >= 180; servo2Pos -=1)
    {
      servo2.write (servo2Pos);
      delay (500);
    }
    //for (servo3Pos = 35; servo3Pos >= 90; servo3Pos -=1)
    {
      //servo3.write (servo3Pos);
      //delay (500);
    }
    //for (servo4Pos = 120; servo4Pos >= 180; servo4Pos -=1)
    {
      //servo4.write (servo4Pos);
      //delay (500);
    }
    //for (servo5Pos = 85; servo5Pos >= 90; servo5Pos -=1)
    {
      //servo5.write (servo5Pos);
      //delay (500);
    }
    //for (servo6Pos = 80; servo6Pos >= 90; servo6Pos -=1)
    {
      //servo6.write (servo6Pos);
      //delay (500);
    }

    delay (1000);

  }

} 
  • please use the {} button to format your code – jsotola Mar 31 at 4:15
  • why are you asking for help? .... you have not said that there is any problem ..... you have not asked a question either – jsotola Mar 31 at 4:24
  • use a state variable to handle the states as you described them – Juraj Mar 31 at 7:16
  • If ultrasonic sensor does not work out consider using a time of flight sensor VL53L0X. – Eric Rohlfs Mar 31 at 18:44

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.