Servo test bench

1. Servo demo device

RC-servo test bench:

• 3 different sized rc-servos
• 3 different test programs
  
• 2 special servos (continuous & with feedback)

• Mechanical design
• Hardware
• Software

An one example of a project documentation

2. Mechanical design

The goal was to build a rigid and clean machine. Here are some examples of manufacturing methods used:

Laser cutting of the polycarbonate frame:

The panels for the box were laser cut from 10mm thick acrylic sheet. The sheet was twice as thick as the maximum cutting thickness of the machine, but the cutting was still successful except for some minor bending of the cuts. Laser cutting provides a clear and good finish to clear plastics. Some deformed cuts had to be milled afterwards. Milling leads to cloudy surfaces, as can be seen on the hole for the LDC screen. Acrylic was chosen because it lets the user also view the inner parts of the device. As a material it is quite heavy and brittle, but it seems to withstand the loads caused by the servos. 

Aluminium parts were milled:

Thin aluminium is an easy material to mill and therefore good for novice machinists. For the right parameters and overall safety, asking help from lab staff is highly recommended or even required.

Multiple custom parts were 3D-printed:

Finding the right parameters for the printer may need some iteration, as it can vary even between printers of the same model. For example, with the switch knobs it took four tries to succeed in manufacturing the right dimensions. The 3D-printers (the Ultimakers located in ADD-lab) have a short course which is required before operating the machines. By completing ENG-A1009, Practical Work Training with 3D-printers, you also gain one credit!

Holes were drilled and tapped to the frame:

Polycarbonate has a fairly low melting point, so it may get stuck into the drill flutes and cause damage to the drill bit. Also, making threads requires precision and careful use of the tools. Using screws to assemble the frame allows later disassembly and is way more rigid than hot glue or tape. 

3. Hardware

Under the polycarbonate panels of the device, there is quite a lot of electronics:

Power source  

• Converts the 230V alternating current to 12V direct current.
• Easy to use terminals with proper labels.
• Non-insulated, needs caution.
• Power cord was taken from an old coffee machine. 
• Strain relief as shown below was made by printing a clamp inside the device.
  
  

• Drops the voltage to 5V for Arduino.
• Needs a circuit board 

Arduino microcontroller

• A regular UNO or similar works fine.
• With enough motors, an Arduino with more pins is needed.

Motor shield 

• 16 channels -> 16 separate motors
• Uses I2C to communicate with Arduino

Servo motors

• Micro
• Medium
• Large
• Continuous 
• Feedback

Behaviour of the servos is inspected with a help of different sensors:

Load cell amplifiers

• Amplify the signal coming from the load cells.
• Placed in a solderable copper plated circuit board.
• HX711 is quite a common example and has a good library.

Load cells

• Two strain gauges per load cell.
• Cells need calibration.
• Many different maximum load ratings available. 

Optical switches

• For measuring the rotational speed of servos.
• Plates with holes were printed and attached to servos. Optical switch detects the holes when the servo rotates. The speed is then counted from the time taken to rotate the certain angle between the holes. 
• Need to be positioned precisely.

Optical switch and load cell.

To operate all this, a user interface is needed:

Display

• Connected to Arduino via I2C.
• Common model, available both with or without I2C module.

Buttons and toggle switches

3.1. About rc-servos

General information:

-RC servos (or hobby servos) are actuators designed primarily for radio-controlled vehicles.

-Good for mechanical actuation and controlling.

-The output shaft has a 180 degree turning radius, but the servo can be modified to rotate continuously.

-Receives internal feedback by a potentiometer. The servo limits itself according to the potentiometer signal.

-Arduino has a built-in “servo.h” library that contains the necessary commands.

-Using multiple servos or heavy-duty servos requires an external power source. A few small servos without load can be attached directly to Arduino pins without issues.

-RC servos cost between 1€ to 100€

-Servos are classified by physical dimensions. Other important features include maximum torque, speed and materials used.

