PaulMurrayCbr:
As everyone said - if you want two independent things happening (a relay and a servo arm), then the sketch needs to be rewritten as a state machine.
The loop() function should always exit quickly - within microseconds. This means that each time it runs, it has to ask and answer the question "is there anything that I need to do right now?". To do this, the sketch has global variables that hold what "step" the process is in, and the time ( using millis() ) at which the current step began.
If you have two independent processes, then each will have its own little cluster of global variables to manage that process.
Like this:
#include <Servo.h>
int in1 = 7;
Servo servoLeft; // Definiere linken Servo
Servo servoRight; // Definiere rechten Servo
// relay operations
byte relayState;
uint32_t relayMs;
// pick and place operations
enum PNPState {
START,
FWDREV_FWD,
FWDREV_REV,
RIGHTLEFT_RIGHT,
RIGHTLEFT_LEFT,
STOPPED
} pnpState;
uint32_t pnpMs;
void setup() {
servoLeft.attach(44); // Setzt linken Servo auf digital pin 44
servoRight.attach(22); // Setzt rechten Servo auf digital pin 22
pinMode(in1, OUTPUT);
digitalWrite(in1, HIGH);
relayState = LOW;
digitalWrite(in1, relayState);
relayMs = millis();
pnpState = START;
pnpMs = millis();
}
void loop() {
handleRelay();
handlePnP();
}
void handleRelay() {
if (millis() - relayMs > 3000) {
relayState = (relayState == LOW ? HIGH : LOW);
digitalWrite(in1, relayState);
relayMs = millis();
}
}
void handlePnP() { // Loop für den Bewegungstest
switch (pnpState) {
case START:
forward();
pnpMs = millis();
pnpState = FWDREV_FWD;
break;
case FWDREV_FWD:
if (millis() - pnpMs >= 2000) {
reverse();
pnpMs = millis();
pnpState = FWDREV_REV;
}
break;
case FWDREV_REV:
if (millis() - pnpMs >= 2000) {
turnRight();
pnpMs = millis();
pnpState = RIGHTLEFT_RIGHT;
}
break;
case RIGHTLEFT_RIGHT:
if (millis() - pnpMs >= 2000) {
turnLeft();
pnpMs = millis();
pnpState = RIGHTLEFT_LEFT;
}
break;
case RIGHTLEFT_LEFT:
if (millis() - pnpMs >= 2000) {
stopRobot();
pnpMs = millis();
pnpState = STOPPED;
}
break;
case STOPPED:
if (millis() - pnpMs >= 2000) {
forward();
pnpMs = millis();
pnpState = FWDREV_FWD;
}
break;
}
}
// Bewegungs Routine für Vorwärts, Rückwärts, Drehungen und STOP
void forward() {
servoLeft.write(0);
servoRight.write(180);
}
void reverse() {
servoLeft.write(180);
servoRight.write(0);
}
void turnRight() {
servoLeft.write(180);
servoRight.write(180);
}
void turnLeft() {
servoLeft.write(0);
servoRight.write(0);
}
void stopRobot() {
servoLeft.write(90);
servoRight.write(90);
}
I taste your code, and the servo turn to left and right don`t work for me. I include a 3rd Servo for the turn to left and right.
#include <Servo.h>
int in1 = 7;
Servo servoLeft; // Definiere linken Servo
Servo servoRight; // Definiere rechten Servo
Servo servoDown; // Definiere unteren Servo
// relay operations
byte relayState;
uint32_t relayMs;
// pick and place operations
enum PNPState {
START,
FWDREV_FWD,
FWDREV_REV,
RIGHTLEFT_RIGHT,
RIGHTLEFT_LEFT,
STOPPED
} pnpState;
uint32_t pnpMs;
void setup() {
servoLeft.attach(44); // Setzt linken Servo auf digital pin 44
servoRight.attach(22); // Setzt rechten Servo auf digital pin 22
servoDown.attach(38); // Setzt unteren Servo auf digital pin 38
pinMode(in1, OUTPUT);
digitalWrite(in1, HIGH);
relayState = LOW;
digitalWrite(in1, relayState);
relayMs = millis();
pnpState = START;
pnpMs = millis();
}
void loop() {
handleRelay();
handlePnP();
}
void handleRelay() {
if (millis() - relayMs > 3000) {
relayState = (relayState == LOW ? HIGH : LOW);
digitalWrite(in1, relayState);
relayMs = millis();
}
}
void handlePnP() { // Loop für den Bewegungstest
switch (pnpState) {
case START:
forward();
pnpMs = millis();
pnpState = FWDREV_FWD;
break;
case FWDREV_FWD:
if (millis() - pnpMs >= 2000) {
reverse();
pnpMs = millis();
pnpState = FWDREV_REV;
}
break;
case FWDREV_REV:
if (millis() - pnpMs >= 2000) {
turnRight();
pnpMs = millis();
pnpState = RIGHTLEFT_RIGHT;
}
break;
case RIGHTLEFT_RIGHT:
if (millis() - pnpMs >= 2000) {
turnLeft();
pnpMs = millis();
pnpState = RIGHTLEFT_LEFT;
}
break;
case RIGHTLEFT_LEFT:
if (millis() - pnpMs >= 2000) {
stopRobot();
pnpMs = millis();
pnpState = STOPPED;
}
break;
case STOPPED:
if (millis() - pnpMs >= 2000) {
forward();
pnpMs = millis();
pnpState = FWDREV_FWD;
}
break;
}
}
// Bewegungs Routine für Vorwärts, Rückwärts, Drehungen und STOP
void forward() {
servoLeft.write(0);
servoRight.write(180);
servoDown.write(0);
}
void reverse() {
servoLeft.write(180);
servoRight.write(0);
servoDown.write(0);
}
void turnRight() {
servoLeft.write(180);
servoRight.write(180);
servoDown.write(0);
}
void turnLeft() {
servoLeft.write(0);
servoRight.write(0);
servoDown.write(0);
}
void stopRobot() {
servoLeft.write(90);
servoRight.write(90);
servoDown.write(0);
}