Decrease servo motor speed with PCA9586

Hi everyone, I am using Arduino mega2560 with 3 PCA9685 modules which I have connected 40 servomotors (16 for each module).

With this setting, I can control them through a software (JMRI) which allow the movement of the single servo from 19 ° to 47 ° (in frequency it is equal to 152/214).

The whole circuit works perfectly and now I would like to slow down the rotation speed of the servo.

I have tried many solutions including the For cycle with the result that the motor has actually slowed down but at the same time it “bypass” the variable “if” which the servo is moved only when it receives the command so now the servo repeat this action in infinite loop.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
#define CMRI_ADDR 1
#define DE_PIN 2
Auto485 bus(DE_PIN); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(CMRI_ADDR, 24, 48, bus); // deafltus to a SMINI with address 0. SMINI = 24 inputs, 48 outputs

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x41);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x42);

int Tbit1;
int Tbit2;
int Tbit3;
int Tbit4;
int Tbit5;
int Tbit6;
int Tbit7;
int Tbit8;
int Tbit9;
int Tbit10;
int Tbit11;
int Tbit12;
int Tbit13;
int Tbit14;
int Tbit15;
int Tbit16;
int Tbit17;
int Tbit18;
int Tbit19;
int Tbit20;
int Tbit21;
int Tbit22;
int Tbit23;
int Tbit24;
int Tbit25;
int Tbit26;
int Tbit27;
int Tbit28;
int Tbit29;
int Tbit30;
int Tbit31;
int Tbit32;
int Tbit33;
int Tbit34;
int Tbit35;
int Tbit36;
int Tbit37;
int Tbit38;
int Tbit39;
int Tbit40;
int Tbit41;

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

  pwm.begin();
  pwm.setPWMFreq(60);
  pwm.setPWM(0, 0, 214);
  pwm.setPWM(1, 0, 214);
  pwm.setPWM(2, 0, 214);
  pwm.setPWM(3, 0, 214);
  pwm.setPWM(4, 0, 214);
  pwm.setPWM(5, 0, 214);
  pwm.setPWM(6, 0, 214);
  pwm.setPWM(7, 0, 214);
  pwm.setPWM(8, 0, 214);
  pwm.setPWM(9, 0, 214);
  pwm.setPWM(10, 0, 214);
  pwm.setPWM(11, 0, 214);
  pwm.setPWM(12, 0, 214);
  pwm.setPWM(13, 0, 214);
  pwm.setPWM(14, 0, 214);
  pwm.setPWM(15, 0, 214);

  pwm1.begin();
  pwm1.setPWMFreq(60);
  pwm1.setPWM(0, 1, 214);
  pwm1.setPWM(1, 1, 214);
  pwm1.setPWM(2, 1, 214);
  pwm1.setPWM(3, 1, 214);
  pwm1.setPWM(4, 1, 214);
  pwm1.setPWM(5, 1, 214);
  pwm1.setPWM(6, 1, 214);
  pwm1.setPWM(7, 1, 214);
  pwm1.setPWM(8, 1, 214);
  pwm1.setPWM(9, 1, 214);
  pwm1.setPWM(10, 1, 214);
  pwm1.setPWM(11, 1, 214);
  pwm1.setPWM(12, 1, 214);
  pwm1.setPWM(13, 1, 214);
  pwm1.setPWM(14, 1, 214);
  pwm1.setPWM(15, 1, 214);

  pwm2.begin();
  pwm2.setPWMFreq(60);
  pwm2.setPWM(0, 2, 214);
  pwm2.setPWM(1, 2, 214);
  pwm2.setPWM(2, 2, 214);
  pwm2.setPWM(3, 2, 214);
  pwm2.setPWM(4, 2, 214);
  pwm2.setPWM(5, 2, 214);
  pwm2.setPWM(6, 2, 214);
  pwm2.setPWM(7, 2, 214);
  pwm2.setPWM(8, 2, 214);
  pwm2.setPWM(9, 2, 214);
  pwm2.setPWM(10, 2, 214);
  pwm2.setPWM(11, 2, 214);
  pwm2.setPWM(12, 2, 214);
  pwm2.setPWM(13, 2, 214);
  pwm2.setPWM(14, 2, 214);
  pwm2.setPWM(15, 2, 214);

  bus.begin(9600);
}