Main components:

-Case/outer shell

  -Fastening points (or mounting tabs) at sides

-Dc motor

  -Operating voltage varies from 5 to 7,4 V. Smaller servos can run with voltages as low as 3,5 V.

-Gearbox

  -Increases torque, decreases speed

  -Made from plastic or metal

  -Physical limiter for turning

-Potentiometer

  -Connected to output shaft

  -Gives feedback of the servo’s current position, tells the servo’s internal controller when to stop when the control        signal changes

-Three (or four) leads

  -Voltage, ground, signal, (potentiometer reading)

  -Typical hobby level servos have three leads, as potentiometer reading is not needed outside of the servo in most      applications

https://electricalstudy.sarutech.com/servo-motor-servo-mechanism-theory-and-working-principle/index.html

Example specifications:

-4.6 kg, 0.13 sec, 22 g

-4.6 kg -> maximum torque 4.6 kg/cm -> approx. 0.45 Nm

-0.13 sec -> turns 60 degrees in 0.13 seconds

-22 g -> servo’s own mass, approx. 22 grams

Problems:

-Small movements only exert little torque

-Some servos have very unstable mounting tabs

-Vibration

-Cheap servos are not very durable or mechanically robust

Instructions and tips:

projects: https://www.hackster.io/projects/tags/servo?sort=respected

sparkfun tutorial: https://learn.sparkfun.com/tutorials/hobby-servo-tutorial

adafruit tuotrial: https://learn.adafruit.com/adafruit-arduino-lesson-14-servo-motors/overview

futaba rc servo choosing guide: https://www.futabarc.com/servos/servo-wizard.php

continuously rotating servo: https://learn.adafruit.com/modifying-servos-for-continuous-rotation/overview

online store: https://hobbyking.com/en_us/radios-servos/servos.html?___store=en_us

online store: http://fi.eurorc.com/category/18/servot

4. Software

This device includes multiple different sensors, actuators etc., so there are several libraries. Some of them may be helpful for other projects too. Most of the libraries are available on Arduino IDE.

This chapter contains the Arduino code which is used in the device.

4.1. main

/*########## Main program for the servo test bench ##########*/

#include "HX711.h"
#include "inc/operations.h"
#include "inc/interruptFunctions.h"
#include "inc/declarations.h"
#include 

/*----------DEFINITIONS AND VARIABLES----------*/ // The rest of the declarations and variables are in declarations.h

/*## Force sensors ##*/

HX711 small_servo_scale(8, 9);
HX711 med_servo_scale(10, 11);
HX711 big_servo_scale(12, 13);

HX711 scales[3] = {big_servo_scale, med_servo_scale, small_servo_scale};

/*## PWM servo driver ##*/

Adafruit_PWMServoDriver servohandler = Adafruit_PWMServoDriver();

/*## Servo info containers ##*/
// Some parts are commented out because of memory shortage

String big_servo_info[7] = {"Turnigy S8166M", "Large", /*"Mass:*/ "154 g", /*"Torque:*/ "33 kg.cm", /*"Speed:*/ "0,21 s/60 deg", "Voltage: 4,8-6 V", "Metal gears"};
String med_servo_info[7] = {"Towerpro MG996R", "Standard", /*"Mass:*/ "55 g", /*"Torque:*/ "9,4-11 kg.cm", /*"Speed:*/ "0,2-0,16 s/60 deg", "Voltage: 4,8-6 V", "Metal gears, digital"};
String small_servo_info[7] = {"Turnigy TG9E", "Micro", /*"Mass:*/ "9 g", /*"Torque:*/ "1,5 kg.cm", /*"Speed:*/ "0,12 s/60 deg", "Voltage: 4,8-6 V", "Plastic gears, analog"};
String continuous_servo_info[7] = {"Fitec FS90R", "Micro", /*"Mass:*/ "9 g", /*"Torque:*/ "1,3-1,5 kg.cm", /*"Speed:*/ "100-300 RPM", "Voltage: 4,8-6 V", "Continuous rotation"};
String feedback_servo_info[7] = {"Datan B2122", "Micro", /*"Mass:*/ "15,81 g", /*"Torque:*/ "1,8 kg.cm", /*"Speed:*/ "0,1 s/60 deg", "Voltage: 4,8-6 V", "Analog feedback"};

