4. Software

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);
}