void loop() {

  cmri.process();
  Tbit1 = cmri.get_bit(0);
  Tbit2 = cmri.get_bit(1);
  Tbit3 = cmri.get_bit(2);
  Tbit4 = cmri.get_bit(3);
  Tbit5 = cmri.get_bit(4);
  Tbit6 = cmri.get_bit(5);
  Tbit7 = cmri.get_bit(6);
  Tbit8 = cmri.get_bit(7);
  Tbit9 = cmri.get_bit(8);
  Tbit10 = cmri.get_bit(9);
  Tbit11 = cmri.get_bit(10);
  Tbit12 = cmri.get_bit(11);
  Tbit13 = cmri.get_bit(12);
  Tbit14 = cmri.get_bit(13);
  Tbit15 = cmri.get_bit(14);
  Tbit16 = cmri.get_bit(15);
  Tbit17 = cmri.get_bit(16);
  Tbit18 = cmri.get_bit(17);
  Tbit19 = cmri.get_bit(18);
  Tbit20 = cmri.get_bit(19);
  Tbit21 = cmri.get_bit(20);
  Tbit22 = cmri.get_bit(21);
  Tbit23 = cmri.get_bit(22);
  Tbit24 = cmri.get_bit(23);
  Tbit25 = cmri.get_bit(24);
  Tbit26 = cmri.get_bit(25);
  Tbit27 = cmri.get_bit(26);
  Tbit28 = cmri.get_bit(27);
  Tbit29 = cmri.get_bit(28);
  Tbit30 = cmri.get_bit(29);
  Tbit31 = cmri.get_bit(30);
  Tbit32 = cmri.get_bit(31);
  Tbit33 = cmri.get_bit(32);
  Tbit34 = cmri.get_bit(33);
  Tbit35 = cmri.get_bit(34);
  Tbit36 = cmri.get_bit(35);
  Tbit37 = cmri.get_bit(36);
  Tbit38 = cmri.get_bit(37);
  Tbit39 = cmri.get_bit(38);
  Tbit40 = cmri.get_bit(39);
  Tbit41 = cmri.get_bit(40);

  if (Tbit1 == 1)
  {
    pwm.setPWM(0, 0, 152);
  }
  if (Tbit1 == 0)

  {
    pwm.setPWM(0, 0, 214);
  }
  if (Tbit2 == 1) {
    pwm.setPWM(1, 0, 152);
  }
  if (Tbit2 == 0) {
    pwm.setPWM(1, 0, 214);
  }
  if (Tbit3 == 1) {
    pwm.setPWM(2, 0, 152);
  }
  if (Tbit3 == 0) {
    pwm.setPWM(2, 0, 214);
  }
if (Tbit4 == 1) {
    pwm.setPWM(3, 0, 152);
  }
  if (Tbit4 == 0) {
    pwm.setPWM(3, 0, 214);
  }
  if (Tbit5 == 1) {
    pwm.setPWM(4, 0, 152);
  }
  if (Tbit5 == 0) {
    pwm.setPWM(4, 0, 214);
  }
  if (Tbit6 == 1) {
    pwm.setPWM(5, 0, 152);
  }
  if (Tbit6 == 0) {
    pwm.setPWM(5, 0, 214);
  }
  if (Tbit7 == 1) {
    pwm.setPWM(6, 0, 152);
  }
  if (Tbit7 == 0) {
    pwm.setPWM(6, 0, 214);
  }
  if (Tbit8 == 1) {
    pwm.setPWM(7, 0, 152);
  }
  if (Tbit8 == 0) {
    pwm.setPWM(7, 0, 214);
  }
  if (Tbit9 == 1) {
    pwm.setPWM(8, 0, 152);
  }
  if (Tbit9 == 0) {
    pwm.setPWM(8, 0, 214);
  }
  if (Tbit10 == 1) {
    pwm.setPWM(9, 0, 152);
  }
  if (Tbit10 == 0) {
    pwm.setPWM(9, 0, 214);
  }
  if (Tbit11 == 1) {
    pwm.setPWM(10, 0, 152);
  }
  if (Tbit11 == 0) {
    pwm.setPWM(10, 0, 214);
  }
  if (Tbit12 == 1) {
    pwm.setPWM(11, 0, 152);
  }
  if (Tbit12 == 0) {
    pwm.setPWM(11, 0, 214);
  }
  if (Tbit13 == 1) {
    pwm.setPWM(12, 0, 152);
  }
  if (Tbit13 == 0) {
    pwm.setPWM(12, 0, 214);
  }
  if (Tbit14 == 1) {
    pwm.setPWM(13, 0, 152);
  }
  if (Tbit4 == 0) {
    pwm.setPWM(13, 0, 214);
  }
  if (Tbit15 == 1) {
    pwm.setPWM(14, 0, 152);
  }
  if (Tbit15 == 0) {
    pwm.setPWM(14, 0, 214);
  }
  if (Tbit16 == 1) {
    pwm.setPWM(15, 0, 152);
  }
  if (Tbit16 == 0) {
    pwm.setPWM(15, 0, 214);
  }
  if (Tbit17 == 1) {
    pwm1.setPWM(0, 1, 152);
  }
  if (Tbit17 == 0) {
    pwm1.setPWM(0, 1, 214);
  }
  
   if (Tbit18 == 1){
   pwm1.setPWM(1, 1, 152);
   delay(15);
  }
  
 if (Tbit18 == 0){ 
   pwm1.setPWM(1, 1, 214);
    delay(15);
}  

if (Tbit19 == 1) {
  pwm1.setPWM(2, 1, 152);
}
if (Tbit19 == 0) {
  pwm1.setPWM(2, 1, 214);
}
if (Tbit20 == 1) {
  pwm1.setPWM(3, 1, 152);
}
if (Tbit20 == 0) {
  pwm1.setPWM(3, 1, 214);
}
if (Tbit21 == 1) {
  pwm1.setPWM(4, 1, 152);
}
if (Tbit21 == 0) {
  pwm1.setPWM(4, 1, 214);
}
if (Tbit22 == 1) {
  pwm1.setPWM(5, 1, 152);
}
if (Tbit22 == 0) {
  pwm1.setPWM(5, 1, 214);
}
if (Tbit23 == 1) {
  pwm1.setPWM(6, 1, 152);
}
if (Tbit23 == 0) {
  pwm1.setPWM(6, 1, 214);
}
if (Tbit24 == 1) {
  pwm1.setPWM(7, 1, 152);
}
if (Tbit24 == 0) {
  pwm1.setPWM(7, 1, 214);
}
if (Tbit25 == 1) {
  pwm1.setPWM(8, 1, 152);
}
if (Tbit25 == 0) {
  pwm1.setPWM(8, 1, 214);
}
if (Tbit26 == 1) {
  pwm1.setPWM(9, 1, 152);
}
if (Tbit26 == 0) {
  pwm1.setPWM(9, 1, 214);
}
if (Tbit27 == 1) {
  pwm1.setPWM(10, 1, 152);
}
if (Tbit27 == 0) {
  pwm1.setPWM(10, 1, 214);
}
if (Tbit28 == 1) {
  pwm1.setPWM(11, 1, 152);
}
if (Tbit28 == 0) {
  pwm1.setPWM(11, 1, 214);
}
if (Tbit29 == 1) {
  pwm1.setPWM(12, 1, 152);
}
if (Tbit29 == 0) {
  pwm1.setPWM(12, 1, 214);
}
if (Tbit30 == 1) {
  pwm1.setPWM(13, 1, 152);
}
if (Tbit30 == 0) {
  pwm1.setPWM(13, 1, 214);
}
if (Tbit31 == 1) {
  pwm1.setPWM(14, 1, 152);
}
if (Tbit31 == 0) {
  pwm1.setPWM(14, 1, 214);
}
if (Tbit32 == 1) {
  pwm1.setPWM(15, 1, 152);
}
if (Tbit32 == 0) {
  pwm1.setPWM(15, 1, 214);
}
if (Tbit33 == 1) {
  pwm2.setPWM(0, 2, 152);
}
if (Tbit33 == 0) {
  pwm2.setPWM(0, 2, 214);
}
if (Tbit34 == 1) {
  pwm2.setPWM(1, 2, 152);
}
if (Tbit34 == 0) {
  pwm2.setPWM(1, 2, 214);
}
if (Tbit35 == 1) {
  pwm2.setPWM(2, 2, 152);
}
if (Tbit35 == 0) {
  pwm2.setPWM(2, 2, 214);
}
if (Tbit36 == 1) {
  pwm2.setPWM(3, 2, 152);
}
if (Tbit36 == 0) {
  pwm2.setPWM(3, 2, 214);
}
if (Tbit37 == 1) {
  pwm2.setPWM(4, 2, 152);
}
if (Tbit37 == 0) {
  pwm2.setPWM(4, 2, 214);
}
if (Tbit38 == 1) {
  pwm2.setPWM(5, 2, 152);
}
if (Tbit38 == 0) {
  pwm2.setPWM(5, 2, 214);
}
if (Tbit39 == 1) {
  pwm2.setPWM(6, 2, 152);
}
if (Tbit39 == 0) {
  pwm2.setPWM(6, 2, 214);
}
if (Tbit40 == 1) {
  pwm2.setPWM(7, 2, 152);
}
if (Tbit40 == 0) {
  pwm2.setPWM(7, 2, 214);
}
if (Tbit41 == 1) {
  pwm2.setPWM(8, 2, 152);
}
if (Tbit41 == 0) {
  pwm2.setPWM(8, 2, 214);
}
}