/*## Servos ##*/

// For the definition and detailed explanation of type c_servo, refer to operations.h
// General format: c_servo servo = {index, minpos, maxpos, pos, startpos, measure_start, led, arm_length, info};

c_servo big_servo = {3, 190, 585, 450, 450, 470, 8, 4.3, big_servo_info};
c_servo med_servo = {2, 125, 550, 520, 520, 260, 7, 3.0, med_servo_info};
c_servo small_servo = {1, 120, 485, 440, 440, 150, 6, 1.4, small_servo_info};
c_servo continuous_servo = {0, 255, 400, 345, 345, 0, 5, 0, continuous_servo_info};
c_servo feedback_servo = {4, 170, 580, 500, 500, 0, 9, 0, feedback_servo_info};

c_servo servos[5] = {continuous_servo, small_servo, med_servo, big_servo, feedback_servo};

/*## LCD ##*/

LiquidCrystal_I2C lcd(0x20, 16, 2);

/*## Misc ##*/

//uint16_t setpoints[5];
uint16_t average = 0;
//uint16_t reading = 0;
//byte index = 0;
bool startup = true;

/*--------DEFINITIONS AND VARIABLES END--------*/

/*------------------FUNCTIONS------------------*/

/*## Initializations ##*/

// Move each servo to its starting position
void init_servos() {
  servohandler.setPWMFreq(60);
  for (int i = 0; i < 5; i++) {
    servohandler.setPWM(servos[i].index, 0, servos[i].startpos);
  }
}

// Set the various pin modes and pullups for the button pins
void init_buttons() {
  pinMode(select_next_servo, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(select_next_servo), next_servo, FALLING);
  pinMode(select_prev_servo, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(select_prev_servo), previous_servo, FALLING);
  pinMode(switch_mode, INPUT);
  pinMode(tare_scales, INPUT);
  pinMode(toggle_info, INPUT);
  digitalWrite(switch_mode, HIGH);
  digitalWrite(tare_scales, HIGH);
  digitalWrite(toggle_info, HIGH);
}

// Set the calibration factors for each load cell amplifier
void init_scales() {
  big_servo_scale.set_scale(big_servo_calib_factor);
  med_servo_scale.set_scale(med_servo_calib_factor);
  small_servo_scale.set_scale(small_servo_calib_factor);
}

// Initialize pin change interrupts for digital pins 4 to 6

void init_PCint() {
  digitalWrite(4, HIGH);
  PCMSK2 |= bit (PCINT20);
  PCIFR |= bit (PCIF2);
  PCICR |= bit (PCIE2);
  digitalWrite(5, HIGH);
  PCMSK2 |= bit (PCINT21);
  PCIFR |= bit (PCIF2);
  PCICR |= bit (PCIE2);
  digitalWrite(6, HIGH);
  PCMSK2 |= bit (PCINT22);
  PCIFR |= bit (PCIF2);
  PCICR |= bit (PCIE2);
}

// Set the remaining pin modes
void init_misc() {
  pinMode(servo_control_pin, INPUT);    // Main potentiometer (controls servos)
  pinMode(feedback_pin, INPUT);         // Potentiometer of the feedback servo
}

/*## Switch to the next servo and/or operation mode and update the LCD accordingly ##*/

