Servo setting up does not correspond with main sketch

I am setting up a number of sg90 servos to run turnouts and turntables on a model railway.
I have my main sketch which runs on a Nano with 4x PCA96585's connected.
I am using RS485 bus, the computer software is JMRI/CMRI.
All works fine but my trouble is setting up the servos accurately.
I have 2 basic sketches.
Sketch1 I connect each servo one at a time and find the positions I need,
I then transfer those positions to Sketch2 which runs the servos back and forth as confirmation.
I then transfer those figures to my main sketch, but they do not match up and the servos go to different positions.
Any help please much appreciated.
Eddy

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#define button1  6
#define button2  7
#define button3  8
int Mid =348;
int x = 348;
byte Button1Pressed;
byte Button2Pressed;
byte Button3Pressed;

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
uint8_t servonum = 0;


void setup() {
  Serial.begin(115200);
  pinMode(button1, INPUT_PULLUP); 
  pinMode(button2, INPUT_PULLUP); 
  pinMode(button3, INPUT_PULLUP); 
  // put your setup code here, to run once:
  Serial.println(x);

  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates not sure about this myself but will stick with it
  pwm.setPWM(0, 0, Mid);
}

void loop() {
  Button1Pressed = digitalRead(button1);
  Button2Pressed = digitalRead(button2);
  Button3Pressed = digitalRead(button3);

  if (Button1Pressed == LOW){
    x = x+5;
    delay(5);  
  }

  if (Button2Pressed == LOW){
    x = x-5;
    delay(5);  
  }

  if (Button3Pressed == LOW){
    x = (Mid);
    delay(5);  
  }

Serial.println(x);
pwm.setPWM(0, 0, x);
delay(500);
}
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN  338 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMID  348 // this should give 90 degs of movement
#define SERVOMAX  638 // this is the 'maximum' pulse length count (out of 4096)

// our servo # counter
uint8_t servonum = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("16 channel Servo test!");

  pwm.begin();
  
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates not sure about this myself but will stick with it

}

void loop() {

  pwm.setPWM(0, 0, SERVOMIN);
  delay(2000);
  pwm.setPWM(0, 0, SERVOMID);
  delay(2000);
  pwm.setPWM(0, 0, SERVOMAX);
  delay(2000);
  pwm.setPWM(0, 0, SERVOMID);
  delay(2000);
 
}
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#include <Auto485.h>
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40); //setup the board address
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
Adafruit_PWMServoDriver pwm3 = Adafruit_PWMServoDriver(0x42);
Adafruit_PWMServoDriver pwm4 = Adafruit_PWMServoDriver(0x43);
Auto485 bus(2); // Arduino pin 2 -> MAX485 DE and RE pins
CMRI cmri(5, 24, 48, bus); //CMRI node & Nano number

int Tbit[48];          //Max bits allowed from CMRI
int currentPos1[16];   // current positions for 16 servos per board
int currentPos2[16];
int currentPos3[16];
int currentPos4[16];