As far as I know for loops are the only practical way to slow down servos driven from PCA9685 (I assume that's what you mean). But if they are to work properly you always need to remember the current position of the servo. Then your for loop will be from currentPos to wantedPos.

If you post the for loop code that you have tried (a shortened version with just a few servos would be fine) then we can start from there. I can't help thinking that your code would be a lot simpler if you used arrays and maybe the odd function but that's another matter.

Steve

Hi Steve,

Thank you for your reply;

The for cycle code that partially worked for me, is this one:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
#define CMRI_ADDR 1
#define DE_PIN 2
Auto485 bus(DE_PIN); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(CMRI_ADDR, 24, 48, bus); // deafltus to a SMINI with address 0. SMINI = 24 inputs, 48 outputs

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

int Tbit1;
int Tbit2;
int Tbit3;
int Tbit4;
int Tbit5;


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

  pwm.begin();
  pwm.setPWMFreq(60);
  pwm.setPWM(0, 0, 214);
  pwm.setPWM(1, 0, 214);
  pwm.setPWM(2, 0, 214);
  pwm.setPWM(3, 0, 214);
  pwm.setPWM(4, 0, 214);
  pwm.setPWM(5, 0, 214);


  bus.begin(9600);
}

void loop() {

  cmri.process();
  Tbit1 = cmri.get_bit(0);
  Tbit2 = cmri.get_bit(1);
  Tbit3 = cmri.get_bit(2);
  Tbit4 = cmri.get_bit(3);
  Tbit5 = cmri.get_bit(4);
 

int angle;
int i;


   angle=19; // write wanted angle
   angle=map(angle,19,47,152,214);
   for(i=214; i>=angle; i=i-1)

  if (Tbit1 == 1){
    pwm.setPWM(0, 0, i);
    delay(15);
  }

   angle=47; // write wanted angle
   angle=map(angle,19,47,152,214);
   for(i=152; i<=angle; i=i+1)
  
  if (Tbit1 == 0) {
    pwm.setPWM(0, 0, i);
    delay(15);
  }

  if (Tbit2 == 1) {
    pwm.setPWM(1, 0, 152);
  }

  if (Tbit2 == 0) {
    pwm.setPWM(1, 0, 214);
  }

  if (Tbit3 == 1) {
    pwm.setPWM(2, 0, 152);
  }

  if (Tbit3 == 0) {
    pwm.setPWM(2, 0, 214);
  }

  if (Tbit4 == 1) {
    pwm.setPWM(3, 0, 152);
  }

  if (Tbit4 == 0) {
    pwm.setPWM(3, 0, 214);
  }

  if (Tbit5 == 1) {
    pwm.setPWM(4, 0, 152);
  }

  if (Tbit5 == 0) {
    pwm.setPWM(4, 0, 214);
  }
 
}

Do you need to do that remapping every time or do you only ever write one of two values in setPwm() 152 and 214 ?

Rather than replicating all that code many times I think I’d write a function called something like slowMove() that does the moving. If you give it arguments of servoNum, delayTime, from and to then it might look something like (untested!):

