Coding the Arduino for the N Gauge Layout (version 1)

By Andy Joel

This is an earlier version of the page, saved to keep a record of the development of the code.

What is an Arduino?

An Arduino is basically a computer. It is rather more basic than your standard computer, and has no easy way to hook up a keyboard or monitor (use a Raspberry Pi for that), but it is still a computer in that it can be programmed to do complex operations.

Here is an image.

The Arduino is the board sat on the breadboard (the white block) to the left.

It has a line of pin down each side long side, and these all extend into the bread board. Each has a specific use, and is labelled as such. In the image, the red leads are connected to a 5 V supply, and the black to GND (ground), for example.

Arduinos can be programmed using software on a computer, and the cable coming out to the left is a USB connection for that purpose. You can get the software here.

Introduction To Arduino Programming

Arduinos are programming using a language called C++. I think it is actually a reduced version, but that is generally not an issue, because the reduced version has what you need.

C++ is a very common programming language that can create small and fast programs, which is probably why it was chosen, though it is not my favourite language by some way… Seriously a language that can iterate through arrays, but not tell you how long they are?!?

Arduino programming is a little odd because the program is run numerous times every second. More specifically, it is the loop() method that does that, but that is the important part. You can stop the loop for a certain time using the delay() method,  but that stops everything, and the Arduino will not respond to inputs during a delay, so for our purpose it is to be avoided.

Instead, we have to move things on in increments. Imagine a point needs to change, so the angle on its servo needs adjusting. The Arduino tracks what angle the servo should be at at each moment in time. In the next loop it works out where it should have got to at that moment.

Before it does that, however, it also checks to see if the user has pressed a button, and decides if it has to also react to that.

A System for Controlling Servo

For our system, then, we need to store data about each servo, some of which can change – its current angle (current_angle)and the angle it should be (target_angle). On the other hand, some will be set up in configuration will never change – its “off” position (off_angle) and it “on” position (on_angle), as well as the controller it is connected to, (cluster), and its position on that controller (number).

In the loop()  method, we need to do three things:

  1. Determine the elapsed time.
  2. Check the state of every input, and react to that, modifying the target_angle of affected servos.
  3. Check the state of every servo (i.e., the values for the servo in the Arduino), and if the target_angle is different to the current_angle adjust the current_angle by a small increment, and then set the servo itself to that angle.

It would be great if we had a display on the layout that reports what the Arduino is doing (in addition to saying what the state of the points is). If the back of the layout does not end up looking like the flight deck of the enterprise, I will be very disappointed…

The Code

Here is an early version that so far only handles inputs on the Arduino, not an I2C module, split into numerous parts. It does handle servos on multiple I2C modules.

Preprocessor directives

The first part is the “preprocessor directives” – a hang over from the seventies, when computers were slow, and and this was done prior to compiling. The first two lines say we want to include two libraries – one for I2C communication, one for servos. The other lines set up constants. This test system has two PCA controllers and six servos.

In theory, up to 62 PCA controllers can be connected to the I2C bus, and this software is designed to handle that. You just need to set CONTROLLER_COUNT to the right number. Note that the addresses must be sequential.

Each controller can handle up to 16 servos. Servos do not have to be connected sequentially.  The means you could control up to nearly a thousand servos, but there may be an impact on performance. That said, I would guess that if you never have more than a dozen moving at the same time, it will not be noticeable. I found a cycles was around 5 ms with six going at one.

TIME_FACTOR is a scaling factor applied to all servos, so we can adjust them all in one go. this has to be a float.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

#define CONTROLLER_COUNT  2
#define CONTROLLER_START  0x40
#define TIME_FACTOR 1.0

// These lines taken from example code
#define SERVOMIN  150 // This is the 'minimum' pulse length count
#define SERVOMAX  600 // This is the 'maximum' pulse length count
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

#define WIRE Wire

Global Variables

There are four global variables that we will use later. One stores the references to the controllers, one each for the number of servos and switches, and one to store the time the previous loop started.