void update_selection() {
  if (digitalRead(toggle_info) == LOW) curr_mode = 4;
  else if (digitalRead(toggle_info) == HIGH) {
    if (curr_servo_index > 0 and curr_servo_index < 4) curr_mode = digitalRead(switch_mode);
    else if (curr_servo_index == 0) curr_mode = 3;
    else if (curr_servo_index == 4) curr_mode = 2;
  }
  if (ledchange) {                                     // Check whether the active LED should be changed
    for (int i = 0; i < 5; i++) {
      servohandler.setPWM(servos[i].led, 4095, 0);     // Turn off all other LEDs 
    }
    servohandler.setPWM(servos[curr_servo_index].led, 0, 4095);   // Turn on the desired LED
    ledchange = false;
  }
  if ((prev_servo_index != curr_servo_index) or startup) {    // Check if a change of active servo is queued
    move_to_start(servohandler, servos[prev_servo_index]);
    prev_servo_index = curr_servo_index;
    operation_mode = curr_mode;
    prev_mode = curr_mode;
    init_view(operation_mode, lcd);
  } else if (prev_mode != curr_mode) {      // Check if the operation mode has changed (evaluated only if the servo hasn't changed)
    operation_mode = curr_mode;
    prev_mode = curr_mode;
    init_view(operation_mode, lcd);
  }
}

/*## Tare all scales ##*/

void tare_all() {
  big_servo_scale.tare();
  med_servo_scale.tare();
  small_servo_scale.tare();
}

/*----------------END FUNCTIONS----------------*/

void setup() {
  init_buttons();
  init_scales();
  init_PCint();
  init_misc();
  lcd.init();
  lcd.backlight();
  lcd.print("Servo test bench");
  delay(2000);
  lcd.clear();
  lcd.print("Press 'Tare'");
  lcd.setCursor(0,1);
  lcd.print("to continue.");
  while (true) {
    if (digitalRead(tare_scales) == LOW) break;
    delay(100);
  }
  lcd.clear();
  lcd.print("Please wait");
  servohandler.begin();
  tare_all();
  init_servos();
  for (int i = 0; i < 5; i++) {
    servohandler.setPWM(servos[i].led, 0, 4095);
  }
  delay(1000);
  for (int i = 0; i < 5; i++) {
    servohandler.setPWM(servos[i].led, 4095, 0);
  }
  init_view(operation_mode, lcd);
  update_selection();
  startup = false;
  delay(200);
}

void loop() {
  update_selection();
  if (digitalRead(tare_scales) == LOW and operation_mode == 1) {
    lcd.setCursor(0,1);
    lcd.print("Zeroing scales       ");
    tare_all();
    lcd.setCursor(0,1);
    lcd.print("Scales zeroed          ");
    delay(1500);
    init_view(operation_mode, lcd);
  }
  /*reading = analogRead(servo_control_pin);
  setpoints[index] = reading;
  if (index++ > 4) index = 0;
  uint16_t sum = 0;
  for (int i = 0; i < 5; i++) sum += setpoints[i];
  average = sum/5;*/
  average = analogRead(servo_control_pin);
  if (operation_mode == 0) {
    servo_speed_test(servos[curr_servo_index], servohandler, lcd, counter, timer);
  } else if (operation_mode == 1) {
    switch (curr_servo_index) {
      case 1: servo_force_measurement(servos[curr_servo_index], servohandler, lcd, small_servo_scale, average);
              break;
      case 2: servo_force_measurement(servos[curr_servo_index], servohandler, lcd, med_servo_scale, average);
              break;
      case 3: servo_force_measurement(servos[curr_servo_index], servohandler, lcd, big_servo_scale, average);
              break;
      default: break;
    }
  } else if (operation_mode == 2) {
    servo_feedback_handling(feedback_servo, servohandler, lcd, feedback_pin, average);
  } else if (operation_mode == 3) {
    servo_continuous_drive(continuous_servo, servohandler, lcd, average);
  } else if (operation_mode == 4) {
    display_info(servos[curr_servo_index], lcd, tare_scales);
  }
  delay(3);
}

4.2. operations