void setup() {
  Serial.begin(115200);
  bus.begin(115200);     //Starts the RS485 bus
  pwm1.begin();
  pwm1.setPWMFreq(60); // This is the maximum PWM frequency
  pwm2.begin();
  pwm2.setPWMFreq(60);
  pwm3.begin();
  pwm3.setPWMFreq(60);
  pwm4.begin();
  pwm4.setPWMFreq(60);

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

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

  for (int i = 0; i < 16; i++)
  {
    pwm4.setPWM(i, 0, 348);  // set initial position
    currentPos4[i] = 348;    // and remember it as current
  }
}
void loop() {
  cmri.process();
  for (int i=0; i<48; i++){
  Tbit[i] = cmri.get_bit(i);
}
  int angle;
  int i;

//  if (Tbit[0] == 1) {      //Bit1 CT5001 received from CMRI when turnout/switch thrown.
//    slowMove1(9, 1, 145);  //CT5001 offset by 1 from Tbit[0].
//  }                        // Moves servo 9 at speed rate 1 to position 145.
//  if (Tbit[0] == 0) {      //Bit0 CT5001 received from CMRI.
//    slowMove1(9, 1, 400);  //Moves servo 9 speed rate 1 to position 400.
//  }
//  if (Tbit[1] == 1) {
//    slowMove1(10, 1, 145);//CT5002
//  }
//  if (Tbit[1] == 0) {
//    slowMove1(10, 1, 400);//CT5002
//  }
//  if (Tbit[2] == 1) {
//    slowMove1(11, 1, 145);//CT5003
//  }
//  if (Tbit[2] == 0) {
//    slowMove1(11, 1, 400);//CT5003
//  }
//  if (Tbit[3] == 1) {
//    slowMove1(12, 1, 145);//CT5004
//  }
//  if (Tbit[3] == 0) {
//    slowMove1(12, 1, 400);//CT5004
//  }
//   if (Tbit[4] == 1) {
//    slowMove1(13, 1, 145);//CT5005
//  }
//  if (Tbit[4] == 0) {
//    slowMove1(13, 1, 400);//CT5005
//  }
//  if (Tbit[5] == 1) {
//    slowMove1(14, 1, 145);//CT5006
//  }
//  if (Tbit[5] == 0) {
//    slowMove1(14, 1, 400);//CT5006
//  }
  if (Tbit[6] == 1) {
    slowMove1(15, 10, 350);//CT5007
  }
  if (Tbit[6] == 0) {
    slowMove1(15, 10, 440);//CT5007
  }
//  if (Tbit[7] == 1) {
//    slowMove4(0, 1, 145);//CT5008
//  }
//  if (Tbit[7] == 0) {
//    slowMove4(0, 1, 400);//CT5008
//  }
//  if (Tbit[8] == 1) {
//    slowMove4(1, 1, 145);//CT5009
//  }
//  if (Tbit[8] == 0) {
//    slowMove4(1, 1, 400);//CT5009
//  }
//  if (Tbit[9] == 1) {
//    slowMove4(2, 1, 145);//CT5010
//  }
//  if (Tbit[9] == 0) {
//    slowMove4(2, 1, 400);//CT5010
//  }
//  if (Tbit[10] == 1) {
//    slowMove1(9, 1, 145);//CT5011
//  }
//  if (Tbit[10] == 0) {
//    slowMove1(9, 1, 400);//CT5011
//  }
//  if (Tbit[11] == 1) {
//    slowMove1(10, 1, 145);//CT5012
//  }
//  if (Tbit[11] == 0) {
//    slowMove1(10, 1, 400);//CT5012
//  }
  if (Tbit[12] == 1) {
    slowMove2(0, 10, 338);//CT5013
  }
  if (Tbit[12] == 0) {
    slowMove2(0, 10, 638);//CT5013
  }
  if (Tbit[13] == 1) {
    slowMove2(1, 10, 413);//CT5014
  }
  if (Tbit[13] == 0) {
    slowMove2(1, 10, 703);//CT5014
  }
//   if (Tbit[14] == 1) {
//    slowMove2(2, 1, 145);//CT5015
//  }
//  if (Tbit[14] == 0) {
//    slowMove2(2, 1, 400);//CT5015
//  }
//  if (Tbit[15] == 1) {
//    slowMove2(3, 1, 145);//CT5016
//  }
//  if (Tbit[15] == 0) {
//    slowMove2(3, 1, 400);//CT5016
//  }
//  if (Tbit[16] == 1) {
//    slowMove2(4, 1, 145);//CT5017
//  }
//  if (Tbit[16] == 0) {
//    slowMove2(4, 1, 400);//CT5017
//  }
//  if (Tbit[17] == 1) {
//    slowMove4(0, 1, 145);//CT5018
//  }
//  if (Tbit[17] == 0) {
//    slowMove4(0, 1, 400);//CT5018
//  }
//  if (Tbit[18] == 1) {
//    slowMove4(1, 1, 145);//CT5019
//  }
//  if (Tbit[18] == 0) {
//    slowMove4(1, 1, 400);//CT5019
//  }
//  if (Tbit[19] == 1) {
//    slowMove4(2, 1, 145);//CT5020
//  }
//  if (Tbit[19] == 0) {
//    slowMove4(2, 1, 400);//CT5020
//  }
//  if (Tbit[20] == 1) {
//    slowMove1(9, 1, 145);//CT5021 
//  }
//  if (Tbit[20] == 0) {
//    slowMove1(9, 1, 400);//CT5021
//  }
//  if (Tbit[21] == 1) {
//    slowMove1(10, 1, 145);//CT5022
//  }
//  if (Tbit[21] == 0) {
//    slowMove1(10, 1, 400);//CT5022
//  }
//  if (Tbit[22] == 1) {
//    slowMove4(1, 1, 145);//CT5023
//  }
//  if (Tbit[22] == 0) {
//    slowMove4(1, 1, 400);//CT5023
//  }
//  if (Tbit[23] == 1) {
//    slowMove4(2, 1, 145);//CT5024
//  }
//  if (Tbit[23] == 0) {
//    slowMove4(2, 1, 400);//CT5024
//  }
//   if (Tbit[24] == 1) {
//    slowMove3(13, 1, 145);//CT5025
//  }
//  if (Tbit[24] == 0) {
//    slowMove3(13, 1, 400);//CT5025
//  }
//  if (Tbit[25] == 1) {
//    slowMove3(14, 1, 145);//CT5026
//  }
//  if (Tbit[25] == 0) {
//    slowMove3(14, 1, 400);//CT5026
//  }
//  if (Tbit[26] == 1) {
//    slowMove3(15, 1, 145);//CT5027
//  }
//  if (Tbit[26] == 0) {
//    slowMove3(15, 1, 400);//CT5027
//  }
//  if (Tbit[27] == 1) {
//    slowMove3(0, 1, 145);//CT5028
//  }
//  if (Tbit[27] == 0) {
//    slowMove3(0, 1, 400);//CT5028
//  }
//  if (Tbit[28] == 1) {
//    slowMove4(1, 1, 145);//CT5029
//  }
//  if (Tbit[28] == 0) {
//    slowMove4(1, 1, 400);//CT5029
//  }
//  if (Tbit[29] == 1) {
//    slowMove4(2, 1, 145);//CT5030
//  }
//  if (Tbit[29] == 0) {
//    slowMove4(2, 1, 400);//CT5030
//  }
//  if (Tbit[30] == 1) {
//    slowMove1(9, 1, 145);//CT5031
//  }
//  if (Tbit[30] == 0) {
//    slowMove1(9, 1, 400);//CT5031
//  }
//  if (Tbit[31] == 1) {
//    slowMove1(10, 1, 145);//CT5032
//  }
//  if (Tbit[31] == 0) {
//    slowMove1(10, 1, 400);//CT5032
//  }
//  if (Tbit[32] == 1) {
//    slowMove4(1, 1, 145);//CT5033
//  }
//  if (Tbit[32] == 0) {
//    slowMove4(1, 1, 400);//CT5033
//  }
//  if (Tbit[33] == 1) {
//    slowMove4(2, 1, 145);//CT5034
//  }
//  if (Tbit[33] == 0) {
//    slowMove4(2, 1, 400);//CT5034
//  }
//   if (Tbit[34] == 1) {
//    slowMove4(0, 1, 145);//CT5035
//  }
//  if (Tbit[34] == 0) {
//    slowMove4(0, 1, 400);//CT5035
//  }
//  if (Tbit[35] == 1) {
//    slowMove4(1, 1, 145);//CT5036
//  }
//  if (Tbit[35] == 0) {
//    slowMove4(1, 1, 400);//CT5036
//  }
//  if (Tbit[36] == 1) {
//    slowMove4(0, 1, 145);//CT5037
//  }
//  if (Tbit[36] == 0) {
//    slowMove4(0, 1, 400);//CT5037
//  }
//  if (Tbit[37] == 1) {
//    slowMove4(1, 1, 145);//CT5038
//  }
//  if (Tbit[37] == 0) {
//    slowMove4(1, 1, 400);//CT5038
//  }
//  if (Tbit[38] == 1) {
//    slowMove4(2, 1, 145);//CT5039
//  }
//  if (Tbit[38] == 0) {
//    slowMove4(2, 1, 400);//CT5039
//  }
//  if (Tbit[39] == 1) {
//    slowMove4(3, 1, 145);//CT5040
//  }
//  if (Tbit[39] == 0) {
//    slowMove4(3, 1, 400);//CT5040
//  }
//  if (Tbit[40] == 1) {
//    slowMove4(15, 1, 145);//CT5041
//  }
//  if (Tbit[40] == 0) {
//    slowMove4(15, 1, 400);//CT5041
//  }
//  if (Tbit[41] == 1) {
//    slowMove1(10, 1, 145);//CT5042
//  }
//  if (Tbit[41] == 0) {
//    slowMove1(10, 1, 400);//CT5042
//  }
//  if (Tbit[42] == 1) {
//    slowMove4(1, 1, 145);//CT5043
//  }
//  if (Tbit[42] == 0) {
//    slowMove4(1, 1, 400);//CT5043
//  }
//  if (Tbit[43] == 1) {
//    slowMove4(2, 1, 145);//CT5044
//  }
//  if (Tbit[43] == 0) {
//    slowMove4(2, 1, 400);//CT5044
//  }
//   if (Tbit[44] == 1) {
//    slowMove4(0, 1, 145);//CT5045
//  }
//  if (Tbit[44] == 0) {
//    slowMove4(0, 1, 400);//CT5045
//  }
//  if (Tbit[45] == 1) {
//    slowMove4(1, 1, 145);//CT5046
//  }
//  if (Tbit[45] == 0) {
//    slowMove4(1, 1, 400);//CT5046
//  }
//  if (Tbit[46] == 1) {
//    slowMove4(2, 1, 145);//CT5047
//  }
//  if (Tbit[46] == 0) {
//    slowMove4(2, 1, 400);//CT5047
//  }
//  if (Tbit[47] == 1) {
//    slowMove4(0, 1, 145);//CT5048
//  }
//  if (Tbit[47] == 0) {
//    slowMove4(0, 1, 400);//CT5048
//  }
  }

