Controlling servos using muscle sensor and a button!

Projects General Guidance
1

Hey all, I am currently working on a little project to control two servos using a Myoware Muscle Sensor as well as a button. Currently my code can do the following

-Muscle contracts --> Servos move one way

-Muscle relaxes --> Servos move the other way

#include <Servo.h>
Servo servo1;
Servo servo2;

const int threshValue = 250;

void setup()
{
  servo1.attach(7);
  servo2.attach(8);
}

void loop()
{
 int value = analogRead(A3);

  if(value < threshValue)        
  {
            servo1.writeMicroseconds(800); 
            servo2.writeMicroseconds(800); 
  }
  else
  {
            servo1.writeMicroseconds(2250);
            servo2.writeMicroseconds(2250);
  }
}

However, I am trying to add a button into the mix. I want to make it so that when:

-Button is not pressed
-Servos behave like the way they do above
-Button pressed
-One of the servos (let's call them servo 1 and servo 2), Servo 2 does not move
-Servo 1 is able to move forwards and back until
-Button is pressed again
-Servo 1 stops at the position it is in
-Servo 2 is able to move forwards and back until
-Button is pressed again
-Servo 2 stops at the position it is in
-Servo 1 is free to move forwards and back again.

The code that I am trying to incorporate is from here. It seems useful but this code doesn't need to be used for this project.

const int Switch = 2;
int state = 0, Loadstate=0;

void setup()
{
  pinMode(Switch, INPUT);
}
void loop()
{
  if (state == 0 && digitalRead(Switch) == HIGH) {
    state = 1;
    Loadstate=!Loadstate;
  }
  if (state == 1 && digitalRead(Switch) == LOW) {   
    state = 0;
  }
   if (Loadstate==HIGH){
    // Add Code block
   }
   else{
    //Add Code block
   }
}

I am trying to combine the two and making it look something like this:

#include <Servo.h>
Servo servo1;
Servo servo2;
const int threshValue = 250, Switch = 2 ;
int state = 0, Loadstate=0;

void setup()
{
  servo1.attach(7);
  servo2.attach(8);
  pinMode(Switch, INPUT);
}

void loop()
{
 int value = analogRead(A3);
  if (state == 0 && digitalRead(Switch) == HIGH)
  {
    state = 1;
    Loadstate=!Loadstate;
  }
  if (state == 1 && digitalRead(Switch) == LOW) 
  {   
    state = 0;
  }
    if (Loadstate==HIGH)
    {
      if(value < threshValue)
      {
        servo1.writeMicroseconds(800);
        if(value < threshValue)
        {
          servo2.writeMicroseconds(800);
          if(value < threshValue)
          {
            servo1.writeMicroseconds(800);
          }
          else
          {
            servo1.writeMicroseconds(2250);
          }
        }
        else
        {
          servo2.writeMicroseconds(2250);
        }
      }
      else
      {
        servo1.writeMicroseconds(2250);
      }
    }
    else
    {
      if(value < threshValue)
        {
          servo1.writeMicroseconds(800); 
          servo2.writeMicroseconds(800);
        }
        else
        {
         servo1.writeMicroseconds(2250);
         servo2.writeMicroseconds(2250);
        }
    }
}

Not sure if it would work though. haha. Thanks in advance!

2

Put print statements in each part of the code to print to the serial monitor so you can see if the code is performing as expected when the sensor and the button is being used. You might need to put a slight delay in the code to slow it down for testing and reading the output on the serial monitor.