Robot arm with led blink warning before it moves and button

johnathonj · April 10, 2024, 1:23am

This is code with a robot arm, button, break beam sensors and led. When the button is pressed once it turns the robot on (not really needed) after the second button push the robot is in cycle mode and after the third button push the robot arm is turned off. Before the robot arm moves the led blinks as a warning then stays on while the arm is moving and goes off when it stops. In cycle mode the robot arm is trigged by a break beam sensor as if to pick up the object and will do that repeatedly. There are movements timed with millis and the movements are just simple one to see it working.

#include <VarSpeedServo.h>
const byte SENSORPIN1 = 1;
const byte SENSORPIN2 = 2;
const byte BUTTONPIN = 9;
const byte LEDPIN = 3;

VarSpeedServo servo1;
VarSpeedServo servo2;
VarSpeedServo servo3;
VarSpeedServo servo4;
VarSpeedServo servo5;

boolean blinkState = LOW;
boolean ledState = LOW;
boolean ledDone;
boolean event;
boolean robotCycle;
boolean robotWarning;
boolean robotEnable;
boolean robotMove;

int sensorState1 = 0, lastState1 = 0;
int sensorState2 = 0, lastState2 = 0;
int buttonPushCounter = 0;
int buttonState = 0;
int lastButtonState = 0;
int robotMoveCount = 0;
int robotMoveDone;

unsigned long robotWarningTime;
unsigned long debounceDuration = 50;
unsigned long lastButtonStateChange = 0;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
unsigned long time1 = 0;
unsigned long time2 = 0;
unsigned long time3 = 0;

const long interval = 100;