Adafruit_PWMServoDriver pwms[CONTROLLER_COUNT];
int servo_count;
int switch_count;
unsigned long previous_time;

Switch Data

The next chunk configures the switches.

Each switch is stored in a struct (short for structure). This is a collection of related data. The first one I am calling it SwitchData, and saying I want there to be three values stored in it; two integers and one Boolean (either true or false).

The next part then uses SwitchData. It sets up an array called switches, of the type SwitchData. The next six lines then create eight switches with the given data – this is the actual configuration. Note that the order of the data is that defined in the struct, so the first is on pin 5, modifies servo 1 to be turned on, while the second does the same but turns it off.

Note that pins 5 and 6 have two entries – they operate two servos at once.

struct SwitchData {
  int pin;
  int servo;
  bool turnOn;
};

SwitchData switches[] = {
  {5, 1, true},
  {6, 1, false},
  {5, 2, true},
  {6, 2, false},
  {7, 3, true},
  {8, 3, false},
  {9, 4, true},
  {10, 4, false},
  {11, 5, true},
  {12, 5, false},
};

Servo data

This is done in a similar manner, first defining a struct called ServoData. The has seven fields, all integers.

The each servo is configured. The first one in on controller 0, in position 0. Its “off” angle is 0, and its “on” angle is 180. Angles are specified in hundredths of a degree. It does not matter if the “off” angle is greater; they can be either way round.

struct ServoData {
  int cluster;  // 0 - 5 or so
  int number; // 0 - 15
  int off_angle;   // all angles in hundredths of a degree
  int on_angle;    // use ints to make comparsons easier
  int current_angle;
  int target_angle;
  int speed;
};

ServoData servos[] = {
  {0, 0, 0, 18000, 0, 18000, 1},
  {0, 1, 1000, 12000, 12000, 1000, 3}, // this one does not like to go beyond 120deg!
  {0, 3, 0, 9000, 9000, 0, 1},
  {1, 1, 0, 18000, 0, 18000, 1},
  {1, 2, 0, 18000, 0, 18000, 4},
  {1, 3, 0, 9000, 0, 9000, 4},
};

Custom Function

Next we create a custom function. I figured I would want to set the angle on a servo a lot, so a function just for that seemed a good idea.

The first line sets it up. The word void means it will not return a value, while setAngle is my name for it. We will pass data in the form of a ServoData type struct, and for the purposes of this function, it will be called servo.

The function basically works out the pulse length, pulselen, then sends a message to the PCA controller to set that on the actual servo. In between, it just checks we are not trying to set it to an illegal value.

void setAngle(ServoData servo) {
  int pulselen = servo.current_angle * (SERVOMAX - SERVOMIN) / 18000 + SERVOMIN;
  if (pulselen > SERVOMAX) {
    Serial.print("ERROR: Too high!!!");
    return;
  }
  if (pulselen < SERVOMIN) {
    Serial.print("ERROR: Too low!!!");
    return;
  }
  pwms[servo.cluster].setPWM(servo.number, 0, pulselen);
}

Be aware that by default C++ passes a copy to a function. If I was to change one of the values in servo in the function, that change would be lost when the function ends. This is different to most computer languages. I think you can change this behaviour by prefixing an ampersand to servo in the first line, but it is not an issue here, so I have not tried it.

setup()

The setup() function is standard for an Arduino. It us run once, when the Arduino is turned on.

The first two lines check the length of the two arrays. Then the serial interface with the computer is set up – this is useful for diagnostics during development.

The next three lines set up the input pins. This is done in a loop, where i is a counter. Note that arrays count from zero in C++ (in common with most other languages). The mode for each pin is set to INPUT_PULLUP, which is a built-in constant.

The next six lines set up communication with the PCA controllers. Again, this is done in a loop, with i being the count. The actual initialisation is pretty much copied from an example program.

There is a delay of 100 millisecond which is again copied from elsewhere, but gives everything a moment to start up. Then we record the time.