void slowMove(int servoNum, int delayTime, int from, int to)
{
  if (to > from)
  {
    for (int i = from; i < to; i++)   
    {
      pwm.setPwm(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else
  {
    for (int i = from; i > to; i--)
    {
      pwm.setPwm(servoNum, 0, i);
      delay(delayTime);
    }
  }
}

then when you want a slow move you could just call that in your main code instead of the pwm.setPWM() that you originally had. If you really need the to and from values to be angles rather than pulse lengths you can add the mapping in there and since I’ve just remembered you have 3 PCA boards you may want another argument or perhaps just 3 different versions of the function, your choice.

It’s a bit crude but it should be easy to understand.

Steve

OK thanks for the explanation. you have been very very clear!
The servomotor have to move a railway tornout, then the pulse lenght value must remain the same as I reported in the code (152 and 214).

Do I have to add the function that you wrote down below, after the void loop?

Function definitions like that can go anywhere provided they are not inside another function. Right at the end after the final } of loop() is a good place.

So long as you're clear on how and when to call the function just give it a try. It might need some modification because I haven't tested it.

Steve

What do you mean with: "then when you want a slow move you could just call that in your main code instead of the pwm.setPWM() that you originally had"

so if I understand correctly, do I have to replace "int servoNum, int delayTime, int from, int to" with the values in number?

Sorry but these are very basic questions. What is your programming experience? It sounds as though you have never used functions before. If that is so then perhaps this project is too much for you until you have a little more experience.

I'm happy to help someone get a program working but you have to put in some effort too. Have a look at Arduino - FunctionDeclaration and see if that helps.

Steve

O.k. I’ve modified your test code so Tbit2 uses slowMove(). It compiles but is not tested. Give it a try and if it works for you see if you can change all the other servos the same way.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
#define CMRI_ADDR 1
#define DE_PIN 2
Auto485 bus(DE_PIN); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(CMRI_ADDR, 24, 48, bus); // deafltus to a SMINI with address 0. SMINI = 24 inputs, 48 outputs

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

int Tbit1;
int Tbit2;
int Tbit3;
int Tbit4;
int Tbit5;


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

  pwm.begin();
  pwm.setPWMFreq(60);
  pwm.setPWM(0, 0, 214);
  pwm.setPWM(1, 0, 214);
  pwm.setPWM(2, 0, 214);
  pwm.setPWM(3, 0, 214);
  pwm.setPWM(4, 0, 214);
  pwm.setPWM(5, 0, 214);


  bus.begin(9600);
}

void loop() {
  cmri.process();
  Tbit1 = cmri.get_bit(0);
  Tbit2 = cmri.get_bit(1);
  Tbit3 = cmri.get_bit(2);
  Tbit4 = cmri.get_bit(3);
  Tbit5 = cmri.get_bit(4);

  int angle;
  int i;

  angle = 19; // write wanted angle
  angle = map(angle, 19, 47, 152, 214);
  for (i = 214; i >= angle; i = i - 1)

    if (Tbit1 == 1) {
      pwm.setPWM(0, 0, i);
      delay(15);
    }

  angle = 47; // write wanted angle
  angle = map(angle, 19, 47, 152, 214);
  for (i = 152; i <= angle; i = i + 1)

    if (Tbit1 == 0) {
      pwm.setPWM(0, 0, i);
      delay(15);
    }

  if (Tbit2 == 1) {
    //pwm.setPWM(1, 0, 152);
    slowMove(1, 15, 214, 152);
  }
  if (Tbit2 == 0) {
    //pwm.setPWM(1, 0, 214);
    slowMove(1, 15, 152, 214);
  }

  if (Tbit3 == 1) {
    pwm.setPWM(2, 0, 152);
  }
  if (Tbit3 == 0) {
    pwm.setPWM(2, 0, 214);
  }

  if (Tbit4 == 1) {
    pwm.setPWM(3, 0, 152);
  }
  if (Tbit4 == 0) {
    pwm.setPWM(3, 0, 214);
  }

  if (Tbit5 == 1) {
    pwm.setPWM(4, 0, 152);
  }
  if (Tbit5 == 0) {
    pwm.setPWM(4, 0, 214);
  }
}

void slowMove(int servoNum, int delayTime, int from, int to)
{
  if (to > from)
  {
    for (int i = from; i < to; i++)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else
  {
    for (int i = from; i > to; i--)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
}

Have fun - Steve

Thank you for the support, you're very kind.

I tried to "test" the sketch, both with tbit1 and with tbit2 but the servo always works in the same order;

It does a slow movement (and that's right) but it does it in infinite loop; if I press the button, it works correctly and then restart to move in loop independently. It moves from grade 19 to grade 47 slowly and then returns from grade 47 to grade 19 immediately, as if there was no delay.

The thing that I can't understand is why the IF variable is "bypassed", allowing the servo to work continuously and that's why I've asked support here.

O.k. now you've explained the problem more clearly I think I see what is going on. Because Tbitx is always going to be either 1 or 0 each time it finishes a move and goes back round loop() Tbitx is still set so the servo will do its last move again. You got away with it when you were just doing direct moves because when you repeatedly tell a servo to go to the same place it just stays where it is. But the slow moving methods all rely on you going to a position from another position.

So as I said earlier you need know where each servo is currently positioned so that you don't try to move it again if it is already where it should be. So basically you always need to keep a list of the last value you wrote to a particular servo. That's the sort of thing you really want to use arrays for. Have you ever programmed with arrays? Do you understand them? Using arrays will make much of your code a lot neater. See https://www.arduino.cc/en/Tutorial/Arrays

I'll try to sort out a simple version for your test program but it's going to be a real pain to do it for the full 3 PCA 40 servo version so you really will want to be able to program with arrays.

Steve

Ok, If you could do an example code (using the same values in the original sketch), it should be better for me to understand the "mechanism".

You're ignoring all of my questions. What is your programming experience? Have you ever written and used functions before? Do you understand and have you used arrays?

These things are useful to know because I don't want to waste a load of time explaining in great detail things that you already know. And I'm only trying to help you learn things...I'm not planning on writing your whole program for you.

Steve

Right here’s another attempt (again not tested). It uses an array currentPos to save the current servo positions into and because of that I’ve changed slowMove() so it doesn’t need a ‘from’ parameter just ‘to’. Most of the interesting work is done in that slowMove() function so do try to work out what it’s doing and ask if you don’t understand anything. In this version servos 1-4 should be moving slowly. Servo 0 is still your original code.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
#define CMRI_ADDR 1
#define DE_PIN 2
Auto485 bus(DE_PIN); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(CMRI_ADDR, 24, 48, bus); // deafltus to a SMINI with address 0. SMINI = 24 inputs, 48 outputs

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

int Tbit1;
int Tbit2;
int Tbit3;
int Tbit4;
int Tbit5;

int currentPos[5];   // current positions for 5 servos numbered 0 - 4

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

  pwm.begin();
  pwm.setPWMFreq(60);

  /*  This not needed, done with the for loop below
    pwm.setPWM(0, 0, 214);
    pwm.setPWM(1, 0, 214);
    pwm.setPWM(2, 0, 214);
    pwm.setPWM(3, 0, 214);
    pwm.setPWM(4, 0, 214);
  */
  // set and remember initial positions using array
  for (int i = 0; i < 5; i++)
  {
    pwm.setPWM(i, 0, 214);  // set initial position
    currentPos[i] = 214;    // and remember it as current
  }

  bus.begin(9600);
}

void loop() {
    cmri.process();
    Tbit1 = cmri.get_bit(0);
    Tbit2 = cmri.get_bit(1);
    Tbit3 = cmri.get_bit(2);
    Tbit4 = cmri.get_bit(3);
    Tbit5 = cmri.get_bit(4);
  
  int angle;
  int i;

  angle = 19; // write wanted angle
  angle = map(angle, 19, 47, 152, 214);
  for (i = 214; i >= angle; i = i - 1)

    if (Tbit1 == 1) {
      pwm.setPWM(0, 0, i);
      delay(15);
    }

  angle = 47; // write wanted angle
  angle = map(angle, 19, 47, 152, 214);
  for (i = 152; i <= angle; i = i + 1)

    if (Tbit1 == 0) {
      pwm.setPWM(0, 0, i);
      delay(15);
    }

  if (Tbit2 == 1) {
    //pwm.setPWM(1, 0, 152);
    slowMove(1, 15, 152);
  }
  if (Tbit2 == 0) {
    //pwm.setPWM(1, 0, 214);
    slowMove(1, 15, 214);
  }

  if (Tbit3 == 1) {
    slowMove(2, 15, 152);
  }
  if (Tbit3 == 0) {
    slowMove(2, 15, 214);
  }

  if (Tbit4 == 1) {
    slowMove(3, 15, 152);
  }
  if (Tbit4 == 0) {
    slowMove(3, 15, 214);
  }

  if (Tbit5 == 1) {
    slowMove(4, 15, 152);
  }
  if (Tbit5 == 0) {
    slowMove(4, 15, 214);
  }
}
// Checks to see if a move is needed using currentPos[]
// if so it uses for loop to move to position given in 'to' argument
// speed of the move is controlled by delayTime smaller delay = faster move

void slowMove(int servoNum, int delayTime, int to)
{
  if (currentPos[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos[servoNum])
  {
    for (int i = currentPos[servoNum]; i < to; i++)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else if (currentPos[servoNum] > to)
  {
    for (int i = currentPos[servoNum]; i > to; i--)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  currentPos[servoNum] = to;    // save the current servo position
}

Give it a try - Steve

ok as far as my programming level is concerned, it's a very basic level. The fact that I asked you to give some examples is because they facilitate me learning Arduino;

in the sense, reading only the theory, it may be that I cannot understand everything immediately and therefore through examples (maybe even specific) I can understand the programming logic. I spend at least 2/3 hours a day reading and consulting the guides until I get to understand how and why about codes etc. So, now that you have explained and given me a concrete example, I can study the sketch and understand the logic with which the Function you have entered and the array works in this case.

Anyway I tried the code you posted and now the servo works perfectly. So I'm going to edit the initial sketch.

Thank you very much for your patience and for what you do.

You're welcome and feel free to come back here if you have any problems with the full scale version.

One thing I can think of is that you'll need to make some small changes to slowMove() to cope with your 3 PWM boards because it currently assumes all servos are on the same board.

Steve

ok, I finished the sketch. When I press the key button, the servomotor 1 moves from the value 145 to the value 221 and then goes back and starts looping alone. If I press the button again, the servo stops when it reaches the value 221 again. I can say that it works correctly at 50%.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
#define CMRI_ADDR 1
#define DE_PIN 2
Auto485 bus(DE_PIN); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(CMRI_ADDR, 24, 48, bus); // deafltus to a SMINI with address 0. SMINI = 24 inputs, 48 outputs

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x41);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x42);

int Tbit1;
int Tbit2;
int Tbit3;
int Tbit4;
int Tbit5;
int Tbit6;
int Tbit7;
int Tbit8;
int Tbit9;
int Tbit10;
int Tbit11;
int Tbit12;
int Tbit13;
int Tbit14;
int Tbit15;
int Tbit16;
int Tbit17;
int Tbit18;
int Tbit19;
int Tbit20;
int Tbit21;
int Tbit22;
int Tbit23;
int Tbit24;
int Tbit25;
int Tbit26;
int Tbit27;
int Tbit28;
int Tbit29;
int Tbit30;
int Tbit31;
int Tbit32;
int Tbit33;
int Tbit34;
int Tbit35;
int Tbit36;


int currentPos[16];   // current positions for 16 servos numbered 0 - 15

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

  pwm.begin();
  pwm.setPWMFreq(60);

  // set and remember initial positions using array
  for (int i = 0; i < 16; i++)
  {
    pwm.setPWM(i, 0, 221);  // set initial position
    currentPos[i] = 221;    // and remember it as current
  }

  for (int i = 0; i < 16; i++)
  {
    pwm.setPWM(i, 1, 221);  // set initial position
    currentPos[i] = 221;    // and remember it as current
  }

  for (int i = 0; i < 16; i++)
  {
    pwm.setPWM(i, 2, 221);  // set initial position
    currentPos[i] = 221;    // and remember it as current
  }

  bus.begin(9600);
}

void loop() {

  cmri.process();
  Tbit1 = cmri.get_bit(0);
  Tbit2 = cmri.get_bit(1);
  Tbit3 = cmri.get_bit(2);
  Tbit4 = cmri.get_bit(3);
  Tbit5 = cmri.get_bit(4);
  Tbit6 = cmri.get_bit(5);
  Tbit7 = cmri.get_bit(6);
  Tbit8 = cmri.get_bit(7);
  Tbit9 = cmri.get_bit(8);
  Tbit10 = cmri.get_bit(9);
  Tbit11 = cmri.get_bit(10);
  Tbit12 = cmri.get_bit(11);
  Tbit13 = cmri.get_bit(12);
  Tbit14 = cmri.get_bit(13);
  Tbit15 = cmri.get_bit(14);
  Tbit16 = cmri.get_bit(15);
  Tbit17 = cmri.get_bit(16);
  Tbit18 = cmri.get_bit(17);
  Tbit19 = cmri.get_bit(18);
  Tbit20 = cmri.get_bit(19);
  Tbit21 = cmri.get_bit(20);
  Tbit22 = cmri.get_bit(21);
  Tbit23 = cmri.get_bit(22);
  Tbit24 = cmri.get_bit(23);
  Tbit25 = cmri.get_bit(24);
  Tbit26 = cmri.get_bit(25);
  Tbit27 = cmri.get_bit(26);
  Tbit28 = cmri.get_bit(27);
  Tbit29 = cmri.get_bit(28);
  Tbit30 = cmri.get_bit(29);
  Tbit31 = cmri.get_bit(30);
  Tbit32 = cmri.get_bit(31);
  Tbit33 = cmri.get_bit(32);
  Tbit34 = cmri.get_bit(33);
  Tbit35 = cmri.get_bit(34);


  if (Tbit1 == 1){
    slowMove0(0, 15, 145);
  }
  if (Tbit1 == 0) {
    slowMove0(0, 15, 221);
  }
  if (Tbit2 == 1) {
    slowMove0(1, 15, 145);
  }
  if (Tbit2 == 0) {
    slowMove0(1, 15, 221);
  }
  if (Tbit3 == 1) {
    slowMove0(2, 15, 145);
  }
  if (Tbit3 == 0) {
    slowMove0(2, 15, 221);
  }
  if (Tbit4 == 1) {
    slowMove0(3, 15, 145);
  }
  if (Tbit4 == 0) {
    slowMove0(3, 15, 221);
  }
  if (Tbit5 == 1) {
    slowMove0(4, 15, 145);
  }
  if (Tbit5 == 0) {
    slowMove0(4, 15, 221);
  }
  if (Tbit6 == 1) {
    slowMove0(5, 15, 145);
  }
  if (Tbit6 == 0) {
    slowMove0(5, 15, 221);
  }
  if (Tbit7 == 1) {
    slowMove0(6, 15, 145);
  }
  if (Tbit7 == 0) {
    slowMove0(6, 15, 221);
  }
  if (Tbit8 == 1) {
    slowMove0(7, 15, 145);
  }
  if (Tbit8 == 0) {
    slowMove0(7, 15, 221);
  }
  if (Tbit9 == 1) {
    slowMove0(8, 15, 145);
  }
  if (Tbit9 == 0) {
    slowMove0(8, 15, 221);
  }
  if (Tbit10 == 1) {
    slowMove0(9, 15, 145);
  }
  if (Tbit10 == 0) {
    slowMove0(9, 15, 221);
  }
  if (Tbit11 == 1) {
    slowMove0(10, 15, 145);
  }
  if (Tbit11 == 0) {
    slowMove0(10, 15, 221);
  }
  if (Tbit12 == 1) {
    slowMove0(11, 15, 145);
  }
  if (Tbit12 == 0) {
    slowMove0(11, 15, 221);
  }
  if (Tbit13 == 1) {
    slowMove0(12, 15, 145);
  }
  if (Tbit13 == 0) {
    slowMove0(12, 15, 221);
  }
  if (Tbit14 == 1) {
    slowMove0(13, 15, 145);
  }
  if (Tbit14 == 0) {
    slowMove0(13, 15, 221);
  }
  if (Tbit15 == 1) {
    slowMove0(14, 15, 145);
  }
  if (Tbit15 == 0) {
    slowMove0(14, 15, 221);
  }
  if (Tbit16 == 1) {
    slowMove0(15, 15, 145);
  }
  if (Tbit16 == 0) {
    slowMove0(15, 15, 221);
  }
  if (Tbit17 == 1) {
    slowMove1(0, 15, 145);
  }
  if (Tbit17 == 0) {
    slowMove1(0, 15, 221);
  }
  if (Tbit18 == 1) {
    slowMove1(1, 15, 145);
  }
  if (Tbit18 == 0) {
    slowMove1(1, 15, 221);
  }
  if (Tbit19 == 1) {
    slowMove1(2, 15, 145);
  }
  if (Tbit19 == 0) {
    slowMove1(2, 15, 221);
  }
  if (Tbit20 == 1) {
    slowMove1(3, 15, 145);
  }
  if (Tbit20 == 0) {
    slowMove1(3, 15, 221);
  }
  if (Tbit21 == 1) {
    slowMove1(4, 15, 145);
  }
  if (Tbit21 == 0) {
    slowMove1(4, 15, 221);
  }
  if (Tbit22 == 1) {
    slowMove1(5, 15, 145);
  }
  if (Tbit22 == 0) {
    slowMove1(5, 15, 221);
  }
  if (Tbit23 == 1) {
    slowMove1(6, 15, 145);
  }
  if (Tbit23 == 0) {
    slowMove1(6, 15, 221);
  }
  if (Tbit24 == 1) {
    slowMove1(7, 15, 145);
  }
  if (Tbit24 == 0) {
    slowMove1(7, 15, 221);
  }
  if (Tbit25 == 1) {
    slowMove1(8, 15, 145);
  }
  if (Tbit25 == 0) {
    slowMove1(8, 15, 221);
  }
  if (Tbit26 == 1) {
    slowMove1(9, 15, 145);
  }
  if (Tbit26 == 0) {
    slowMove1(9, 15, 221);
  }
  if (Tbit27 == 1) {
    slowMove1(10, 15, 145);
  }
  if (Tbit27 == 0) {
    slowMove1(10, 15, 221);
  }
  if (Tbit28 == 1) {
    slowMove1(11, 15, 145);
  }
  if (Tbit28 == 0) {
    slowMove1(11, 15, 221);
  }
  if (Tbit29 == 1) {
    slowMove1(12, 15, 145);
  }
  if (Tbit29 == 0) {
    slowMove1(12, 15, 221);
  }
  if (Tbit30 == 1) {
    slowMove1(13, 15, 145);
  }
  if (Tbit30 == 0) {
    slowMove1(13, 15, 221);
  }
  if (Tbit31 == 1) {
    slowMove1(14, 15, 145);
  }
  if (Tbit31 == 0) {
    slowMove1(14, 15, 221);
  }
  if (Tbit32 == 1) {
    slowMove1(15, 15, 145);
  }
  if (Tbit32 == 0) {
    slowMove1(15, 15, 221);
  }
  if (Tbit33 == 1) {
    slowMove2(0, 15, 145);
  }
  if (Tbit33 == 0) {
    slowMove2(0, 15, 221);
  }
  if (Tbit34 == 1) {
    slowMove2(1, 15, 145);
  }
  if (Tbit34 == 0) {
    slowMove2(1, 15, 221);
  }
  if (Tbit35 == 1) {
    slowMove2(2, 15, 145);
  }
  if (Tbit35 == 0) {
    slowMove2(2, 15, 221);
  }
 
}

void slowMove0(int servoNum, int delayTime, int to)
{
  if (currentPos[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos[servoNum])
  {
    for (int i = currentPos[servoNum]; i < to; i++)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else if (currentPos[servoNum] > to)
  {
    for (int i = currentPos[servoNum]; i > to; i--)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  currentPos[servoNum] = to;    // save the current servo position
}

void slowMove1(int servoNum, int delayTime, int to)
{
  if (currentPos[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos[servoNum])
  {
    for (int i = currentPos[servoNum]; i < to; i++)
    {
      pwm.setPWM(servoNum, 1, i);
      delay(delayTime);
    }
  }
  else if (currentPos[servoNum] > to)
  {
    for (int i = currentPos[servoNum]; i > to; i--)
    {
      pwm.setPWM(servoNum, 1, i);
      delay(delayTime);
    }
  }
  currentPos[servoNum] = to;    // save the current servo position
}

void slowMove2(int servoNum, int delayTime, int to)
{
  if (currentPos[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos[servoNum])
  {
    for (int i = currentPos[servoNum]; i < to; i++)
    {
      pwm.setPWM(servoNum, 2, i);
      delay(delayTime);
    }
  }
  else if (currentPos[servoNum] > to)
  {
    for (int i = currentPos[servoNum]; i > to; i--)
    {
      pwm.setPWM(servoNum, 2, i);
      delay(delayTime);
    }
  }
  currentPos[servoNum] = to;    // save the current servo position
}

That's fast work and a good effort but unfortunately but your attempts to handle the 3 different PCA9685 boards pwm, pwm1 and pwm2 will not work. The second parameter in setPWM() is NOT a board number as you seem to have assumed. You should have checked the Adafruit tutorial information. In slowMove1 and slowMove2 you will need to use the names pwm1 and pwm2 and the same in the initialisation for loops. And you also need separate currentPos arrays for each board e.g. currentPos1 and currentPos2.

Anyway the servos on the first board should more or less work so which is "the key button" and do you mean servo 1 or servo 0? And are you saying this problem happens only the first time a servo is addressed (i.e. first press wrong, all subsequent presses o.k.) or does it really only work 50% of the time (i.e. first press wrong, 2nd press o.k., 3rd press wrong again, 4th press right etc)?

As it is your description doesn't seem likely...the servos are initialised at 221 so there's no way the first command to a servo can make it move from 145 to 221. It's never been at 145 and at that point knows nothing about the number 145.

Steve

I compiled the sketch following your advices, now all the servomotors attached to PCA 0 are working properly. As for the other 2 PCAs, apparently they do not communicate with the Arduino card; in the sense, pressing the button, nothing moves (I changed more pins).

I answer to your questions:

The button I mentioned is the “open” / “closed” button in the software; in the sketch, the open / closed function is given by the “If Tbit == 0” (closed) and “if tbit == 1” (open).

By servo 1 or 0 etc I meant which pin the servo is attached to on the board.

The problem arose when I pressed the button the first time the servo reached the position, after which it began to loop around on its own.

Now however this problem is solved

I insert the complete sketch below. So I just need to understand why the other 2 PCAs don’t communicate with the Arduino board

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
#define CMRI_ADDR 1
#define DE_PIN 2
Auto485 bus(DE_PIN); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(CMRI_ADDR, 24, 48, bus); // deafltus to a SMINI with address 0. SMINI = 24 inputs, 48 outputs

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x41);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x42);

int Tbit1;
int Tbit2;
int Tbit3;
int Tbit4;
int Tbit5;
int Tbit6;
int Tbit7;
int Tbit8;
int Tbit9;
int Tbit10;
int Tbit11;
int Tbit12;
int Tbit13;
int Tbit14;
int Tbit15;
int Tbit16;
int Tbit17;
int Tbit18;
int Tbit19;
int Tbit20;
int Tbit21;
int Tbit22;
int Tbit23;
int Tbit24;
int Tbit25;
int Tbit26;
int Tbit27;
int Tbit28;
int Tbit29;
int Tbit30;
int Tbit31;
int Tbit32;
int Tbit33;
int Tbit34;


int currentPos[16];   // current positions for 16 servos numbered 0 - 15
int currentPos1[16];
int currentPos2[16];

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

  pwm.begin();
  pwm.setPWMFreq(60);

  // set and remember initial positions using array
  for (int i = 0; i < 16; i++)
  {
    pwm.setPWM(i, 0, 221);  // set initial position
    currentPos[i] = 221;    // and remember it as current
  }

  for (int i = 0; i < 16; i++)
  {
    pwm1.setPWM(i, 0, 221);  // set initial position
    currentPos1[i] = 221;    // and remember it as current
  }

  for (int i = 0; i < 16; i++)
  {
    pwm2.setPWM(i, 0, 221);  // set initial position
    currentPos2[i] = 221;    // and remember it as current
  }

  bus.begin(9600);
}

void loop() {

  cmri.process();
  Tbit1 = cmri.get_bit(0);
  Tbit2 = cmri.get_bit(1);
  Tbit3 = cmri.get_bit(2);
  Tbit4 = cmri.get_bit(3);
  Tbit5 = cmri.get_bit(4);
  Tbit6 = cmri.get_bit(5);
  Tbit7 = cmri.get_bit(6);
  Tbit8 = cmri.get_bit(7);
  Tbit9 = cmri.get_bit(8);
  Tbit10 = cmri.get_bit(9);
  Tbit11 = cmri.get_bit(10);
  Tbit12 = cmri.get_bit(11);
  Tbit13 = cmri.get_bit(12);
  Tbit14 = cmri.get_bit(13);
  Tbit15 = cmri.get_bit(14);
  Tbit16 = cmri.get_bit(15);
  Tbit17 = cmri.get_bit(16);
  Tbit18 = cmri.get_bit(17);
  Tbit19 = cmri.get_bit(18);
  Tbit20 = cmri.get_bit(19);
  Tbit21 = cmri.get_bit(20);
  Tbit22 = cmri.get_bit(21);
  Tbit23 = cmri.get_bit(22);
  Tbit24 = cmri.get_bit(23);
  Tbit25 = cmri.get_bit(24);
  Tbit26 = cmri.get_bit(25);
  Tbit27 = cmri.get_bit(26);
  Tbit28 = cmri.get_bit(27);
  Tbit29 = cmri.get_bit(28);
  Tbit30 = cmri.get_bit(29);
  Tbit31 = cmri.get_bit(30);
  Tbit32 = cmri.get_bit(31);
  Tbit33 = cmri.get_bit(32);
  Tbit34 = cmri.get_bit(33);


  if (Tbit1 == 1){
    slowMove0(0, 15, 145);
  }
  if (Tbit1 == 0) {
    slowMove0(0, 15, 221);
  }
  if (Tbit2 == 1) {
    slowMove0(1, 15, 145);
  }
  if (Tbit2 == 0) {
    slowMove0(1, 15, 221);
  }
  if (Tbit3 == 1) {
    slowMove0(2, 15, 145);
  }
  if (Tbit3 == 0) {
    slowMove0(2, 15, 221);
  }
  if (Tbit4 == 1) {
    slowMove0(3, 15, 145);
  }
  if (Tbit4 == 0) {
    slowMove0(3, 15, 221);
  }
  if (Tbit5 == 1) {
    slowMove0(4, 15, 145);
  }
  if (Tbit5 == 0) {
    slowMove0(4, 15, 221);
  }
  if (Tbit6 == 1) {
    slowMove0(5, 15, 145);
  }
  if (Tbit6 == 0) {
    slowMove0(5, 15, 221);
  }
  if (Tbit7 == 1) {
    slowMove0(6, 15, 145);
  }
  if (Tbit7 == 0) {
    slowMove0(6, 15, 221);
  }
  if (Tbit8 == 1) {
    slowMove0(7, 15, 145);
  }
  if (Tbit8 == 0) {
    slowMove0(7, 15, 221);
  }
  if (Tbit9 == 1) {
    slowMove0(8, 15, 145);
  }
  if (Tbit9 == 0) {
    slowMove0(8, 15, 221);
  }
  if (Tbit10 == 1) {
    slowMove0(9, 15, 145);
  }
  if (Tbit10 == 0) {
    slowMove0(9, 15, 221);
  }
  if (Tbit11 == 1) {
    slowMove0(10, 15, 145);
  }
  if (Tbit11 == 0) {
    slowMove0(10, 15, 221);
  }
  if (Tbit12 == 1) {
    slowMove0(11, 15, 145);
  }
  if (Tbit12 == 0) {
    slowMove0(11, 15, 221);
  }
  if (Tbit13 == 1) {
    slowMove0(12, 15, 145);
  }
  if (Tbit13 == 0) {
    slowMove0(12, 15, 221);
  }
  if (Tbit14 == 1) {
    slowMove0(13, 15, 145);
  }
  if (Tbit14 == 0) {
    slowMove0(13, 15, 221);
  }
  if (Tbit15 == 1) {
    slowMove0(14, 15, 145);
  }
  if (Tbit15 == 0) {
    slowMove0(14, 15, 221);
  }
  if (Tbit16 == 1) {
    slowMove0(15, 15, 145);
  }
  if (Tbit16 == 0) {
    slowMove0(15, 15, 221);
  }
  if (Tbit17 == 1) {
    slowMove1(0, 15, 145);
  }
  if (Tbit17 == 0) {
    slowMove1(0, 15, 221);
  }
  if (Tbit18 == 1) {
    slowMove1(1, 15, 145);
  }
  if (Tbit18 == 0) {
    slowMove1(1, 15, 221);
  }
  if (Tbit19 == 1) {
    slowMove1(2, 15, 145);
  }
  if (Tbit19 == 0) {
    slowMove1(2, 15, 221);
  }
  if (Tbit20 == 1) {
    slowMove1(3, 15, 145);
  }
  if (Tbit20 == 0) {
    slowMove1(3, 15, 221);
  }
  if (Tbit21 == 1) {
    slowMove1(4, 15, 145);
  }
  if (Tbit21 == 0) {
    slowMove1(4, 15, 221);
  }
  if (Tbit22 == 1) {
    slowMove1(5, 15, 145);
  }
  if (Tbit22 == 0) {
    slowMove1(5, 15, 221);
  }
  if (Tbit23 == 1) {
    slowMove1(6, 15, 145);
  }
  if (Tbit23 == 0) {
    slowMove1(6, 15, 221);
  }
  if (Tbit24 == 1) {
    slowMove1(7, 15, 145);
  }
  if (Tbit24 == 0) {
    slowMove1(7, 15, 221);
  }
  if (Tbit25 == 1) {
    slowMove1(8, 15, 145);
  }
  if (Tbit25 == 0) {
    slowMove1(8, 15, 221);
  }
  if (Tbit26 == 1) {
    slowMove1(9, 15, 145);
  }
  if (Tbit26 == 0) {
    slowMove1(9, 15, 221);
  }
  if (Tbit27 == 1) {
    slowMove1(10, 15, 145);
  }
  if (Tbit27 == 0) {
    slowMove1(10, 15, 221);
  }
  if (Tbit28 == 1) {
    slowMove1(11, 15, 145);
  }
  if (Tbit28 == 0) {
    slowMove1(11, 15, 221);
  }
  if (Tbit29 == 1) {
    slowMove1(12, 15, 145);
  }
  if (Tbit29 == 0) {
    slowMove1(12, 15, 221);
  }
  if (Tbit30 == 1) {
    slowMove1(13, 15, 145);
  }
  if (Tbit30 == 0) {
    slowMove1(13, 15, 221);
  }
  if (Tbit31 == 1) {
    slowMove1(14, 15, 145);
  }
  if (Tbit31 == 0) {
    slowMove1(14, 15, 221);
  }
  if (Tbit32 == 1) {
    slowMove1(15, 15, 145);
  }
  if (Tbit32 == 0) {
    slowMove1(15, 15, 221);
  }
  if (Tbit33 == 1) {
    slowMove2(0, 15, 145);
  }
  if (Tbit33 == 0) {
    slowMove2(0, 15, 221);
  }
  if (Tbit34 == 1) {
    slowMove2(1, 15, 145);
  }
  if (Tbit34 == 0) {
    slowMove2(1, 15, 221);
  }

}

void slowMove0(int servoNum, int delayTime, int to)
{
  if (currentPos[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos[servoNum])
  {
    for (int i = currentPos[servoNum]; i < to; i++)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else if (currentPos[servoNum] > to)
  {
    for (int i = currentPos[servoNum]; i > to; i--)
    {
      pwm.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  currentPos[servoNum] = to;    // save the current servo position
}

void slowMove1(int servoNum, int delayTime, int to)
{
  if (currentPos1[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos1[servoNum])
  {
    for (int i = currentPos1[servoNum]; i < to; i++)
    {
      pwm1.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else if (currentPos1[servoNum] > to)
  {
    for (int i = currentPos1[servoNum]; i > to; i--)
    {
      pwm1.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  currentPos1[servoNum] = to;    // save the current servo position
}

void slowMove2(int servoNum, int delayTime, int to)
{
  if (currentPos2[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos2[servoNum])
  {
    for (int i = currentPos2[servoNum]; i < to; i++)
    {
      pwm2.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else if (currentPos2[servoNum] > to)
  {
    for (int i = currentPos2[servoNum]; i > to; i--)
    {
      pwm2.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  currentPos2[servoNum] = to;    // save the current servo position
}