#include "inc/operations.h"

/*## Functions to execute depending on the current operation mode ##*/

void servo_speed_test(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, volatile uint8_t& counter, volatile unsigned long& end_time) {
  unsigned long result, start_time;
  if (servo.pos > servo.measure_start) {                        // Move the servo to the measurement start point
    while (servo.pos > servo.measure_start) {
      pwm.setPWM(servo.index, 0, servo.pos);
      servo.pos--;
      delay(3);
    }
  } else {
    while (servo.pos < servo.measure_start) {
      pwm.setPWM(servo.index, 0, servo.pos);
      servo.pos++;
      delay(3);
    }
  }
  delay(500);
  if (servo.index < 3) {                                        // Speed test procedure for the small and standard servos
    counter = 0;
    while ((counter < 2) and (servo.pos < servo.maxpos)) {      // Move the servo until the optical switch finds the first hole in the plate
      pwm.setPWM(servo.index, 0, servo.pos);
      servo.pos++;
      delay(7);
    }
    delay(1000);
    start_time = micros();                                      // Get the start time
    pwm.setPWM(servo.index, 0, servo.maxpos);                   // Move servo to max position
    delay(300);
    result = (end_time - start_time)/1000;                      // The plate has two holes, the second of which will have triggered an ISR that updates the value of "end_time"
    delay(100);
  } else if (servo.index == 3) {                                // Same procedure as previously, but for the large servo. This servo has the opposite direction of rotation
    delay(500);                                                 // to the other two.
    counter = 0;
    while ((counter < 2) and (servo.pos > servo.minpos)) {
      pwm.setPWM(servo.index, 0, servo.pos);
      servo.pos--;
      delay(7);
    }
    delay(1000);
    start_time = micros();
    pwm.setPWM(servo.index, 0, servo.minpos);
    delay(300);
    result = (end_time - start_time)/1000;
    delay(100);
  }
  lcd.setCursor(0,1);
  if (servo.index == 3) {
    result *= 60;
    result /= 32;
  } else if ( servo.index == 2) {
    result *= 60;
    result /= 70;
  } else if (servo.index == 1) {
    result *= 60;
    result /= 75;
  }
  lcd.print(float(result)/1000);                  // Display the result (seconds taken for a 60-degree turn, s/60 deg)
  lcd.print(" s/60 deg            ");
  delay(2000);
  servo.pos = servo.maxpos;
}

void servo_force_measurement(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, HX711 scale, uint16_t avg) {
  static byte update_delay = 19;
  float value;
  float torque;
  uint16_t setpoint = avg;
  if (servo.index < 3) servo.pos = map(setpoint, 0, 1023, servo.maxpos, servo.minpos);      // Map the received position setpoint (avg) to match the servo's range of motion
  else servo.pos = map(setpoint, 0, 1023, servo.minpos, servo.maxpos);
  pwm.setPWM(servo.index, 0, servo.pos);
  if (update_delay++ >= 20) {                   // To prevent the LCD from flickering and keep the system responsive (as far as reading input is concerned),
    lcd.setCursor(4,1);                         // the display only updates every 20 function calls.
    value = scale.get_units();
    torque = servo.arm_length * value;
    lcd.print(torque);
    lcd.print(" kg.cm        ");
    update_delay = 0;
  }
}

// The following function is only usable for the feedback servo (index 4)
void servo_feedback_handling(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, byte feedback_pin, uint16_t avg) {
  static byte update_delay = 19;
  uint16_t setpoint = avg;
  int feedback_val = analogRead(feedback_pin);                                  // Read the value from the servo's external feedback pin
  //feedback_val = map(feedback_val, 0, 1023, servo.minpos, servo.maxpos);
  servo.pos = map(setpoint, 0, 1023, servo.minpos, servo.maxpos);
  pwm.setPWM(servo.index, 0, servo.pos);
  if (update_delay++ >= 20) {
    lcd.setCursor(9,0);
    lcd.print(servo.pos);
    lcd.setCursor(9,1);
    lcd.print(feedback_val);
    update_delay = 0;
  }
}

