How to interrupt my robotic arm using ISR?

Projects Programming
1

I'm beginner in Arduino and I wrote this code to execute three cycle sequential movements of an 6-servos robot arm into the void loop (). Later I tried to implement an interruption to stop and resume the movements of the arm.

AttachInterrupt is declared in the void setup ().
Void checkSwitch () checks the status of the interruption button.
Serial monitor detects when the button is pressed.

I need a help to implement a interruption in this program to stop the void loop () when button is pressed and return from the point it left when the button is pressed again?
Basically I want the robot arm stop the movement when I pressed interrupt buttom and resume it when i pressed it again.

Thoughts? I really appreciate your help!

See code below:

// add servo libraries
#include <Servo.h>

// define servo pins (changed the pin numbers on 11-09-2022 to make pin #2 free for interrupt use)
#define pinServ1 3
#define pinServ2 4
#define pinServ3 5
#define pinServ4 6
#define pinServ5 7
#define pinServ6 8

// define potentiometers pins
//#define pot1 A0
//#define pot2 A1
//#define pot3 A2
//#define pot4 A3
//#define pot5 A4
//#define pot6 A5

//name servers
Servo serv1, serv2, serv3, serv4, serv5, serv6;

//create position variable
int base;      //create position variable for servo1
int shoulder;  //create position variable for servo2
int elbow;     //create position variable for servo3
int fupdown;   //create position variable for servo4
int frotate;   //create position variable for servo5
int gripper;   //create position variable for servo6

//define Led and button connections
const byte ledPin = 13;
const byte buttonPin = 2;

//Boolean to represent toggle state
volatile byte toggleState = LOW;

void checkSwitch() {
  //check status of swicth
  //toggle Led if button pressed

  if (digitalRead(buttonPin) == LOW) {
    //Switch was pressed
    //Change state of toggle
    toggleState = !toggleState;
    //Indicate state on LED
    digitalWrite(ledPin, toggleState);
    Serial.println("interruption");
  }
}

void setup() {

  // Set LED pin as Output
  pinMode(ledPin, OUTPUT);
  // Set switch pin as Input with Pullup
  pinMode (buttonPin, INPUT_PULLUP);
  // Setup serial monitor. The serial monitor reads the data in the serial port.
  Serial.begin(9600);
  // Attach Interrupt to Interrupt Service Routine (ISR)
  attachInterrupt(digitalPinToInterrupt(buttonPin), checkSwitch, FALLING);
  // Atribute pins to their respective servos
  serv1.attach(pinServ1);
  serv2.attach(pinServ2);
  serv3.attach(pinServ3);
  serv4.attach(pinServ4);
  serv5.attach(pinServ5);
  serv6.attach(pinServ6);
}