// 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 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
}
void slowMove3(int servoNum, int delayTime, int to)
{
  if (currentPos3[servoNum] == to) return;  // Nothing to do if it's already there

  else if (to > currentPos3[servoNum])
  {
    for (int i = currentPos3[servoNum]; i < to; i++)
    {
      pwm3.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  else if (currentPos3[servoNum] > to)
  {
    for (int i = currentPos3[servoNum]; i > to; i--)
    {
      pwm3.setPWM(servoNum, 0, i);
      delay(delayTime);
    }
  }
  currentPos3[servoNum] = to;    // save the current servo position
}
void slowMove4(int servoNum, int delayTime, int to)
{
  if (currentPos4[servoNum] == to) return;  // Nothing to do if it's already there

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

post a schematic and an image of the servos in test and a schematic and picture is circuit.

Sorry I don't have a schematic. All I do is connect the wires!
Is it relevant to the positions not corresponding in the sketches?
eddy

Anyways, good luck to you.

Are the servo arms all connected in the same relative position? You have to set the servo to a known position before attaching the arm/wheel. I recommend position 369 which should be the midpoint (1500 microseconds) when running 60 Hz.

1/60 = 16666.666666667 microseconds per cycle
16666.666666667 / 4096 = 4.069010417 microseconds per LSB
1500 / 4.069010417 = 368.63 counts

I would expect the range of positions to be roughly 246 to 491 (1000 to 2000 microseconds).

John, yes, all servo arms set to the mid-point before attaching the arms.
I thought I could use the setup sketch to setup the servo while disconnected from the layout. Use the serial monitor to read the positions and transferee those figures to the main sketch.
Thanks for the formular but I am not sure how I use this to match the position figures in both sketches?

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.