// The following function is only usable for the continuously rotating servo (index 0)
void servo_continuous_drive(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, uint16_t avg) {
  static byte update_delay = 19;
  uint16_t setpoint = avg;
  servo.pos = map(setpoint, 0, 1023, servo.maxpos, servo.minpos);
  pwm.setPWM(servo.index, 0, servo.pos);
  if (update_delay++ >= 20) {
    lcd.setCursor(9,1);
    lcd.print(servo.pos);
    lcd.print("    ");
    update_delay = 0;
  }
}

/*## Function for moving a servo to its starting position ##*/

void move_to_start(Adafruit_PWMServoDriver& pwm, c_servo& servo) {
  pwm.setPWM(servo.index, 0, servo.pos);
  if (servo.pos < servo.startpos) {
    while (servo.pos < servo.startpos) {
      pwm.setPWM(servo.index, 0, servo.pos);
      servo.pos++;
      delay(2);
    }
  }
  else if (servo.pos > servo.startpos) {
    while (servo.pos > servo.startpos) {
      pwm.setPWM(servo.index, 0, servo.pos);
      servo.pos--;
      delay(2);
    }
  }
  delay(500);
}

/*## Function for displaying various information about the selected servo ##*/

void display_info(c_servo& servo, LiquidCrystal_I2C& lcd, byte pin) {
  static unsigned long timer = millis();
  static unsigned long button_delay = millis();
  static byte section = 0;
  static byte character = 0;
  static bool first_print = true;
  static byte prev_servo = servo.index;
  if (prev_servo != servo.index) {
    first_print = true;
    section = 0;
    character = 0;
    prev_servo = servo.index;
  }
  if (digitalRead(pin) == LOW and millis() - button_delay > 200) {      // Checking for a button press (polling, not interrupt). Pressing the button
    character = 0;                                                      // switches to the next info section (i.e. from servo name to servo category
    first_print = true;
    if (section++ > 5) section = 0;
    button_delay = millis();
  }
  if (first_print) {
    first_print = false;
    lcd.setCursor(0,0);
    for (int i = 0; i < 16; i++) {
      if (i > servo.info[section].length() - 1) lcd.print(" ");
      else lcd.print(servo.info[section][i]);
    }
    timer = millis();
  }
  if (servo.info[section].length() > 16) {        // The following part makes the text scroll after a short delay, but only if the length of the
    if (millis() - timer > 2000) {                // text exceeds the screen dimensions
      lcd.setCursor(0,0);
      for (int i = character; i < character + 16; i++) {
        if (i > servo.info[section].length() - 1) lcd.print(" ");
        else lcd.print(servo.info[section][i]);
      }
      if (character++ > servo.info[section].length() - 16) {
        character = 0;
        lcd.setCursor(0,0);
        lcd.print("                ");
        lcd.setCursor(0,0);
        for (int i = 0; i < min(16, servo.info[section].length()); i++) {
              lcd.print(servo.info[section][i]);
        }
        timer = millis();
      } else timer = millis() - 1500;
    }
  }
}

/*## Function for initializing the LCD to match the current operating mode ##*/

void init_view(byte operation_mode, LiquidCrystal_I2C lcd) {
  lcd.clear();
  lcd.setCursor(0,0);
  switch (operation_mode) {
    case 0: lcd.print("Speed test: ");
            break;
    case 1: lcd.print("Torque: ");
            break;
    case 2: lcd.print("Setpoint: ");
            lcd.setCursor(0,1);
            lcd.print("Position: ");
            break;
    case 3: lcd.print("Setpoint: ");
            break;
    case 4: lcd.setCursor(0,1);
            lcd.print("Tare to continue");
    default: break;
  }
}

4.3. operations header

