Help with multiple servo and potentiometer controls

well at work I was asked to move from using pneumatic bellows in my design to servos, to add the ability to control acceleration and deceleration accurately without all the mess and size of adding several air valves.

To keep things simple, each output from our midi decoder will be sent to an input on the Arduino to control the movements of 6 servos bending 6 separate tremolos attached to 6 guitar strings. One input pin can set whether it is a full movement of the servo or a half movement (bend from A to B or from A to A#) and to keep the strings in tune, there will need to be 6 potentiometers to set the distance the servo moves for full , and 6 more for half. Also, 3 input pins to set the (could add more later) 4 speeds of movement. The speed inputs must also affect the speed the servo returns home the instant its bend input changes state, even it hasn't finished its initial movement.
My knowledge of programming basically began today, so forgive me if my attempt at coding has major holes, and most of the values are estimations, as nothing is hooked to hardware yet to get the actual values.
Also I am sure it would help the program loop faster if everything is squished into one section filled with variables instead of 6 sections for each servo, but I wanted to ask if that would create problems with initiating a 2nd action on another servo, in the middle of the first servos action. Causing a monophonic instead of polyphonic situation, for lack of a better example.

will probably use, or at least use something similar to the varspeedservo.h library to address speeds.
http://forum.arduino.cc/index.php?topic=61586.0

an example of the pneumatic mechanism it will be replacing, in use.

#include <VarSpeedServo.h>
int Tuning;  // set variable to value of potentiometer
int Pot_E_Full = 0;
int Pot_A_Full = 1;
int Pot_D_Full = 2;
int Pot_g_Full = 3;
int Pot_b_Full = 4;
int Pot_e_Full = 5;
int Pot_E_Half = 6;
int Pot_A_Half = 7;
int Pot_D_Half = 8;
int Pot_g_Half = 9;
int Pot_b_Half = 10;
int Pot_e_Half = 11;
int Pos_E;
int Pos_A;
int Pos_D;
int Pos_g;
int Pos_b;
int Pos_e;
int GuitarString_E_Pin = 22;
int GuitarString_A_Pin = 24;
int GuitarString_D_Pin = 25;
int GuitarString_g_Pin = 26;
int GuitarString_b_Pin = 27;
int GuitarString_e_Pin = 28;
int BendSpeed;
int Bend_Fast_Pin = 30;
int Bend_Slow_Pin = 32;
int Bend_Long_Pin = 34;
int Half_to_Full_Note_Pin = 36;
int High_Position = 160;
int Low_Position = 0;
VarSpeedServo Servo_E;
VarSpeedServo Servo_A;
VarSpeedServo Servo_D;
VarSpeedServo Servo_g;
VarSpeedServo Servo_b;
VarSpeedServo Servo_e;
//Servo_E.attach (mainPin, ServoMin, ServoMax);
//Servo_E.slowmove (newpos, speed);
//1 slowest, 255 fastest (not noticeable above 127)

void setup() { // put your setup code here, to run once:
  pinMode(GuitarString_E_Pin, INPUT);
  pinMode(GuitarString_A_Pin, INPUT);
  pinMode(GuitarString_D_Pin, INPUT);
  pinMode(GuitarString_g_Pin, INPUT);
  pinMode(GuitarString_b_Pin, INPUT);
  pinMode(GuitarString_e_Pin, INPUT);
  pinMode(Bend_Fast_Pin, INPUT);
  pinMode(Bend_Slow_Pin, INPUT);
  pinMode(Bend_Long_Pin, INPUT);
  pinMode(Half_to_Full_Note_Pin, INPUT);
  Servo_E.attach (2, High_Position, Low_Position);
  Servo_A.attach (3, High_Position, Low_Position);
  Servo_D.attach (4, High_Position, Low_Position);
  Servo_g.attach (5, High_Position, Low_Position);
  Servo_b.attach (6, High_Position, Low_Position);
  Servo_e.attach (7, High_Position, Low_Position);
}

void loop() {
  if ((GuitarString_E_Pin == HIGH), (Half_to_Full_Note_Pin == HIGH)) {
    Tuning = analogRead(Pot_E_Full) / 10;    // read the tuning value
    if (Pos_E <= 80) {
      Servo_E.slowmove (81 * Tuning, BendSpeed + 1);
    }
    else {
      Servo_E.slowmove (150 * Tuning, BendSpeed + 21);
    }
  }
  else {
    if (Pos_E >= 81) {
      Servo_E.slowmove (80 * Tuning, BendSpeed + 1);
    }
    else {
      Servo_E.slowmove (0, BendSpeed + 21);
      delay (50);
    }
  }
  if ((GuitarString_E_Pin == HIGH), (Half_to_Full_Note_Pin == LOW)) {
    Tuning = analogRead(Pot_E_Half) / 10;    // read the tuning value
    if (Pos_E <= 50) {
      Servo_E.slowmove (51 * Tuning, BendSpeed + 1);
    }
    else {
      Servo_E.slowmove (90 * Tuning, BendSpeed + 21);
    }
  }
  else {
    if (Pos_E >= 51) {
      Servo_E.slowmove (50 * Tuning, BendSpeed + 1);
    }
    else {
      Servo_E.slowmove (0, BendSpeed + 21);
      delay (50);
    }
  }
  //everything from line 64 to 99 repeated for each guitar string
  //after making sure it actually works
  if (Bend_Fast_Pin == HIGH) { //check bend speed
    BendSpeed = 0;
  }
  if ((Bend_Fast_Pin == LOW) && (Bend_Slow_Pin == LOW) && (Bend_Long_Pin == LOW)) {
    BendSpeed = 60;
  }
  if (Bend_Slow_Pin == HIGH) {
    BendSpeed = 20;
  }
  if (Bend_Long_Pin == HIGH) {
    BendSpeed = 1;
  }
}

yaybacon:
I wanted to ask if that would create problems with initiating a 2nd action on another servo, in the middle of the first servos action.

I had to search a bit to find the actual question.
A tip, too - it's much better to post your code in between 'code' tags, (</> at the top-left of the 'Post' window), rather than 'quote' tags. It would be much easier to read, and wouldn't be converted to italics.

Also, correct formatting helps enormously, with indents in all the right places, so we can see where the 'if()' statements begin and end. (In the Arduino IDE, you can click on >Tools >Auto Format before copying and pasting, and the formatting will be done for you.)

Regarding your question, initiating an action on the second servo won't affect operation of the first one. I've only used the std Servo library, but as I understand it you can run up to 8 servos simultaneously with VarSpeedServo, with all servos acting independently.
In my project, I run two continuous-rotation servos, and can vary the speed or direction of one servo while the other just continues turning at the same speed uninterrupted.

int Pot_E_Full = 0;
int Pot_A_Full = 1;
int Pot_D_Full = 2;
int Pot_g_Full = 3;
int Pot_b_Full = 4;
int Pot_e_Full = 5;
int Pot_E_Half = 6;
int Pot_A_Half = 7;
int Pot_D_Half = 8;
int Pot_g_Half = 9;
int Pot_b_Half = 10;
int Pot_e_Half = 11;
int Pos_E;
int Pos_A;
int Pos_D;
int Pos_g;
int Pos_b;
int Pos_e;
int GuitarString_E_Pin = 22;
int GuitarString_A_Pin = 24;
int GuitarString_D_Pin = 25;
int GuitarString_g_Pin = 26;
int GuitarString_b_Pin = 27;
int GuitarString_e_Pin = 28;

Have you ever heard of arrays?

haha thank you, ill edit that. But, that question was more an afterthought of the actual question of, "Am I heading in the right direction?" Since this is my first attempt at programming with close to no previous experience :smiley:

The Afterthought was me trying to ask, I don't know the actual arduino language very well so I'll try to do it in regular english~ would it be better to use a single changing code to control everything. or copy and pasting the same code 6 times with the changed values? If the shorter changing code is the way to go, I was failing to find information on how to write certain parts of it... Already started, but erased my work to go the other route when I ran into a wall.

servo(a,b,c,d,e,or f )
moves (amount(a or b) * speed (a,b,c, or d))

or as

if button (a)
servo (a)
moves
if button (b)
servo(b)
moves
if button (c)
servo (c)
moves
if button (d)
servo (d)
moves

Thanks for the tip, I started to look into arrays but was sidetracked, I'll find some information on it straight away

yaybacon:
would it be better to use a single changing code to control everything. or copy and pasting the same code 6 times with the changed values?

The shorter code with changing variables is definitely the best way to do things.

If the shorter changing code is the way to go, I was failing to find information on how to write certain parts of it... Already started, but erased my work to go the other route when I ran into a wall.

It'll take some figuring out, but as mentioned short is the way to go.

Looking over your first code, there are a number of issues. I think that you need to do a bit more reading and get a little more practice on simple stuff first up.
I'll point out a couple of things.
First, this compiles, but won't actually work the way (I think) you want it to. (Unfortunately, this is often the case.):-

if ((GuitarString_E_Pin == HIGH), (Half_to_Full_Note_Pin == LOW))

In English, I'm assuming that this is meant to mean:
"if GuitarString_E_Pin is HIGH and Half_to_Full_Note_Pin is LOW"

First, to read a digital pin, you need to use digitalRead().
Second, the 'and' part should be expressed like this && , rather than with a comma.
So, your line should look like this:-

if ((digitalRead(GuitarString_E_Pin) == HIGH) && (digitalRead(Half_to_Full_Note_Pin) == LOW))

I see that further down in your code you used && , so if I'm mistaken about your intended meaning please let me know.

This is an example of the code further down that I'm referring to.

if ((Bend_Fast_Pin == LOW) && (Bend_Slow_Pin == LOW) && (Bend_Long_Pin == LOW))

You use the && correctly, but still don't use digitalRead().
So this line should look like this:-

if ((digitalRead(Bend_Fast_Pin) == LOW) && (digitalRead(Bend_Slow_Pin) == LOW) && (digitalRead(Bend_Long_Pin) == LOW))

That's probably enough to get you on the right track for now with this aspect of your coding.

Actually, there is one other point that'll make your code shorter, therefore easier to read.
When only one statement follows an if statement, you don't need to enclose it in brackets.
So where you have this:-

if (Pos_E <= 50){
    Servo_E.slowmove (51 * Tuning, BendSpeed + 1);
}
else {
    Servo_E.slowmove (90 * Tuning, BendSpeed + 21);
}

You could write it like this if you wanted:-

if (Pos_E <= 50)
    Servo_E.slowmove (51 * Tuning, BendSpeed + 1);
else
    Servo_E.slowmove (90 * Tuning, BendSpeed + 21);

It is better to use a function that takes one or more arguments than it is to copy/paste code several times. If you get it wrong, and copy it 5 times, you have to fix all 6 places. If you get it wrong, in a function, and call it 6 times, you only need to fix the function.

yaybacon:
well at work I was asked to move from using pneumatic bellows in my design to servos, to add the ability to control acceleration and deceleration accurately without all the mess and size of adding several air valves.

What sort of precision are you after when controlling acceleration?

I'm personally very interested in moving servos precisely and I don't see a way a getting around setting the individual servo positions at 50Hz.

I calculate (and set) the position of each servo every 20ms. This allows more complex movements than just having the servos move at a constant speed.

Here are a couple examples of servos moving.

My hexapod.

The xyz coordinates of each foot are computed at 50Hz and these xyz coordinates are used with an IK algorithm to compute the servo positions.

32 servo test.

The varspeedservo library looks like it would be a very useful tool for lots of applications but if you want to be able to precisely control the acceleration of a servo, I think you need to compute each individual position using the appropriate equations of motion.

Will the motion you want to produce be predetermined (following a script)? If so, the calculations involved will be a lot easier. Having to adjust the motion parameters on the fly is a lot harder than working from a script. The 32 servo demo was much easier to program than the 22 servo hexapod.

The examples I posted were done with a Propeller microcontroller but I think the same sort of motion is possible with the Arduino but with few servos.

May have figured out why I was so lost in searching the almighty googles for answers, the problem isn't in the Arduino language, but a failing in the usage of the VarSpeedservo library.

if (New_Position >= 51) {
          Servo_1.slowmove (50 * TuningValue, Bend_Speed + 1);
        }
        else {

The programming wall I kept running into, was getting the Servo_E.slowmove section to be variable to address all servos, something like ~

int Servo[] = {0,1,2,3,4,5,6};
if (New_Position >= 51) {
          Servo(Servo[i]).slowmove (50 * TuningValue, Bend_Speed + 1);
        }
        else {

I believe my answer lies in removing any reference to the VarSpeedServo library and going to controlling the pulses directly

I believe my answer lies in removing any reference to the VarSpeedServo library and going to controlling the pulses directly

No. The answer lies in using an array of Servo instances, so you can iterate through the array. Names are lost during the compilation process. The Arduino has no idea what the name of any instance is. Trying to manufacture a name is pointless.

Sorry, but you lost me a little bit. I thought I was explaining it that same way, would it be possible for you give an example? Here is how the code looks now, after staying up another night.

int Note_Pins[] = {22, 24, 26, 28, 30, 32};
int Note_Var;                         //which string is bending

int Half_Bend_Pin = 34;
int Half_Bend = 2;

int Servo_Pins[] = {2, 3, 4, 5, 6, 7};
int Servo_Number;                     //Increases in setup to declare pins
int Servo_Name;                       //sends servo pin number to void servoPulse
int Angle;
int pwm;

int Speed_Pins[] = {36, 38, 40};      //pin numbers
int Speed_Array[] = {100, 40, 20, 1};
int Bend_Array_Var;                   //used to cycle between switches
int Bend_Speed;                       //Which int inside speed array is called
int Bend_Apex;

int Tuning_Pins[] = {1, A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11};
int Tuning_Var;
int TuningValue;


void setup() {
  for (int Tuning_Var = 0; Tuning_Var < 12; Tuning_Var++)  {
    pinMode(Tuning_Pins[Tuning_Var], INPUT);
  }
  for (int Servo_Number = 0; Servo_Number < 5; Servo_Number++)  {
    pinMode(Servo_Pins[Servo_Number], OUTPUT);
  }
}


void loop() {
  Servo_Name = Servo_Pins[Note_Var];
  if (digitalRead(Half_Bend_Pin == HIGH)) {
    TuningValue = analogRead(Tuning_Pins[Note_Var]);
  }
  else {
    TuningValue = analogRead(Tuning_Pins[Note_Var]) * 2;
  }
  if (digitalRead(Speed_Pins[Bend_Array_Var] == HIGH)) { //check bend speed
    Bend_Speed = Speed_Array[Bend_Array_Var];
  }
  else {
    Bend_Speed = Speed_Array[1];
    if (Bend_Array_Var < 4) {
      Bend_Array_Var++;
    }
    else {
      Bend_Array_Var = 0;
    }
  }
  if (digitalRead(Note_Pins[Note_Var] == HIGH)) { //begin Bend
    if (Angle <= Bend_Apex) {
      for (Angle = 0; Angle <= Bend_Apex; Angle += 1)  {
        ServoPulse(Servo_Name, Angle);
        delay (Bend_Speed);
      }
    }
    else {
      for (Angle = 0; Angle <= TuningValue; Angle += 1)  {
        ServoPulse(Servo_Name, Angle);
        delay (Bend_Speed);
      }
    }
  }
  else {                                      //Releasing Bend
    if (Angle >= Bend_Apex) {
      for (Angle = 0; Angle <= Bend_Apex; Angle -= 1)  {
        ServoPulse(Servo_Name, Angle);
        delay (Bend_Speed);
      }
    }
    else {
      for (Angle = 0; Angle >= 0; Angle -= 1)  {
        ServoPulse(Servo_Name, Angle);
        delay (Bend_Speed);
      }
    }
  }
  if (Note_Var < 6) {
    Note_Var++;
  }
  else {
    Note_Var = 0;
  }
}


void ServoPulse (int Servo_Name, int Angle)
{
  pwm = (Angle * 11) + 500;    // Convert angle to microseconds
  digitalWrite(Servo_Name, HIGH);
  delayMicroseconds(pwm);
  digitalWrite(Servo_Name, LOW);
  delay(50);                   // Refresh cycle of servo
}
  Servo_Name = Servo_Pins[Note_Var];

The pin number that a servo is attached to does not define its name.

In that last code, you are not using the Servo library, so any talk of Servo instances is wrong.

Ok! I added serial prints, and realized I didn't assign pin inputs to basically anything. Somehow the outputs were correctly assigned but inputs were overlooked. Found 2 Else's in a row leading to a bend delay stopping the entire loop, so moved all the servo commands into a seperate function. Also I changed all the Servo_Names to Actuator_... to keep PaulS happy, Even though it was my understanding that Servo and servo(lowercase) were completely different.

The buttons are all working in the serial monitor, the speed ones switch while the guitar string pins allow holding multiples at the same time. Can finally add the servos back again tonight.

int Note_Pins[] = {22, 24, 26, 28, 30, 32};
int Note_Var;                         //which string is bending
int note_on;
int Half_Bend_Pin = 34;
int Half_Bend = 2;
int Actuator_Pins[] = {2, 3, 4, 5, 6, 7};
int Actuator_Var;                     //Increases in setup to declare pins
int Actuator_Array[] = {0, 0, 0, 0, 0, 0};
int Actuator_New;
int Actuator_Last;                     //sends servo pin to void ActuatorPulse()
int Angle;
int pwm;
int Bendspeed_new;
int Bendspeed_Last;
int Speed_Pins[] = {36, 38, 40};
int Speed_Pin_Var;                        //used to cycle between speed pins array
int Actuator_Delay_Array[] = {100, 20, 1, 45};
int Tuning_Pins[] = {1, A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11};
int Tuning_Var;
int TuningValue;
int Hide_Serialprint_Half = 1;
int Hide_Serialprint_Note[] = {0, 0, 0, 0, 0, 0};
int debug = 1;

void setup() {
  if (debug == 1) {
    Serial.begin(9600);
  }
  delay(10);
  pinMode(Half_Bend_Pin, INPUT_PULLUP);
  Serial.println("Half/Full input pin assigned");
  Serial.println("Note input pins assigned");
  for (int Note_Var = 0; Note_Var < 6; Note_Var++)  {
    pinMode(Note_Pins[Note_Var], INPUT_PULLUP);
    Serial.println(Note_Var);
  }
  Serial.println("Bend speed pins assigned");
  for (int Speed_Pin_Var = 0; Speed_Pin_Var < 3; Speed_Pin_Var++)  {
    pinMode(Speed_Pins[Speed_Pin_Var], INPUT_PULLUP);
    Serial.println(Speed_Pin_Var);
  }
  Serial.println("Tuning input pins assigned~ ");
  for (int Tuning_Var = 0; Tuning_Var < 12; Tuning_Var++)  {
    pinMode(Tuning_Pins[Tuning_Var], INPUT_PULLUP);
    Serial.println(Tuning_Var);
  }
  Serial.println("Actuator output pins assigned~ ");
  for (int Actuator_Var = 0; Actuator_Var < 6; Actuator_Var++)  {
    pinMode(Actuator_Pins[Actuator_Var], OUTPUT);
    Serial.println(Actuator_Var);
  }
}
void loop() {
  delay(5);
  //  Actuator_Last = Actuator_Pins[Note_Var];
  if (digitalRead(Half_Bend_Pin) == LOW) {
    TuningValue = 400;
    //TuningValue = analogRead(Tuning_Pins[Note_Var]);
    if (debug == 1 && Hide_Serialprint_Half != 1) {
      Serial.println("Half Note");
      Hide_Serialprint_Half = 1;
    }
  }
  else {
    TuningValue = 800;
    //TuningValue = analogRead(Tuning_Pins[Note_Var]) * 2;
    if (debug == 1 && Hide_Serialprint_Half != 2) {
      Serial.println("Full Note");
      Hide_Serialprint_Half = 2;
    }
  }
  if (digitalRead(Speed_Pins[0]) == LOW) {
    Bendspeed_new = Actuator_Delay_Array[0];
  }
  if (digitalRead(Speed_Pins[1]) == LOW) {
    Bendspeed_new = Actuator_Delay_Array[1];
  }
  if (digitalRead(Speed_Pins[2]) == LOW) {
    Bendspeed_new = Actuator_Delay_Array[2];
  }
  if (digitalRead(Speed_Pins[0]) == HIGH
      && digitalRead(Speed_Pins[1]) == HIGH
      && digitalRead(Speed_Pins[2]) == HIGH)
  {
    Bendspeed_new = Actuator_Delay_Array[3];
  }
  if (Bendspeed_new != Bendspeed_Last) {
    Bendspeed_Last = Bendspeed_new;
    if (debug == 1) {
      Serial.println("Bend speed");
      Serial.println(Bendspeed_new);
    }
  }
  for (int Note_Var = 0; Note_Var < 6; Note_Var++)  {
    if (digitalRead(Note_Pins[Note_Var]) == LOW) {
      Actuator_Array[Note_Var] = 1;
      note_on = 1;
    }
    else {
      Actuator_Array[Note_Var] = 0;
      note_on = 0;
    }
    if (Hide_Serialprint_Note[Note_Var] != Actuator_Array[Note_Var]
        && debug == 1) {
      if (note_on == 1) {
        Serial.println("Servo # on");
      }
      else {
        Serial.println("Servo # off");
      }
      Serial.println(Note_Var);
      Hide_Serialprint_Note[Note_Var] = 0;
    }
    Hide_Serialprint_Note[Note_Var] = Actuator_Array[Note_Var];
  }
}
int Actuator_Last;                     //sends servo pin to void ActuatorPulse()

No, it doesn't.

The potentiometers for the fine tuning are in the mail so none of that code is written in, but I think I have it from here. Thank you to everyone who posted help.

#include <Servo.h>
int Note_Pins[] = {22, 24, 26, 28, 30, 32};
int Note_Var;
int Half_Bend_Pin = 34;
int Half_Bend = 2;
int Actuator_Pins[6] = {2, 3, 4, 5, 6, 7};
int Speed_Pins[] = {36, 38, 40};
int Speed_Pin_Var;
int Actuator_Delay_Array[] = {150, 70, 1, 40};
int Bendspeed_new;
int Bendspeed_Last;
int Tuning_Pins[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11};
int Tuning_Var;
int TuningValue;
int GearShift;
int FirstGear = 50;
int AngleRaw;
int Angle_Array[6] = {0, 0, 0, 0, 0, 0};
int Bend_Apex = 30;
int Bend_Max = 100;

int Hide_Serialprint_Half = 1;
int debug = 1;

Servo Actuator_E;
Servo Actuator_A;
Servo Actuator_D;
Servo Actuator_g;
Servo Actuator_b;
Servo Actuator_e;

void setup() {
  AngleRaw = 0;
  if (debug == 1) {
    Serial.begin(9600);
  }
  delay(10);
  pinMode(Half_Bend_Pin, INPUT_PULLUP);
  Serial.println("Half/Full input pin assigned");
  Serial.println(Half_Bend_Pin);
  Serial.println("Note input pins assigned");
  for (int Note_Var = 0; Note_Var < 6; Note_Var++)  {
    pinMode(Note_Pins[Note_Var], INPUT_PULLUP);
    Serial.println(Note_Pins[Note_Var]);
  }
  Serial.println("Bend speed pins assigned");
  for (int Speed_Pin_Var = 0; Speed_Pin_Var < 3; Speed_Pin_Var++)  {
    pinMode(Speed_Pins[Speed_Pin_Var], INPUT_PULLUP);
    Serial.println(Speed_Pins[Speed_Pin_Var]);
  }
  Serial.println("Tuning input pins assigned~ ");
  for (int Tuning_Var = 0; Tuning_Var < 12; Tuning_Var++)  {
    pinMode(Tuning_Pins[Tuning_Var], INPUT_PULLUP);
    Serial.println(Tuning_Pins[Tuning_Var]);
  }
  Actuator_E.attach(Actuator_Pins[0]);
  Actuator_A.attach(Actuator_Pins[1]);
  Actuator_D.attach(Actuator_Pins[2]);
  Actuator_g.attach(Actuator_Pins[3]);
  Actuator_b.attach(Actuator_Pins[4]);
  Actuator_e.attach(Actuator_Pins[5]);
  Actuator_E.write(0);
  Actuator_A.write(0);
  Actuator_D.write(0);
  Actuator_g.write(0);
  Actuator_b.write(0);
  Actuator_e.write(0);
}

void loop() {
  if (digitalRead(Half_Bend_Pin) == LOW) {
    TuningValue = 400;
    //TuningValue = analogRead(Tuning_Pins[Note_Var]);
    if (debug == 1 && Hide_Serialprint_Half != 1) {
      Serial.println("Half Note");
      Hide_Serialprint_Half = 1;
    }
  }
  else {
    TuningValue = 800;
    //TuningValue = analogRead(Tuning_Pins[Note_Var]) * 2;
    if (debug == 1 && Hide_Serialprint_Half != 2) {
      Serial.println("Full Note");
      Hide_Serialprint_Half = 2;
    }
  }
  if (digitalRead(Speed_Pins[0]) == LOW) {
    Bendspeed_new = Actuator_Delay_Array[0];
    delay (Bendspeed_Last);
  }
  if (digitalRead(Speed_Pins[1]) == LOW) {
    Bendspeed_new = Actuator_Delay_Array[1];
    delay (Bendspeed_Last);
  }
  if (digitalRead(Speed_Pins[2]) == LOW) {
    Bendspeed_new = Actuator_Delay_Array[2];
    delay (Bendspeed_Last);
  }
  if (digitalRead(Speed_Pins[0]) == HIGH
      && digitalRead(Speed_Pins[1]) == HIGH
      && digitalRead(Speed_Pins[2]) == HIGH)
  {
    Bendspeed_new = Actuator_Delay_Array[3];
    delay (Bendspeed_Last);
  }
  if (Bendspeed_new != Bendspeed_Last) {
    Bendspeed_Last = Bendspeed_new;
    if (debug == 1) {
      Serial.println("Bend speed");
      Serial.println(Bendspeed_new);
    }
  }
  if (GearShift == 1) {
    delay(FirstGear);
  }
  for (int Note_Var = 0; Note_Var < 6; Note_Var++) {
    AngleRaw = Angle_Array[Note_Var];
    if (digitalRead(Note_Pins[Note_Var]) == LOW) {  //start bend
      AngleRaw = Angle_Array[Note_Var];
      if (AngleRaw < Bend_Max) {
        AngleRaw += 5;
        Angle_Array[Note_Var] = AngleRaw;
        if (AngleRaw > Bend_Apex) {
          GearShift = 1;
          ActuatorPulse(AngleRaw, Note_Var);
        }
      }
      if (AngleRaw <= Bend_Apex) {
        GearShift = 0;
        ActuatorPulse(AngleRaw, Note_Var);
      }
    }
    else {                                          //return bend
      AngleRaw = Angle_Array[Note_Var];
      if (AngleRaw > 0) {
        AngleRaw -= 5;
        Angle_Array[Note_Var] = AngleRaw;
        if (AngleRaw < Bend_Apex) {
          GearShift = 0;
          ActuatorPulse(AngleRaw, Note_Var);
        }
      }
      if (AngleRaw >= Bend_Apex) {
        GearShift = 1;
        ActuatorPulse(AngleRaw, Note_Var);
      }
    }
  }
}

void ActuatorPulse(int AngleRaw, int Note_Var)
{
  switch (Note_Var) {
    case 0:
      Actuator_E.write(AngleRaw);
      break;
    case 1:
      Actuator_A.write(AngleRaw);
      break;
    case 2:
      Actuator_D.write(AngleRaw);
      break;
    case 3:
      Actuator_g.write(AngleRaw);
      break;
    case 4:
      Actuator_b.write(AngleRaw);
      break;
    case 5:
      Actuator_e.write(AngleRaw);
      break;
  }
  delay(2);
}