Help with making if statement routine more efficient

Projects General Guidance
1

I have two servos (servoX and ServoY) that are traveling a two different deltas via a table of values. Once the servos start to move, a LED is lit. When one of the two servos nears the end of its travel (the one with the greater amount of travel) needs to trigger the led to start fading down.

Untitled
Untitled1920×1080 146 KB

I have the following function running in my loop() called LEDhold(). It's purpose is to determine:
1st - if servoY has greater total travel than servoX,
2nd - then determine whether the angle is increasing or decreasing in value
3rd - if angle is increasing (servoXpos > servoXprev assuming that servoXpos - i.e. servo's target position is greater in value than the previous postion) read the servo's current position and then compare it to the final travel minus 15 degrees. If this condition is met, then fade the led down.
4th - if angle is decreasing (servoXpos < servoXprev) read the servo's current position and then compare it to the final travel plus 15 degrees. if this condition met, then fade led down

4th if servo X has greater total travel than servoY,
then a duplicate 1st - 3rd but all are servoX (instead of servoY)...

Is there a more efficent way to write this routine that you can think of? switch...case comes to mind as an alternative way to do it but not sure it would be more efficent since my switch(var) would have to be a number of boolean conditions?...

void LEDhold() {
  if (fadedir == HOLD) {                            // LEDhold is in the loop() and when another function fades up the 
                                                    // led's fadedir is changed from  ON to HOLD allowing for entering the LEDhold() function

    if (servoYtravel > servoYtravel) {              // if Y servo has more total travel than X servo
      if (servoYpos > servoYprev)                   // and if its target postion (servoYpos) is greater than its previous position
        if (servoY.read() >= (servoYpos - 15))      // and if its current position (angle is encreasing) is greater than or equal to the target postion minus 15 degrees
          fadedir = DOWN;                           // then fade led down
      if (servoYpos < servoYprev)                   // if the target position (servoYpos) is less than the previous position
        if (servoYstep <= (servoYpos + LEDdown))    // its current position (angle is decreasing) is less than or equal to the target postion plus 15 degrees
          fadedir = DOWN;
    }

if (servoXtravel > servoXtravel) {                  // if Y servo has more total travel than X servo
      if (servoXpos > servoXprev)                   // and if its target postion (servoYpos) is greater than its previous position
        if (servoX.read() >= (servoXpos - 15))      // and if its current position (angle is encreasing) is greater than or equal to the target postion minus 15 degrees
          fadedir = DOWN;                           // then fade led down
      if (servoXpos < servoXprev)                   // if the target position (servoYpos) is less than the previous position
        if (servoXstep <= (servoXpos + LEDdown))    // its current position (angle is decreasing) is less than or equal to the target postion plus 15 degrees
          fadedir = DOWN;
    }

  }
}
2

Lots of words.
Suppose You could express it in a graphic diagram?
Know that a picture tells more than thousand words.

3

servoYtravel > servoYtravel is always false.

servoXtravel > servoXtravel is always false.

Is this function working properly?

4

What kind of servos are you using? With normal servos, you cannot read where it is in its travel, you merely give them a position to travel to, and the hardware does that with feedback within itself, but with no actual positional feedback communicated back to the Arduino. I.e., if you do myServo.write(180), and an instantly print(myservo.read()), it will print "180" even if the Servo arm is just starting a 0.8s duration move and is still at 0°.

In the Servo library code, read(), just echos out the last write():

For example:

#include <Servo.h>

Servo myservo;  // create Servo object to control a servo

void setup() {
  Serial.begin(115200);
  myservo.attach(9);  // attaches the servo on pin 9 to the Servo object
}

void loop(){
  int target = 0;
  Serial.print(target);
  myservo.write(target);
  Serial.print(" ");
  Serial.print(myservo.read());
  Serial.print(" +2s...");
  delay(2000);
  Serial.print(myservo.read());
  Serial.println(" ");
  target = 180;
  Serial.print(target);
  Serial.print(" ");
  myservo.write(target);
  Serial.print(myservo.read());
  Serial.print(" +2s...");
  delay(2000);
  Serial.print(myservo.read());
  Serial.println(" ");
}

prints:

0 0 +2s...0 
180 180 +2s...180 
0 0 +2s...0 
180 180 +2s...180 
0 0 +2s...0 
180 180 +2s...180 
0 0 +2s...0

... with the read() instantly matching the targetted write() before any motion happens.

5

What is the task of the program in real life?

6

code looks very similar. Code duplicates are not very efficent.

Make a generic function or consider to use OOP.

7

which Servo library are you using ?

And are you controlling the movement degree by degree somewhere else in the code?

although no longer maintained (but working on "classic" arduinos) the VarSpeedServo library could possibly help you out.

8

Your description and comments don't quite match the code you posted. Taking a few guesses...

Using a struct instead of variables with coincidentally similar names would help.

struct Servo {
  int travel, pos, prev, step;
  int read();
  bool fadeDown() {
    if (pos > prev) {
      return read() >= pos - 15;
    } else if (pos < prev) {
      return step <= pos + LEDdown;
    }
    return false;  // equal
  }
} servoX, servoY;

The paths are mutually exclusive, so use else; however nothing happens when values are equal -- right? -- so you still need to have two tests.

void LEDhold() {
  if (fadedir == HOLD) {
    if (servoY.travel > servoX.travel) {
      if (servoY.fadeDown()) {
        fadedir = DOWN;
      }
    } else if {servoX.travel > servoY.travel) {
      if (servoX.fadeDown()) {
        fadedir = DOWN;
      }
    }
  }
}

It's possible to have a single fadedir = DOWN statement, but that's probably going too far to avoid duplication, making the code less clear.

You already have actual servo variables of some unspecified type that can do a read. There is a proper way to integrate it into the struct; or you can just hack it.

int Servo::read() {
  if (this == &servoX) {
    return actualX.read();
  } else {
    return actualY.read();
  }
}
9

Please post a compilable example, not a snippet with undefined, hidden initialization.

Since the Servo.h. Servo.read() function just gets the last Servo.write() angle, it would be more efficient to work with the setting part of the code.

The Servo write() portion or whatever sets Servo??Travel may be the only part of the code that has the information to schedule fading. For example:

maxTraveltime = max(servoXtravel,servoYtravel)*MsPerDegree;
brightInterval=max(0, maxTravelTime-FadeInterval);
if(currentMillis-travelStartMillis > brightInterval) fadeLed();
10

Thank you for the correction, no it is not, and correct statements are:

if (servoYpos > servoYprev)

if (servoXtravel > servoYtravel)

11

Good point, I did not realize that the servo.read() does not actually query the position of the hardware. This answers my question as to why it wasn't working for me. My alternative in my code is to count steps using a variable: servoXsteps

thanks

12

Have a look at the library I mentioned before - it might help you out

13

Very helpful. I had not considered using a struct, but have had experience with them in other projects. Thanks - I will explore this alternative method.

14

I've tried out VarSpeedServo and also slowmotionservo libraries. Both are excellent but don't give me the ablity to interact with my Led fading function.

15

Good point. I don't expect all of you to gues at what I'm doing, but didn't want to overwhelm you all with my entire sketch. This is essentially what I'm trying to do:

Untitled
Untitled1920×1080 146 KB

16

see reply to @Railroader

17

After trying other servo libraries I decided to just use the basic servo.h and control the speed of the servo using millis and stepping the servo angle. This allows me to query the position at any time thru the loop:

void move_servos() {
  if ((servos == ON) && (currMillis - prevspeedMillis > servospeed)) {  // if one or both servos are true and time is met to advance step of servo

    if ((servoXstate == false) && (servoYstate == false)) {
      servos = OFF;
      preveventMillis = currMillis;
      servoArraypos++;  // advance for next time around
    }

    if (servoXstep == servoXpos)
      servoXstate = false;
    if (servoYstep == servoYpos)
      servoYstate = false;

    if (servoXstate == true) {
      if (Xslope == UP)
        servoXstep++;
      else
        servoXstep--;
      servoX.write(servoXstep);
    }

    if (servoYstate) {
      if (Yslope == UP)
        servoYstep++;
      else
        servoYstep--;
      servoY.write(servoYstep);
    }
18 
#include <Servo.h>

Servo servoX;  // create servo object to control a servo
Servo servoY;


const int servoYpin = 6;  // the digital pin used for the servo
const int servoXpin = 5;  // the digital pin used for the servo
const int LEDpin = 3;

uint8_t servos;
uint8_t servoXstart = 90;
uint8_t servoXpos;  // inital position / 8 variable to read the servoXposue from the analog pin
uint8_t servoXprev;
uint16_t servoXtravel;
uint8_t servoYstart = 90;
uint8_t servoYpos;  // initial position / 8variable to read the servoXposue from the analog pin
uint8_t servoYprev;
uint16_t servoYtravel;

uint8_t Xslope;
uint8_t Yslope;

bool servoXstate = false;
bool servoYstate = false;

uint8_t fadedir;  // for led state
uint8_t servoXstep;
uint8_t servoYstep;

#define ON 4
#define OFF 0
#define UP 1
#define HOLD 2
#define DOWN 3

//////////////////////////////TIMING//////////////////////////////////
uint8_t eventOn;
uint8_t event_min = 3;
uint8_t event_max = 3;
uint8_t servospeed = 20;
uint8_t led_predelay = 250;  // milliseconds
uint8_t LEDdown = 25;       // x  degrees before end of travel LED is triggered to fade down
uint8_t fadespeed = 8;       // 5 good value   larger value is slower fade
//////////////////////////////END TIMING/////////////////////////////

int long currMillis;
int long preveventMillis;
int long prevspeedMillis;
int long prevLEDevent;
int long previousFadeMillis;

uint8_t sine_increment;
const uint8_t sine_table[96] PROGMEM = { 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 6, 7, 7, 8, 9, 10, 11, 12, 14, 15, 16, 18, 19, 21, 23, 25, 27, 29, 32,
                                         35, 38, 42, 63, 72, 83, 122, 152, 169, 181, 190, 198, 203, 208, 212, 216, 220, 223, 225, 228, 230, 232, 234, 236, 237, 239, 240, 242,
                                         243, 244, 245, 246, 247, 248, 248, 249, 250, 250, 251, 251, 252, 252, 253, 253, 253, 254, 254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255 };

uint8_t servoArraypos;
const int Arraypos = 3;
const int moveArray[2][3] PROGMEM = {
  // row 0 is Y servo 1 to 180 degrees   row 1 is X servo 45 to 135 degrees
  /* 0 */ { 0, 90, 180 },
  /* 1 */ { 45, 90, 135 },

};

void setup() {
  Serial.begin(115200);
  delay(700);
  servoX.attach(servoXpin, 530, 2500);
  servoY.attach(servoYpin, 530, 2500);
  randomSeed(analogRead(A1));  // floating ADC1 - pin 12 of attiny84
  servoX.write(servoXstart);
  servoY.write(servoYstart);
  servoXstep = servoXstart;
  servoYstep = servoYstart;
  servoArraypos = 0;
  servoXstate = false;
  servoYstate = false;
  servos = OFF;
  fadedir = OFF;
  Serial.println("starting program");
  delay(1000);
}

void loop() {
  // /*
  currMillis = millis();
  event();
  move_servos();
  LEDevent();
  doTheFade();
  LEDhold();
  // */
  //servo_sweep();
}


void event() {
  if ((servos == OFF) && (currMillis - preveventMillis > eventOn * 1000)) {  // both servos are false and event time is met
    Serial.println("event triggered");


    if (servoArraypos > (Arraypos - 1))  // if end of array is met reset to beginning
      servoArraypos = 0;

    servoXpos = pgm_read_byte(&moveArray[1][servoArraypos]);  // assign new target postion  from the array
    servoYpos = pgm_read_byte(&moveArray[0][servoArraypos]);

    Serial.print("array position = "), Serial.println(servoArraypos);

    Serial.print(" current X position = "), Serial.print((servoX.read())), Serial.print(" assigned X position = "), Serial.print(servoXpos), Serial.print(" current Y position = "), Serial.print((servoY.read())), Serial.print(" assigned Y position = "), Serial.println(servoYpos);
    delay(500);

    servoXtravel = abs(servoXprev - servoXpos);
    servoYtravel = abs(servoYprev - servoYpos);

    if (servoXpos > servoX.read())
      Xslope = UP, Serial.println(" X sloping UP ");
    else
      Xslope = DOWN, Serial.println(" X sloping DOWN ");
    delay(500);

    if (servoYpos > servoX.read())
      Yslope = UP;
    else
      Yslope = DOWN;

    servos = ON;
    servoXstate = true;
    servoYstate = true;
    fadedir = ON;
    prevLEDevent = currMillis;
    eventOn = random(event_min, event_max);  // chose new event time randomly for next time around
  }
}

void move_servos() {
  if ((servos == ON) && (currMillis - prevspeedMillis > servospeed)) {  // if one or both servos are true and time is met to advance step of servo

    if ((servoXstate == false) && (servoYstate == false)) {
      servos = OFF;
      preveventMillis = currMillis;
      servoArraypos++;  // advance for next time around
    }

    if (servoXstep == servoXpos)
      servoXstate = false;
    if (servoYstep == servoYpos)
      servoYstate = false;

    if (servoXstate == true) {
      if (Xslope == UP)
        servoXstep++;
      else
        servoXstep--;
      servoX.write(servoXstep);
    }

    if (servoYstate) {
      if (Yslope == UP)
        servoYstep++;
      else
        servoYstep--;
      servoY.write(servoYstep);
    }

    //Serial.print("current Xposition = "), Serial.print(servoX.read()); Serial.print("Servo X step ="), Serial.println(servoXstep);
    prevspeedMillis = currMillis;
  }
}

void LEDevent() {
  if ((fadedir == ON) && (currMillis - prevLEDevent >= led_predelay)) {  // led trigger is true and time lapses till fade direction is flagged to up
    Serial.println("led predelay met");
    fadedir = UP;
    sine_increment = 0;
  }
}


void doTheFade() {
  if (fadedir == UP) {
    if (currMillis - previousFadeMillis >= fadespeed) {
      //Serial.println("led fade up");
      int brightness = pgm_read_byte(&sine_table[sine_increment]);  // Read element from the array
      analogWrite(LEDpin, brightness);
      // // Serial.print (sine_increment); Serial //.print(" "); Serial //.println(brightness);                                               // for monitoring Serial // plot
      sine_increment++;

      if (sine_increment > 95) {
        fadedir = HOLD;
      }
      previousFadeMillis = currMillis;
    }
  }
  if (fadedir == DOWN) {
    if (currMillis - previousFadeMillis >= fadespeed) {
      //Serial.println("Led fade down");
      int brightness = pgm_read_byte(&sine_table[sine_increment]);  // Read element from the array
      analogWrite(LEDpin, brightness);
      // // Serial.print (sine_increment); Serial //.print(" "); Serial //.println(brightness);                                               // for monitoring Serial // plot
      sine_increment--;
      if (sine_increment <= 0) {
        fadedir = OFF;
        preveventMillis = currMillis;
      }
      previousFadeMillis = currMillis;
    }
  }
}

void LEDhold() {
  if (fadedir == HOLD) {  // LEDhold is in the loop() and when another function fades up the
                          // led's fadedir is changed from  ON to HOLD allowing for entering the LEDhold() function

    if (servoYtravel > servoXtravel) {            // if Y servo has more total travel than X servo
      if (servoYpos > servoYprev)                 // and if its target postion (servoYpos) is greater than its previous position
        if (servoYstep >= (servoYpos - LEDdown))    // and if its current position (angle is encreasing) is greater than or equal to the target postion minus 15 degrees
          fadedir = DOWN;                         // then fade led down
      if (servoYpos < servoYprev)                 // if the target position (servoYpos) is less than the previous position
        if (servoYstep <= (servoYpos + LEDdown))  // its current position (angle is decreasing) is less than or equal to the target postion plus 15 degrees
          fadedir = DOWN;
    }

    if (servoXtravel > servoYtravel) {            // if Y servo has more total travel than X servo
      if (servoXpos > servoXprev)                 // and if its target postion (servoYpos) is greater than its previous position
        if (servoXstep >= (servoXpos - 15))    // and if its current position (angle is encreasing) is greater than or equal to the target postion minus 15 degrees
          fadedir = DOWN;                         // then fade led down
      if (servoXpos < servoXprev)                 // if the target position (servoYpos) is less than the previous position
        if (servoXstep <= (servoXpos + LEDdown))  // its current position (angle is decreasing) is less than or equal to the target postion plus 15 degrees
          fadedir = DOWN;
    }
  }
}


void servo_sweep() {
  for (int pos = 0; pos <= 180; pos += 1) {  // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servoX.write(pos), servoY.write(pos);  // tell servo to go to position in variable 'pos'
    delay(15);                             // waits 15 ms for the servo to reach the position
  }
  delay(850);
  for (int pos = 180; pos >= 0; pos -= 1) {  // goes from 180 degrees to 0 degrees
    servoX.write(pos), servoY.write(pos);
    delay(15);  // waits 15 ms for the servo to reach the position4
  }
  delay(2000);
}
19

Hi, @mbiasotti!

Have you considered using steppers instead of servos?

20

No, thanks for the suggestion but too expensive for my application.