#ifndef OPERATIONS
#define OPERATIONS

#include 
#include 
#include 

// Struct for handling a servo's information. Positions are in terms of pulse width (0 to 4096)
typedef struct Customservo {
  uint8_t index;		// PWM output pin number (Adafruit servo shield)
  uint16_t minpos;		// Minimum position
  uint16_t maxpos;		// Maximum position
  uint16_t pos;			// Current position
  uint16_t startpos;		// Predetermined "safe" initial position
  uint16_t measure_start;	// Starting position for speed measurement (used only for small, standard, and large servos)
  byte led;			// PWM output pin number for the servo's LED
  float arm_length;		// Length of the servo's arm (used only for small, standard, and large servos)
  String* info;			// List containing the servo's information
} c_servo;

// Function declarations
void servo_speed_test(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, volatile uint8_t& counter, volatile unsigned long& end_time);
void servo_force_measurement(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, HX711 scale, uint16_t avg);
void servo_feedback_handling(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, byte feedback_pin, uint16_t avg);
void servo_continuous_drive(c_servo& servo, Adafruit_PWMServoDriver& pwm, LiquidCrystal_I2C lcd, uint16_t avg);
void move_to_start(Adafruit_PWMServoDriver& pwm, c_servo& servo);
void display_info(c_servo& servo, LiquidCrystal_I2C& lcd, byte pin);
void init_view(byte operation_mode, LiquidCrystal_I2C lcd);


#endif

4.4. declarations header

#ifndef DECLARATIONS
#define DECLARATIONS

/*## Button pins ##*/

#define select_next_servo 2
#define select_prev_servo 3
#define tare_scales 0
#define switch_mode 1
#define toggle_info 7

/*## Inputs ##*/

#define servo_control_pin A0
#define feedback_pin A1

/*## Servos ##*/

#define first_servo 0			// Indices correspond directly with the Servo Shield servo connection points
#define last_servo 4
volatile uint8_t curr_servo_index = 2;	// First servo to activate, in this case the middle servo
volatile uint8_t prev_servo_index = 2;

/*## Misc ##*/

volatile bool ledchange = true;		// Indicates whether the active LED should change

volatile unsigned long timer;

volatile uint8_t counter = 0;

byte operation_mode = 1;    // 0 for speed test, 1 for force measurement, 2 for feedback servo, 3 for continuous servo, 4 for info screen
byte curr_mode = 1;
byte prev_mode = curr_mode;

float big_servo_calib_factor = -211330;    // The calibration factors must be verified with a separate program.
float med_servo_calib_factor = -206420;    //
float small_servo_calib_factor = -209240;  //

#endif

4.5. interrupts header

#ifndef INTERRUPT_FUNCTIONS
#define INTERRUPT_FUNCTIONS

#include "declarations.h"

/*## Interrupt service routines used for switching between servos  ##*/

void next_servo() {
  static unsigned long last_interrupt_time_1 = 0;
  unsigned long interrupt_time = millis();
  if (interrupt_time - last_interrupt_time_1 > 200) {		// This part is for debouncing the buttonpress. Debouncing is necessary
    if (curr_servo_index < last_servo) curr_servo_index++;	// to prevent multiple calls to this ISR (interrupt service routine)
    last_interrupt_time_1 = interrupt_time;			// with one buttonpress.
    ledchange = true;						//
  }
}

void previous_servo() {
  static unsigned long last_interrupt_time_2 = 0;
  unsigned long interrupt_time = millis();
  if (interrupt_time - last_interrupt_time_2 > 200) {
    if (curr_servo_index > first_servo) curr_servo_index--;
    last_interrupt_time_2 = interrupt_time;
    ledchange = true;
  }
}

/*## Interrupt service routine (pin change) ##*/

ISR (PCINT2_vect) {		// This ISR is used by the optical switches when performing a speed test
  timer = micros();
  counter++;
}

#endif

5. Project documentation

Servo test bench