void loop() {
  
  // initial position. Execution of the initial position for each servo
  serv1.write(5); //base
  delay(300);
  serv2.write(7); //shoulder
  delay(300);
  serv3.write(0); //elbow
  delay(300);
  serv4.write(0); //fupdown
  delay(300);
  serv5.write(85); //frotate
  delay(300);
  serv6.write(74); //gripper
  delay(10000);

  //cycle 1 moviment (picking and dropping roll #1)

  //1.1 - fist moves from 0 to 90 degrees
  for (fupdown = 0; fupdown <= 90; fupdown += 1) {
    serv4.write(fupdown);
    delay(10); //delay execution for 10ms
  }
  //1.2 - elbow move from 0 to 75 degrees
  for (elbow = 0; elbow <= 75; elbow += 1) {
    serv3.write(elbow);
    delay(10);
  }
  //1.3 - fist rotate from 85 to 180
  for (frotate = 85; frotate <= 180; frotate += 1) {
    serv5.write(frotate);
    delay(10);
  }
  //1.4 - fist moves from 90 to 180 degrees
  for (fupdown = 90; fupdown <= 180; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //1.5 - gripper close
  for (gripper = 74; gripper <= 138; gripper += 1) {
    serv6.write(gripper);
    delay(20);
  }
  delay(2000);
  //1.6 - fist move from 180 to 0 degrees
  for (fupdown = 180; fupdown >= 0; fupdown -= 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //1.7 - fist moves from 0 to 30 degrees
  for (fupdown = 0; fupdown <= 30; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //1.8 - elbow moves from 75 to 0 degrees
  for (elbow = 75; elbow >= 0; elbow -= 1) {
    serv3.write(elbow);
    delay(10);
  }
  //1.9 - shoulder moves from 7 to 112 degrees
  for (shoulder = 7; shoulder <= 112; shoulder += 1) {
    serv2.write(shoulder);
    delay(10);
  }
  delay(2000);
  //1.10 - gripper open
  for (gripper = 135; gripper >= 74; gripper -= 1) {
    serv6.write(gripper);
    delay(20);
  }
  //1.11 - elbow moves from 0 to 30 degrees
  for (elbow = 0; elbow <= 30; elbow += 1) {
    serv3.write(elbow);
    delay(10);
  }
  //1.12 - fist rotate from 180 to 85
  for (frotate = 180; frotate >= 85; frotate -= 1) {
    serv5.write(frotate);
    delay(10);
  }
  // Cycle 2 movement (picking and dropping roll #2)
  // return to initial position:
  serv1.write(5); //base
  delay(300);
  serv2.write(7); //shoulder
  delay(300);
  serv3.write(0); //elbow
  delay(300);
  serv4.write(0); //fupdown
  delay(300);
  serv5.write(85); //frotate
  delay(300);
  serv6.write(74); //gripper
  delay(1000);

  //2.1 - fist moves from 0 to 90 degrees
  for (fupdown = 0; fupdown <= 90; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //2.2 - elbow move from 0 to 75 degrees
  for (elbow = 0; elbow <= 75; elbow += 1) {
    serv3.write(elbow);
    delay(10);
  }
  //2.3 - fist rotate from 85 to 180
  for (frotate = 85; frotate <= 180; frotate += 1) {
    serv5.write(frotate);
    delay(10);
  }
  //2.4 - fist moves from 90 to 180 degrees
  for (fupdown = 90; fupdown <= 180; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //2.5 - gripper close
  for (gripper = 74; gripper <= 138; gripper += 1) {
    serv6.write(gripper);
    delay(20);
  }
  delay(2000);
  //2.6 - fist move from 180 to 0 degrees
  for (fupdown = 180; fupdown >= 0; fupdown -= 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //2.7 - fist moves from 0 to 20 degrees (fist slighly more closed to align the roll-2 on the top of roll-1 in the later movement #2.9 )
  for (fupdown = 0; fupdown <= 20; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //2.8 - elbow moves from 75 to 0 degrees
  for (elbow = 75; elbow >= 0; elbow -= 1) {
    serv3.write(elbow);
    delay(10);
  }
  //2.9 - shoulder moves from 7 to 95 degrees
  for (shoulder = 7; shoulder <= 95; shoulder += 1) {
    serv2.write(shoulder);
    delay(10);
  }
  delay(2000);
  //2.10 - gripper open
  for (gripper = 135; gripper >= 74; gripper -= 1) {
    serv6.write(gripper);
    delay(20);
  }
  //2.11 - elbow moves from 0 to 30 degrees
  for (elbow = 0; elbow <= 30; elbow += 1) {
    serv3.write(elbow);
    delay(10);
  }
  //2.12 - fist rotate from 180 to 85
  for (frotate = 180; frotate >= 85; frotate -= 1) {
    serv5.write(frotate);
    delay(10);
  }
  
 // Cycle 3 movement (picking and dropping roll #3)
 // return to initial position:
  serv1.write(5); //base
  delay(300);
  serv2.write(7); //shoulder
  delay(300);
  serv3.write(0); //elbow
  delay(300);
  serv4.write(0); //fupdown
  delay(300);
  serv5.write(85); //frotate
  delay(300);
  serv6.write(74); //gripper
  delay(1000);

  //3.1 - fist moves from 0 to 90 degrees
  for (fupdown = 0; fupdown <= 90; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //3.2 - elbow move from 0 to 75 degrees
  for (elbow = 0; elbow <= 75; elbow += 1) {
    serv3.write(elbow);
    delay(10);
  }
  //3.3 - fist rotate from 85 to 180
  for (frotate = 85; frotate <= 180; frotate += 1) {
    serv5.write(frotate);
    delay(10);
  }
  //3.4 - fist moves from 90 to 180 degrees
  for (fupdown = 90; fupdown <= 180; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //3.5 - gripper close
  for (gripper = 74; gripper <= 138; gripper += 1) {
    serv6.write(gripper);
    delay(20);
  }
  delay(2000);
  //3.6 - fist move from 180 to 0 degrees
  for (fupdown = 180; fupdown >= 0; fupdown -= 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //3.7 - fist moves from 0 to 10 degrees (fist slighly more closed to align the roll-3 on the top of roll-2 in the later movement #3.9 )
  for (fupdown = 0; fupdown <= 10; fupdown += 1) {
    serv4.write(fupdown);
    delay(10);
  }
  //3.8 - elbow moves from 75 to 0 degrees
  for (elbow = 75; elbow >= 0; elbow -= 1) {
    serv3.write(elbow);
    delay(10);
  }
  //3.9 - shoulder moves from 7 to 90 degrees
  for (shoulder = 7; shoulder <= 90; shoulder += 1) {
    serv2.write(shoulder);
    delay(10);
  }
  delay(2000);
  //3.10 - gripper open
  for (gripper = 135; gripper >= 74; gripper -= 1) {
    serv6.write(gripper);
    delay(20);
  }
  //3.11 - elbow moves from 0 to 30 degrees
  for (elbow = 0; elbow <= 30; elbow += 1) {
    serv3.write(elbow);
    delay(10);
  }
  //3.12 - fist rotate from 180 to 85
  for (frotate = 180; frotate >= 85; frotate -= 1) {
    serv5.write(frotate);
    delay(10);
  }
}
2

This is absolutely the wrong application for an interrupt. What you really need to do is restructure your code to get rid of delay() calls and eliminate all of the for loops.

If you use a state machine to control progression through your sequence of moves then you can check the switch at the beginning of loop() and pause your movements by entering a pause state and then resuming the state that was executing.

Check out the following tutorials:

State Machine
Example-code for timing based on millis()
StateChangeDetection

3

Please edit your post to add code tags.

I agree with the comment above: avoid interrupts if at all possible, especially if you are a beginning programmer. Using them almost always introduces more problems than are solved.

4

Also please follow the forum rules and edit your post to put the code inside code tags.

1 Like
5

Welcome to the forum

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the </> icon above the compose window) to make it easier to read and copy for examination

6

Not a tutorial but a Task macro implementation that simplifies conversion of existing code with delay(). Use of taskDelay() allows to retain the sequential structure of the code. Interruption or start/stop of the task can be accomplished in loop() as with any state machine.

7

Just call this function after every delay().

void CheckPause()
{
  boolean isPaused;
  // Note: `do{}while();` is like a `while(){}` loop but the test is done at 
  // the end.  The body is always executed at leat once.
  do
  {
    // Always disable interrupts while reading or writing 
    // volatile variables.
    noInterrupts();
    isPaused = toggleState == HIGH;
    interrupts();
  } while (isPaused);
}
8

Or name that function Delay() and use the passed delay time within the function. This allows to easily replace or switch between delay() and Delay() by simple search/replace or e.g. use of a #define.

9

Even better!

// add servo libraries
#include <Servo.h>

// define servo pins (changed the pin numbers on 11-09-2022 to make pin #2 free for interrupt use)
#define pinServ1 3
#define pinServ2 4
#define pinServ3 5
#define pinServ4 6
#define pinServ5 7
#define pinServ6 8

// define potentiometers pins
//#define pot1 A0
//#define pot2 A1
//#define pot3 A2
//#define pot4 A3
//#define pot5 A4
//#define pot6 A5

//name servers
Servo serv1, serv2, serv3, serv4, serv5, serv6;

//create position variable
int base; //create position variable for servo1
int shoulder; //create position variable for servo2
int elbow; //create position variable for servo3
int fupdown; //create position variable for servo4
int frotate; //create position variable for servo5
int gripper; //create position variable for servo6

//define Led and button connections
const byte ledPin = 13;
const byte buttonPin = 2;

//Boolean to represent toggle state
volatile byte toggleState = LOW;

void checkSwitch()
{
  //check status of swicth
  //toggle Led if button pressed

  if (digitalRead(buttonPin) == LOW)
  {
    //Switch was pressed
    //Change state of toggle
    toggleState = !toggleState;
    //Indicate state on LED
    digitalWrite(ledPin, toggleState);
    Serial.println("interruption");
  }
}

void MyDelay(unsigned long del)
{
  boolean isPaused;
  unsigned long startTime = millis();
  do
  {
    noInterrupts();
    isPaused = toggleState == HIGH;
    interrupts();
  } while (isPaused || millis() - startTime < del);
}

void setup()
{

  // Set LED pin as Output
  pinMode(ledPin, OUTPUT);
  // Set switch pin as Input with Pullup
  pinMode (buttonPin, INPUT_PULLUP);
  // Setup serial monitor. The serial monitor reads the data in the serial port.
  Serial.begin(9600);
  // Attach Interrupt to Interrupt Service Routine (ISR)
  attachInterrupt(digitalPinToInterrupt(buttonPin), checkSwitch, FALLING);
  // Atribute pins to their respective servos
  serv1.attach(pinServ1);
  serv2.attach(pinServ2);
  serv3.attach(pinServ3);
  serv4.attach(pinServ4);
  serv5.attach(pinServ5);
  serv6.attach(pinServ6);
}

void loop()
{

  // initial position. Execution of the initial position for each servo
  serv1.write(5); //base
  MyDelay(300);
  serv2.write(7); //shoulder
  MyDelay(300);
  serv3.write(0); //elbow
  MyDelay(300);
  serv4.write(0); //fupdown
  MyDelay(300);
  serv5.write(85); //frotate
  MyDelay(300);
  serv6.write(74); //gripper
  MyDelay(10000);

  //cycle 1 moviment (picking and dropping roll #1)

  //1.1 - fist moves from 0 to 90 degrees
  for (fupdown = 0; fupdown <= 90; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10); //delay execution for 10ms
  }
  //1.2 - elbow move from 0 to 75 degrees
  for (elbow = 0; elbow <= 75; elbow += 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //1.3 - fist rotate from 85 to 180
  for (frotate = 85; frotate <= 180; frotate += 1)
  {
    serv5.write(frotate);
    MyDelay(10);
  }
  //1.4 - fist moves from 90 to 180 degrees
  for (fupdown = 90; fupdown <= 180; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //1.5 - gripper close
  for (gripper = 74; gripper <= 138; gripper += 1)
  {
    serv6.write(gripper);
    MyDelay(20);
  }
  MyDelay(2000);
  //1.6 - fist move from 180 to 0 degrees
  for (fupdown = 180; fupdown >= 0; fupdown -= 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //1.7 - fist moves from 0 to 30 degrees
  for (fupdown = 0; fupdown <= 30; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //1.8 - elbow moves from 75 to 0 degrees
  for (elbow = 75; elbow >= 0; elbow -= 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //1.9 - shoulder moves from 7 to 112 degrees
  for (shoulder = 7; shoulder <= 112; shoulder += 1)
  {
    serv2.write(shoulder);
    MyDelay(10);
  }
  MyDelay(2000);
  //1.10 - gripper open
  for (gripper = 135; gripper >= 74; gripper -= 1)
  {
    serv6.write(gripper);
    MyDelay(20);
  }
  //1.11 - elbow moves from 0 to 30 degrees
  for (elbow = 0; elbow <= 30; elbow += 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //1.12 - fist rotate from 180 to 85
  for (frotate = 180; frotate >= 85; frotate -= 1)
  {
    serv5.write(frotate);
    MyDelay(10);
  }
  // Cycle 2 movement (picking and dropping roll #2)
  // return to initial position:
  serv1.write(5); //base
  MyDelay(300);
  serv2.write(7); //shoulder
  MyDelay(300);
  serv3.write(0); //elbow
  MyDelay(300);
  serv4.write(0); //fupdown
  MyDelay(300);
  serv5.write(85); //frotate
  MyDelay(300);
  serv6.write(74); //gripper
  MyDelay(1000);

  //2.1 - fist moves from 0 to 90 degrees
  for (fupdown = 0; fupdown <= 90; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //2.2 - elbow move from 0 to 75 degrees
  for (elbow = 0; elbow <= 75; elbow += 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //2.3 - fist rotate from 85 to 180
  for (frotate = 85; frotate <= 180; frotate += 1)
  {
    serv5.write(frotate);
    MyDelay(10);
  }
  //2.4 - fist moves from 90 to 180 degrees
  for (fupdown = 90; fupdown <= 180; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //2.5 - gripper close
  for (gripper = 74; gripper <= 138; gripper += 1)
  {
    serv6.write(gripper);
    MyDelay(20);
  }
  MyDelay(2000);
  //2.6 - fist move from 180 to 0 degrees
  for (fupdown = 180; fupdown >= 0; fupdown -= 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //2.7 - fist moves from 0 to 20 degrees (fist slighly more closed to align the roll-2 on the top of roll-1 in the later movement #2.9 )
  for (fupdown = 0; fupdown <= 20; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //2.8 - elbow moves from 75 to 0 degrees
  for (elbow = 75; elbow >= 0; elbow -= 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //2.9 - shoulder moves from 7 to 95 degrees
  for (shoulder = 7; shoulder <= 95; shoulder += 1)
  {
    serv2.write(shoulder);
    MyDelay(10);
  }
  MyDelay(2000);
  //2.10 - gripper open
  for (gripper = 135; gripper >= 74; gripper -= 1)
  {
    serv6.write(gripper);
    MyDelay(20);
  }
  //2.11 - elbow moves from 0 to 30 degrees
  for (elbow = 0; elbow <= 30; elbow += 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //2.12 - fist rotate from 180 to 85
  for (frotate = 180; frotate >= 85; frotate -= 1)
  {
    serv5.write(frotate);
    MyDelay(10);
  }

  // Cycle 3 movement (picking and dropping roll #3)
  // return to initial position:
  serv1.write(5); //base
  MyDelay(300);
  serv2.write(7); //shoulder
  MyDelay(300);
  serv3.write(0); //elbow
  MyDelay(300);
  serv4.write(0); //fupdown
  MyDelay(300);
  serv5.write(85); //frotate
  MyDelay(300);
  serv6.write(74); //gripper
  MyDelay(1000);

  //3.1 - fist moves from 0 to 90 degrees
  for (fupdown = 0; fupdown <= 90; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //3.2 - elbow move from 0 to 75 degrees
  for (elbow = 0; elbow <= 75; elbow += 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //3.3 - fist rotate from 85 to 180
  for (frotate = 85; frotate <= 180; frotate += 1)
  {
    serv5.write(frotate);
    MyDelay(10);
  }
  //3.4 - fist moves from 90 to 180 degrees
  for (fupdown = 90; fupdown <= 180; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //3.5 - gripper close
  for (gripper = 74; gripper <= 138; gripper += 1)
  {
    serv6.write(gripper);
    MyDelay(20);
  }
  MyDelay(2000);
  //3.6 - fist move from 180 to 0 degrees
  for (fupdown = 180; fupdown >= 0; fupdown -= 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //3.7 - fist moves from 0 to 10 degrees (fist slighly more closed to align the roll-3 on the top of roll-2 in the later movement #3.9 )
  for (fupdown = 0; fupdown <= 10; fupdown += 1)
  {
    serv4.write(fupdown);
    MyDelay(10);
  }
  //3.8 - elbow moves from 75 to 0 degrees
  for (elbow = 75; elbow >= 0; elbow -= 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //3.9 - shoulder moves from 7 to 90 degrees
  for (shoulder = 7; shoulder <= 90; shoulder += 1)
  {
    serv2.write(shoulder);
    MyDelay(10);
  }
  MyDelay(2000);
  //3.10 - gripper open
  for (gripper = 135; gripper >= 74; gripper -= 1)
  {
    serv6.write(gripper);
    MyDelay(20);
  }
  //3.11 - elbow moves from 0 to 30 degrees
  for (elbow = 0; elbow <= 30; elbow += 1)
  {
    serv3.write(elbow);
    MyDelay(10);
  }
  //3.12 - fist rotate from 180 to 85
  for (frotate = 180; frotate >= 85; frotate -= 1)
  {
    serv5.write(frotate);
    MyDelay(10);
  }
}
1 Like
10

When using a number of the same objects nor devices, look into structs, and array[]s.

Understanding millis() timing is critical, along with creating function()s to handle all your ‘similar’ actions.

These are not advanced concepts, but ware critical to writing fast, effective, easy to debug code.

Interrupts are next month’s lesson!

11

thanks I quoted the code now.

12

why not have a power switch?
what should happen afterwards: resume operation?

does your robot only ever move one actuator at a time? could these movements be captured in a sequencer table where each entry describes some movement or sequence of movements?

13

@johnwasser will always get credit for the hack which is myDelay().

No need for interrupts at all here.

a7

14

consider
data driven, uses millis() instead of delay() and immediately pauses operation when a button is pressed

char s [80];

#undef MyHW
#ifdef MyHW
struct Servo {
    int  pin;
    void attach (int p)  { pin = p; }
    void write (int n)  {
        sprintf (s, " wr: %d %3d", pin, n);
        Serial.println (s);
    }
};
const byte buttonPin = A1;

#else
# include <Servo.h>
const byte buttonPin = 2;
#endif

Servo serv1, serv2, serv3, serv4, serv5, serv6;
Servo servo [6];

const byte ledPin    = 13;
byte       mode;
byte       butState;

struct Seq {
    int     op;
    int     servo;
    int     delay;
    int     pos0;
    int     posN;
    int     dPos;
    const char * text;
};

enum { NUL, SET, FOR, STOP, TXT };
enum { S1, S2, S3, S4, S5, S6 };

Seq seqs [] = {
    { TXT, 0,      0,   0,   0,   0, "start" },
    { SET, S1,   300,   5 },
    { SET, S2,   300,   7 },
    { SET, S3,   300,   0 },
    { SET, S4,   300,   0 },
    { SET, S5,   300,  85 },
    { SET, S6, 10000,  74 },
    { FOR, S4,    10,   0,  90,   1 },
    { FOR, S3,    10,   0,  75,   1 },
    { FOR, S5,    10,  85, 180,   1 },
    { FOR, S4,    10,  90, 180,   1 },
    { FOR, S6,    20,  74, 138,   1 },
    { FOR, S4,    10, 180,   0,  -1 },
    { FOR, S4,    10,   0,  30,   1 },
    { FOR, S3,    10,  75,   0,  -1 },
    { FOR, S2,    10,   7, 112,   1 },
    { FOR, S6,    20, 135,  74,  -1 },
    { FOR, S3,    10,   0,  30,   1 },
    { FOR, S5,    10, 180,  85,  -1 },
    { TXT, 0,      0,   0,   0,   0, "2nd stage" },
    { SET, S1,   300,   5 },
    { SET, S2,   300,   7 },
    { SET, S3,   300,   0 },
    { SET, S4,   300,   0 },
    { SET, S5,   300,  85 },
    { SET, S6,  1000,  74 },
    { FOR, S4,    10,   0,  90,   1 },
    { FOR, S3,    10,   0,  75,   1 },
    { FOR, S5,    10,  85, 180,   1 },
    { FOR, S4,    10,  90, 180,   1 },
    { FOR, S6,    20,  74, 138,   1 },
    { FOR, S4,    10, 180,   0,  -1 },
    { FOR, S4,    10,   0,  20,   1 },
    { FOR, S3,    10,  75,   0,  -1 },
    { FOR, S2,    10,   7,  95,   1 },
    { FOR, S6,    20, 135,  74,  -1 },
    { FOR, S3,    10,   0,  30,   1 },
    { FOR, S5,    10, 180,  85,  -1 },
    { SET, S1,   300,   5 },
    { SET, S2,   300,   7 },
    { SET, S3,   300,   0 },
    { SET, S4,   300,   0 },
    { SET, S5,   300,  85 },
    { SET, S6,  1000,  74 },
    { FOR, S4,    10,   0,  90,   1 },
    { FOR, S3,    10,   0,  75,   1 },
    { FOR, S5,    10,  85, 180,   1 },
    { FOR, S4,    10,  90, 180,   1 },
    { FOR, S6,    20,  74, 138,   1 },
    { FOR, S4,    10, 180,   0,  -1 },
    { FOR, S4,    10,   0,  10,   1 },
    { FOR, S3,    10,  75,   0,  -1 },
    { FOR, S2,    10,   7,  90,   1 },
    { FOR, S6,    20, 135,  74,  -1 },
    { FOR, S3,    10,   0,  30,   1 },
    { FOR, S5,    10, 180,  85,  -1 },
    { STOP, 0 },
    { NUL, 0 },
};

int seqIdx;
int seqN;

// -----------------------------------------------------------------------------
unsigned long msecLst;
unsigned long msecDelay;

enum { Reset, Run, Pause };

void sequencer (
    int mode )
{
    if (Pause == mode)
        return;

    if (Reset == mode)  {
        seqIdx = 0;
        seqN   = -1;
        return;
    }

    unsigned long msec = millis ();
    if (msecDelay && msec - msecLst < msecDelay)  {
        return;
    }
    msecLst   = msec;
    msecDelay = 0;

    Seq *p = & seqs [seqIdx];

    switch (p->op)  {
    case STOP:
        break;

    case NUL:
        seqIdx = 0;
        seqN   = -1;
        break;

    case TXT:
        Serial.println (p->text);
        seqIdx++;
        break;

    case SET:
        servo [p->servo].write (p->pos0);
        msecDelay = p->delay;
        seqIdx++;
        break;

    case FOR:
        if (-1 == seqN)
            seqN = p->pos0;
        servo [p->servo].write (seqN);
        seqN += p->dPos;
        if (p->posN < seqN)  {
            seqIdx++;
            seqN = -1;
        }

        msecDelay = p->delay;
        break;
    }
}

// -----------------------------------------------------------------------------
void loop ()
{
    byte but = digitalRead (buttonPin);
    if (butState != but)  {
        butState = but;

        if (LOW == but) {     // press
            if (Run == mode)
                mode = Pause;
            else
                mode = Run;

            digitalWrite (ledPin, ! digitalRead (ledPin));
        }
    }

    sequencer (mode);
}

// -----------------------------------------------------------------------------
void setup ()
{
    Serial.begin (9600);

    pinMode (buttonPin, INPUT_PULLUP);
    butState = digitalRead (buttonPin);

    pinMode (ledPin, OUTPUT);

    servo [0].attach (3);
    servo [1].attach (4);
    servo [2].attach (5);
    servo [3].attach (6);
    servo [4].attach (7);
    servo [5].attach (8);

    sequencer (Reset);
    mode = Run;
}
15

The 'return' part is needed if you want to exit from one delay pattern so you can switch to a different delay pattern. In this case, the pattern isn't changed, just paused.

16

Thank you! I implemented your solution and it worked perfectly. I read your code to understand your additions and changes. The only question i have now is about the void MyDelay. Sorry my lack of experience but what's the logic behind the variable del and this part of do-while:
while (isPaused || millis() - startTime < del);

17

While something is paused or millis minus the start time is less than the delay constant, repeat.

...dangerous because if 'isPaused' is true, there is no escape from an endless loop.

1 Like
18

Thank you! I tested your solution, the arm stopped when button is pressed, then the interruption of the arm works as intended.. However, the arm only execute the 1st out of 3 cycles of movement (from start to the moment before 2nd stage start), then it returned to the it's initial position. I will investigate why the 2nd and 3rd cycle of movements does not run with this code.

19

No, it's declared in the Arduino core. It's called by you in setup().

1 Like
20

Except that 'isPaused' is copied from the volatile variable 'toggleState' which is set by the button interrupt.