robotarm

#include <Servo.h>

boolean drej1 = true;
boolean object = false;
Servo led1;
Servo led2;
Servo led3;

long duration1, cm1;
const int trigPin1 = 5; // Trigger Pin på Ultrasonic Sensor
const int echoPin1 = 4; // Echo Pin på Ultrasonic Sensor

int dstep = +5;
int step = 0;
int dstep1 = +5;
int step1 = 0;

void setup() {
  Serial.begin(9600);
  led1.attach(9);
  led2.attach(10);
  led3.attach(8);

}

void loop() {
  sensor1();

  if(drej1 == true){
    drej();
  }
 if(objectdetect() < 14 && objectdetect() > 3){
  drej1 = false;
  lift();
 }else{
  drej1 = true;
 }
 if(objectdetect() < 2){
  led2.write(90);

 }

  //Serial.println("Hello World \n");

}

   long microsecondsToCentimeters(long microseconds) {
   return microseconds / 29 / 2;
   }
void sensor1(){
   pinMode(trigPin1, OUTPUT);
   digitalWrite(trigPin1, LOW);
   delayMicroseconds(2);
   digitalWrite(trigPin1, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPin1, LOW);
   pinMode(echoPin1, INPUT);
   duration1 = pulseIn(echoPin1, HIGH);
   cm1 = microsecondsToCentimeters(duration1);
   Serial.print(cm1);
   Serial.print("cm sensor1");
   Serial.println();
   delay(100);
}

void drej(){
  step = step + dstep;
  if(step > 175 || step < 5){
    dstep = -dstep;
  }
    led3.write(step);
    delay(120);
}

void lift(){
  step1 = step1 + dstep1;
  if(step1 > 175 || step1 < 5){
    dstep1 = -dstep1;
  }
  led1.write(step1);
  delay(120);
}


int objectdetect(){
  return cm1;
}