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