void setup() {
  servo_count = sizeof(servos)/sizeof(servos[0]);
  switch_count = sizeof(switches)/sizeof(switches[0]);

  Serial.begin(9600);

  for (int i = 0; i < switch_count; i++) {
    pinMode(switches[i].pin, INPUT_PULLUP);      // set pin to input
  }

  for (int i = 0; i < CONTROLLER_COUNT; i++) {
    pwms[i] = Adafruit_PWMServoDriver(CONTROLLER_START + i);
    pwms[i].begin();
    pwms[i].setOscillatorFrequency(27000000);
    pwms[i].setPWMFreq(SERVO_FREQ);
  }

  delay(100);
  
  previous_time = millis();
}

loop()

The loop() function is where the action happens. This is repeated hundreds of times every second.

loop: Handling time

This is complicated because of the types of numbers involved…

In the Arduino a standard integer can have a value from -32,768 to 32,767. A long integer, however, has twice the storage space, and can be from -2,147,483,648 to 2,147,483,647. The millis() function returns the number of milliseconds since the unit was turned on as an unsigned long integer, which can range from 0 to 4,294,967,295.

void loop() {
  unsigned long now_time = millis();
  unsigned long elapsed = now_time - previous_time;
  if (elapsed > 1000000) {
    elapsed = now_time;
  }
  previous_time = now_time;

Every 50 days, 4,294,967,295 milliseconds will pass, and the time will exceed the size of an unsigned long integer. What happens then? It goes back to zero.

I cannot see this happening for us, as everything is unplugged at the end of the day, but for completeness the code allows for that event. When that happens, the elapsed time will be almost 4,294,967,295

loop: Handling Inputs

We go though each switch, and see if it is in the LOW state. We set them earlier to use a pull-up resistor, so LOW indicates the switch is closed.

If it is, we set the target angle, to either the “on” position of the “off”

  // HANDLE INPUTS
  for (int i = 0; i < switch_count; i++) {
    if (digitalRead(switches[i].pin) == LOW) {
      ServoData servo = servos[switches[i].servo];
      if (switches[i].turnOn) {
        servos[switches[i].servo].target_angle = servo.on_angle;
      }
      else {
        servos[switches[i].servo].target_angle = servo.off_angle;
      }
    }
  }

Note that holding down the button will do no harm. In fact, holding down both the left and the right buttons at the same time will not cause any issues.

 

loop: Handling Outputs

Again we have a loop, with i as the counter, but now we are going through every servo. we check if the target angle is different to the current angle. If not, continue to the next iteration.

If it is different, we need to do something different depending on whether it is higher or lower, but in either case we cap the difference to the speed of the servo, modify the current angle, then use setAngle to adjust the actual servo.

 

  // HANDLE SERVOS
  for (int i = 0; i < servo_count; i++) {
    int diff = servos[i].current_angle - servos[i].target_angle;
    if (diff == 0) {
      continue;
    }
    int increment = TIME_FACTOR * elapsed * servos[i].speed;
    if (diff > 0) {
      if (diff > servos[i].speed) diff = increment;  // cap at speed
      servos[i].current_angle -= diff;
    }
    else {
      if (diff < servos[i].speed) diff = increment;  // cap at speed
      servos[i].current_angle += diff;
    }
    setAngle(servos[i]);
  }
  Serial.println(String(elapsed));
}

 

All Together Now…

The full code is here:

// Servo control via I2C bus version 0.1
// Copyright Andy Joel and Preston&District Model Railway Club
// Documentation here:
// http://www.prestonanddistrictmrs.org.uk/articles/point-control-with-servos/coding-the-arduino-for-the-n-gauge-layout/

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#define CONTROLLER_COUNT  2
#define CONTROLLER_START  0x40
#define TIME_FACTOR 1.0

#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

// Set I2C bus to use: Wire, Wire1, etc.
#define WIRE Wire

Adafruit_PWMServoDriver pwms[CONTROLLER_COUNT];
int servo_count;
int switch_count;
unsigned long previous_time;