void setup() {
  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(7);
  servo5.attach(8);
  pinMode(BUTTONPIN, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  pinMode(SENSORPIN1, INPUT_PULLUP);
  pinMode(SENSORPIN2, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  blinkState = HIGH;
  event = true;
  robotMove = true;
  ledDone = true;
  robotWarning = false;
}

void loop() {
  currentMillis = millis();
  control_switch();
  led_blink();
  robot();
}

void control_switch() {
  sensorState1 = digitalRead(SENSORPIN1);
  sensorState2 = digitalRead(SENSORPIN2);
  buttonState = digitalRead(BUTTONPIN);
  if (millis() - lastButtonStateChange >= debounceDuration) {
    if (buttonState != lastButtonState) {
      lastButtonStateChange = millis();
      if (buttonState == LOW) {
        buttonPushCounter++;
      }
      lastButtonState = buttonState;
    }
  }
  if (buttonPushCounter == 1) {
    robotMoveCount = 1;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    buttonPushCounter = 2;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if (((buttonPushCounter == 3) || (robotCycle == true)) && (ledDone == true)
      && (sensorState1 == LOW) && (sensorState2 == HIGH)) {
    robotMoveCount = 2;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    buttonPushCounter = 4;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if (buttonPushCounter == 5) {
    robotMoveCount = 3;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    robotCycle = false;
    buttonPushCounter = 6;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if ((currentMillis - robotWarningTime >= 1000) && (ledDone == false)) {
    blinkState = HIGH;
    event = false;
    digitalWrite(LEDPIN, HIGH);
    robotWarning = true;
  }
  if (robotEnable == false) {
    digitalWrite(LEDPIN, LOW);
    event = true;
    ledDone = true;
    robotWarning = false;
  }
}

void led_blink() {

  if (event == true) {
    if (blinkState == LOW) {
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        if (ledState == LOW) {
          ledState = HIGH;

        } else {
          ledState = LOW;
        }
        digitalWrite(LEDPIN, ledState);
      }
    }
  }
}

void robot() {
  if ((robotMoveCount == 1) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85, 70, true);
    servo2.write(140, 70, true);
    servo3.write(120, 70, true);
    servo4.write(100, 70, true);
    servo5.write(110, 70, true);
    robotEnable = false;
  }
  if ((robotMoveCount == 2) && (robotEnable == true) && (robotWarning == true)) {
    if (robotMove == true) {
      servo4.write(40, 70, true);
      servo5.write(50, 70, true);
      servo4.write(120, 70, true);
      servo5.write(90, 70, true);
      servo4.write(100, 70, true);
      time1 = currentMillis;
      robotMove = false;
      robotMoveDone = 1;
    }
    if (robotMoveDone == 1) {
      if (currentMillis - time1 >= 2000) {
        servo1.write(50, 60, true);
        servo2.write(130, 60, true);
        servo3.write(60, 60, true);
        servo5.write(150, 70, true);
        time2 = currentMillis;
        robotMoveDone = 2;
      }
    }
    if (robotMoveDone == 2) {
      if (currentMillis - time2 >= 2000) {
        servo1.write(85, 60, true);
        servo2.write(160, 60, true);
        servo3.write(170, 60, true);
        servo5.write(90, 70, true);
        robotMoveDone = 0;
        robotMove = true;
        robotEnable = false;
        robotCycle = true;
      }
    }
  }
  if ((robotMoveCount == 3) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85, 60);
    servo2.write(160, 60, true);
    servo3.write(180, 60, true);
    servo4.write(100, 60);
    servo5.write(150, 60);
    buttonPushCounter = 0;
    robotMoveCount = 0;
    robotEnable = false;
  }
}

xfpd · April 10, 2024, 4:48am

Include a drawing to show your project, and maybe put it on WOKWI to show how the servos move (and a button).

johnathonj · April 11, 2024, 10:03pm

I had to make some changes to the code. I had to change the servo library add a bunch of delays and use switches in the simulator instead of break beam sensors. Just flip the left switch to trigger break beam sensor 1 and flip the right switch to trigger break beam sensor 2. Remember the robot wont move until break beam sensor 2 is clear or the switch turned off in the simulator. Thats to simulate an object already blocking the place where the robot is to set an object down.

johnathonj · April 12, 2024, 8:44pm

This code fixed a bug in the original post. The bug was if the button is pressed three times and the break beam sensors had not been triggered the robot arm would not go off, it would take four button pushes. Code in simulator also updated.

#include <VarSpeedServo.h>
const byte SENSORPIN1 = 1;
const byte SENSORPIN2 = 2;
const byte BUTTONPIN = 9;
const byte LEDPIN = 3;

VarSpeedServo servo1;
VarSpeedServo servo2;
VarSpeedServo servo3;
VarSpeedServo servo4;
VarSpeedServo servo5;

boolean blinkState = LOW;
boolean ledState = LOW;
boolean ledDone;
boolean event;
boolean robotCycle;
boolean robotWarning;
boolean robotEnable;
boolean robotMove;

int sensorState1 = 0, lastState1 = 0;
int sensorState2 = 0, lastState2 = 0;
int buttonPushCounter = 0;
int buttonState = 0;
int lastButtonState = 0;
int robotMoveCount = 0;
int robotMoveDone;

unsigned long robotWarningTime;
unsigned long debounceDuration = 50;
unsigned long lastButtonStateChange = 0;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
unsigned long time1 = 0;
unsigned long time2 = 0;
unsigned long time3 = 0;

const long interval = 100;

void setup() {
  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(7);
  servo5.attach(8);
  pinMode(BUTTONPIN, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  pinMode(SENSORPIN1, INPUT_PULLUP);
  pinMode(SENSORPIN2, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  blinkState = HIGH;
  event = true;
  robotMove = true;
  ledDone = true;
  robotWarning = false;
}

void loop() {
  currentMillis = millis();
  control_switch();
  led_blink();
  robot();
}

void control_switch() {
  sensorState1 = digitalRead(SENSORPIN1);
  sensorState2 = digitalRead(SENSORPIN2);
  buttonState = digitalRead(BUTTONPIN);
  if (millis() - lastButtonStateChange >= debounceDuration) {
    if (buttonState != lastButtonState) {
      lastButtonStateChange = millis();
      if (buttonState == LOW) {
        buttonPushCounter++;
      }
      lastButtonState = buttonState;
    }
  }
  if (buttonPushCounter == 1) {
    robotMoveCount = 1;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    buttonPushCounter = 2;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if (((buttonPushCounter == 3) || (robotCycle == true)) && (ledDone == true)
      && (sensorState1 == LOW) && (sensorState2 == HIGH)) {
    robotMoveCount = 2;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    buttonPushCounter = 4;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if (buttonPushCounter == 5) {
    robotMoveCount = 3;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    robotCycle = false;
    buttonPushCounter = 6;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if ((currentMillis - robotWarningTime >= 1000) && (ledDone == false)) {
    blinkState = HIGH;
    event = false;
    digitalWrite(LEDPIN, HIGH);
    robotWarning = true;
  }
  if (robotEnable == false) {
    digitalWrite(LEDPIN, LOW);
    event = true;
    ledDone = true;
    robotWarning = false;
  }
  if ((buttonPushCounter == 4) && (robotMoveCount == 1)){//these lines fixed bug that if button is
    buttonPushCounter = 5;                               //pushed three times it turns the robot
  }                                                      //off again if break beam sensors are not
}                                                        //trigged without these lines it would
                                                         //take four button pushes.
void led_blink() {

  if (event == true) {
    if (blinkState == LOW) {
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        if (ledState == LOW) {
          ledState = HIGH;

        } else {
          ledState = LOW;
        }
        digitalWrite(LEDPIN, ledState);
      }
    }
  }
}

void robot() {
  if ((robotMoveCount == 1) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85, 70, true);
    servo2.write(140, 70, true);
    servo3.write(120, 70, true);
    servo4.write(100, 70, true);
    servo5.write(110, 70, true);
    robotEnable = false;
  }
  if ((robotMoveCount == 2) && (robotEnable == true) && (robotWarning == true)) {
    if (robotMove == true) {
      servo4.write(40, 70, true);
      servo5.write(50, 70, true);
      servo4.write(120, 70, true);
      servo5.write(90, 70, true);
      servo4.write(100, 70, true);
      time1 = currentMillis;
      robotMove = false;
      robotMoveDone = 1;
    }
    if (robotMoveDone == 1) {
      if (currentMillis - time1 >= 2000) {
        servo1.write(50, 60, true);
        servo2.write(130, 60, true);
        servo3.write(60, 60, true);
        servo5.write(150, 70, true);
        time2 = currentMillis;
        robotMoveDone = 2;
      }
    }
    if (robotMoveDone == 2) {
      if (currentMillis - time2 >= 2000) {
        servo1.write(85, 60, true);
        servo2.write(160, 60, true);
        servo3.write(170, 60, true);
        servo5.write(90, 70, true);
        robotMoveDone = 0;
        robotMove = true;
        robotEnable = false;
        robotCycle = true;
      }
    }
  }
  if ((robotMoveCount == 3) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85, 60);
    servo2.write(160, 60, true);
    servo3.write(180, 60, true);
    servo4.write(100, 60);
    servo5.write(150, 60);
    buttonPushCounter = 0;
    robotMoveCount = 0;
    robotEnable = false;
  }
}

xfpd · April 13, 2024, 2:40am

That is good.

system · October 10, 2024, 2:41am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
helping with a push button Programming	12	1426	May 5, 2021
Esperimento Arduino applicato a robotica di base Italiano	20	7275	May 6, 2021
Using 'Arm' Button.. SO CLOSE! Help please!! General Guidance	20	3946	May 5, 2021
My Robot,My journey Robotics	14	8049	May 6, 2021
Beginner's Robotic Arm Robotics	43	19614	May 6, 2021

Robot arm with led blink warning before it moves and button

Related topics