struct SwitchData {
  int pin;
  int servo;
  bool turnOn;
};

SwitchData switches[] = {
  {5, 1, true},
  {6, 1, false},
  {5, 2, true},
  {6, 2, false},
  {7, 3, true},
  {8, 3, false},
  {9, 4, true},
  {10, 4, false},
  {11, 5, true},
  {12, 5, false},
};


struct ServoData {
  int cluster;  // 0 - 5 or so
  int number; // 0 - 15
  int off_angle;   // all angles in hundredths of a degree
  int on_angle;    // use ints to make comparsons easier
  int current_angle;
  int target_angle;
  int speed;
};

ServoData servos[] = {
  {0, 0, 0, 18000, 0, 18000, 1},
  {0, 1, 1000, 12000, 12000, 1000, 3}, // this one does not like to go beyond 120deg!
  {0, 3, 0, 9000, 9000, 0, 1},
  {1, 1, 0, 18000, 0, 18000, 1},
  {1, 2, 0, 18000, 0, 18000, 4},
  {1, 3, 0, 9000, 0, 9000, 4},
};


void setAngle(ServoData servo) {
  int pulselen = servo.current_angle * (SERVOMAX - SERVOMIN) / 18000 + SERVOMIN;
  if (pulselen > SERVOMAX) {
    Serial.print("ERROR: Too high!!!");
    return;
  }
  if (pulselen < SERVOMIN) {
    Serial.print("ERROR: Too low!!!");
    return;
  }
  pwms[servo.cluster].setPWM(servo.number, 0, pulselen);
}




void setup() {
  servo_count = sizeof(servos)/sizeof(servos[0]);
  switch_count = sizeof(switches)/sizeof(switches[0]);

  Serial.begin(9600);

  for (int i = 0; i < switch_count; i++) {
    pinMode(switches[i].pin, INPUT_PULLUP);           // set pin to input
  }

  for (int i = 0; i < CONTROLLER_COUNT; i++) {
    pwms[i] = Adafruit_PWMServoDriver(CONTROLLER_START + i);
    pwms[i].begin();
    pwms[i].setOscillatorFrequency(27000000);
    pwms[i].setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  }

  delay(100);
  
  //while (!Serial); // wait for output to fire up before sending. But what if it is not connected???
  //Serial.println("Found " + String(servo_count) + " servos!");  // does not appear
  previous_time = millis();
}



void loop() {
  // HANDLE TIME
  unsigned long now_time = millis();
  unsigned long elapsed = now_time - previous_time;
  previous_time = now_time;


  // HANDLE INPUTS
  for (int i = 0; i < switch_count; i++) {
    if (digitalRead(switches[i].pin) == LOW) {
      ServoData servo = servos[switches[i].servo];
      if (switches[i].turnOn) {
        servos[switches[i].servo].target_angle = servo.on_angle;
      }
      else {
        servos[switches[i].servo].target_angle = servo.off_angle;
      }
      Serial.println("servo_count:" + String(servo_count) + " (" + String(servos[5].target_angle) + ", " + String(servos[5].current_angle) + ")");
    }
  }


  // HANDLE SERVOS
  for (int i = 0; i < servo_count; i++) { int diff = servos[i].current_angle - servos[i].target_angle; if (diff == 0) { //Serial.print("."); continue; } // increment is how much the servo can change, given its speed and the elapsed time // We are taking a float, a long and an int, and converting to an int, but hopefully okay! // The calculation will be done as a float because it starts with a float, and converted on assignment int increment = TIME_FACTOR * elapsed * servos[i].speed; // diff is then capped at that if (diff > 0) {
      if (diff > servos[i].speed) diff = increment;  // cap at speed
      servos[i].current_angle -= diff;
      //Serial.print("-");
    }
    else {
      if (diff < servos[i].speed) diff = increment;  // cap at speed
      servos[i].current_angle += diff;
      //Serial.print("+");
    }
    setAngle(servos[i]);
  }
 
}