Servo Tester with Ammeter

/**
 * ATtiny1614 Servo Tester & Ammeter
 * based on Smart Servo Motor Tester with mAmp meter from Science 3D 
 * (https://www.hackster.io/science3d/smart-servo-motor-tester-with-mamp-meter-f1fd42)
 * 
 * John Bradnam (jbrad2089@gmail.com)
 * 2022-12-15 - Initial Code Base
 *            - Rewrote for ATtiny1614
 *            - Redesigned display libaries to reduced footprint
 * ---------------------------------------
 * ATtiny1614 Pins mapped to Ardunio Pins
 *
 *             +--------+
 *         VCC + 1   14 + GND
 * (SS)  0 PA4 + 2   13 + PA3 10 (SCK)
 *       1 PA5 + 3   12 + PA2 9  (MISO)
 * (DAC) 2 PA6 + 4   11 + PA1 8  (MOSI)
 *       3 PA7 + 5   10 + PA0 11 (UPDI)
 * (RXD) 4 PB3 + 6    9 + PB0 7  (SCL)
 * (TXD) 5 PB2 + 7    8 + PB1 6  (SDA)
 *             +--------+
 *             
 *             
 * BOARD: ATtiny1614/1604/814/804/414/404/214/204
 * Chip: ATtiny1614
 * Clock Speed: 20MHz
 * millis()/micros(): "TCD0 (1 series only, default there)"
 * Programmer: jtag2updi (megaTinyCore)
 * ----------------------------------------
 */
#include <Wire.h>
#include "font6x8.h"
#include "font8x16.h"
#include "SSD1306_ATtiny1614.h"
#include <Servo_megaTinyCore.h>

#define ENCA_PIN 1    //PA5
#define ENCB_PIN 0    //PA4
#define ENCS_PIN 2    //PA6
#define SDA_PIN 6     //PB1
#define SCL_PIN 7     //PB0
#define A_PIN 8       //PA1
#define POT_PIN 9     //PA2
#define SERVO_PIN 10  //PA3

//Running average variables to monitor current
#define AMP_SIZE 128
#define AMP_MASK 0x7F
uint32_t ampTotal = 0;
uint16_t ampCount = 0;
uint8_t ampHead = 0; 
uint8_t ampTail = 0;
uint16_t ampValues[AMP_SIZE];
uint16_t ampMaximum = 0;
int oldCurrent = -1;

//Running average variables for potentiometer
#define POT_SIZE 16
#define POT_MASK 0x0F
uint32_t potTotal = 0;
uint16_t potCount = 0;
uint8_t potHead = 0; 
uint8_t potTail = 0;
uint16_t potValues[POT_SIZE];

//Rotary encoder
volatile int rotaryDirection = 0;
volatile bool rotarySwitchPressed = false;
bool volatile lastRotA = false;                               // Used to determine direction of rotary encoder
bool volatile lastRotB = false;                               // Used to determine direction of rotary encoder

//Display
#define DISPLAY_WIDTH 128
#define DISPLAY_HEIGHT 64
//Adafruit_SSD1306 display(DISPLAY_WIDTH, DISPLAY_HEIGHT, &Wire, -1);

//Menus
#define TOTAL_MENU_ITEMS 6
#define MAX_TEXT_LEN 11
enum MENU_ENUM { SERVO_MIN, SERVO_MAX, MODE_SWEEP, MODE_POT, CENTER, RESET };
char menuItems[TOTAL_MENU_ITEMS][MAX_TEXT_LEN] = {{"SERVO MIN"},{"SERVO MAX"},{"MODE SWEEP"},{"MODE POT"},{"CENTER"},{"RESET"}};
MENU_ENUM currentMenuItem;
MENU_ENUM previousMenuItem;
bool inMenuSelect = true;
char charBuf[MAX_TEXT_LEN];
char horzLine[] = "{{{{{{{{{{{{{{{{{{{{";

//Servo
Servo servo;
int servoPosition = 0;
int servoMinAngle = 0;
int servoMaxAngle = 180;
bool servoMiddle = false;
bool servoAuto = false;
bool servoDirection = false;
int oldPosition = -1;
int oldServoMin = -1;
int oldServoMax = -1;
bool oldServoAuto = false;

//--------------------------------------------------------------------
//Program Setup
void setup() 
{

  //Setup rotary encoder
  pinMode(ENCS_PIN, INPUT_PULLUP);
  pinMode(ENCA_PIN, INPUT);
  pinMode(ENCB_PIN, INPUT);
  attachInterrupt(ENCA_PIN, rotaryInterrupt, CHANGE);
  //attachInterrupt(ENCB_PIN, rotaryInterrupt, CHANGE);
  attachInterrupt(ENCS_PIN,switchInterrupt,CHANGE);

  //Setup ammeter
  pinMode(A_PIN, INPUT);

  //Setup angle potentiometer
  pinMode(POT_PIN, INPUT);
  
  //Setup display
  ssd1306_init();
  ssd1306_fillscreen(0x00);

  //Setup servo
  pinMode(SERVO_PIN, OUTPUT);
  servo.attach(SERVO_PIN);    //PWM pin
  resetServoParameters();
  
  inMenuSelect = true;
  currentMenuItem = SERVO_MIN;
  previousMenuItem = SERVO_MIN;
  displayMenu(true);
}

//---------------------------------------------------------------------
//Main program loop
void loop()
{
  if (inMenuSelect)
  {
    //Check if rotary encoder has moved
    if (rotaryDirection == -1 && currentMenuItem != SERVO_MIN)
    {
      previousMenuItem = currentMenuItem;
      currentMenuItem = (MENU_ENUM)((int)currentMenuItem - 1);
      displayMenu(false);
    }
    else if (rotaryDirection == 1 && currentMenuItem != RESET)
    {
      previousMenuItem = currentMenuItem;
      currentMenuItem = (MENU_ENUM)((int)currentMenuItem + 1);
      displayMenu(false);
    }
    rotaryDirection = 0;      

    //Check if switch is pressed
    if (rotarySwitchPressed && (digitalRead(ENCS_PIN) == LOW))
    {
      if (servoMiddle)
      {
        servoMiddle = false;
      }
      else
      {
        switch (currentMenuItem)
        {
          case SERVO_MIN: servoMinAngle = servoPosition; currentMenuItem = MODE_POT; break;
          case SERVO_MAX: servoMaxAngle = servoPosition; currentMenuItem = MODE_POT; break;
          case MODE_SWEEP: servoAuto = true; break;
          case MODE_POT:  servoAuto = false; break;
          case CENTER: servoCenter(); currentMenuItem = MODE_POT; break;
          case RESET: resetServoParameters(); currentMenuItem = MODE_POT; break;
        }
        displayStatus(true);
        inMenuSelect = false;
      }
    }
  }
  else
  {
    displayStatus(false);
    if (rotarySwitchPressed && (digitalRead(ENCS_PIN) == LOW))
    {
      displayMenu(true);
      inMenuSelect = true;
      rotaryDirection = 0;      
    }
  }
  rotarySwitchPressed = false;
}


//---------------------------------------------------------------------
// Interrupt Handler: Rotary encoder has moved
void rotaryInterrupt()
{
  bool a = (digitalRead(ENCA_PIN) == LOW);
  bool b = (digitalRead(ENCB_PIN) == LOW);
  if (a != lastRotA) 
  {
    lastRotA = a;
    if (b != lastRotB) 
    {
      lastRotB = b;
      if (b)
      {
        rotaryDirection = ((a == b) ? -1 : 1);
      }
    }
  }
}

//--------------------------------------------------------------------
//Handle pin change interrupt when button is pressed

void switchInterrupt()
{
  rotarySwitchPressed = true;
}

//--------------------------------------------------------------------
//Display the menu and highlight the current selection
// repaintScreen - true to redraw the whole screen

void displayMenu(bool repaintScreen)
{
  if (repaintScreen)
  {
    displayHeader(); 
    
    for(int i=0, y=2; i<TOTAL_MENU_ITEMS; i++, y++)
    {
      ssd1306_char_f6x8(10, y, (i == (int)currentMenuItem) ? "|" : " ");
      ssd1306_char_f6x8(22, y, menuItems[i]);
    }
  }
  else
  {
    ssd1306_char_f6x8(10, 2 + (int)previousMenuItem, " ");
    ssd1306_char_f6x8(10, 2 + (int)currentMenuItem, "|");
  }
}

//--------------------------------------------------------------------
//Display the status screen
// repaintScreen - true to redraw the whole screen

void displayStatus(bool repaintScreen)
{
  if (repaintScreen)
  {
    displayHeader();
    ssd1306_char_f6x8(65, 2, "AVG mA");
    ssd1306_char_f6x8(0, 4, horzLine);
    ssd1306_char_f6x8(0, 5, "MIN");
    ssd1306_char_f6x8(50, 5, "MAX");
    ssd1306_char_f6x8(94, 5, "MODE");
  }

  int curPos = (servoMiddle) ? (servoMinAngle + servoMaxAngle) / 2 : servoPosition;
  if (repaintScreen || oldPosition != curPos)
  {
    sprintf(charBuf, "%d  ", curPos);
    ssd1306_char_f8x16(0, 2, charBuf);
    oldPosition = curPos;
  }
    
  int current = readCurrent();
  if (repaintScreen || oldCurrent != current)
  {
    sprintf(charBuf, "%d  ", current);
    ssd1306_char_f6x8(65, 3, charBuf);
    oldCurrent = current;
  }

  if (repaintScreen || oldServoMin != servoMinAngle)
  {
    sprintf(charBuf, "%d  ", servoMinAngle);
    ssd1306_char_f6x8(0, 6, charBuf);
    oldServoMin = servoMinAngle;
  }

  if (repaintScreen || oldServoMax != servoMaxAngle)
  {
    sprintf(charBuf, "%d  ", servoMaxAngle);
    ssd1306_char_f6x8(50, 6, charBuf);
    oldServoMax = servoMaxAngle;
  }

  ssd1306_setpos(94, 6);
  if (servoAuto)
  {
    if (!servoMiddle)
    {
      servoSweep();
    }
    if (repaintScreen || oldServoAuto != servoAuto)
    {
      ssd1306_string_font6x8("SWEEP");
      oldServoAuto = servoAuto;
    }
  }
  else
  {
    if (!servoMiddle)
    {
      servoKnob();
    }
    if (repaintScreen || oldServoAuto != servoAuto)
    {
      ssd1306_string_font6x8("POT  ");
      oldServoAuto = servoAuto;
    }
  }
}

//--------------------------------------------------------------------
//Displays the screen header

void displayHeader()
{ 
  ssd1306_fillscreen(0x00);
  ssd1306_char_f6x8(14, 0, "Servo Controller");
  ssd1306_char_f6x8(0, 1, horzLine);
}

//--------------------------------------------------------------------
//Sweep the servo between min and max values

void servoSweep()
{
  int yy = map(analogRead(POT_PIN), 1022, 0, 10, 1);     // scale it to use it with the servo

  if (servoPosition > servoMaxAngle)
  {
    servoPosition = servoMaxAngle;
    delay(10);
    servoDirection = false;   //start decreasing angle
  }
  else if (servoPosition < servoMinAngle)
  {
    servoPosition = servoMinAngle;
    delay(10);
    servoDirection = true;   //start increasing angle
  }

  if (servoDirection)  //increasing
  {
    if ((servoPosition + (yy / 2)) >= servoMaxAngle)
    {
      yy = round(abs(servoMaxAngle - servoPosition) / 3);
    }
    if (yy < 1)
    {
      yy = 1;
    }
    servoPosition = servoPosition + yy;
    servo.write(180 - servoPosition);              // tell servo to go to position in variable 'pos'
  }
  else //decreasing
  {
    if ((servoPosition - (yy / 2)) <= servoMinAngle)
    {
      yy = round(abs(servoPosition - servoMinAngle) / 3);
    }
    if (yy < 1)
    {
      yy = 1;
    }
    servoPosition = servoPosition - yy;
    servo.write(180 - servoPosition);              // tell servo to go to position in variable 'pos'
  }
}

//--------------------------------------------------------------------
// Manual movement of servo

void servoKnob()
{
  servoPosition = readPot();
  if (servoPosition > servoMaxAngle)
  {
    servoPosition = servoMaxAngle;
  }
  else if (servoPosition < servoMinAngle)
  {
    servoPosition = servoMinAngle;
  }
  servo.write(180 - servoPosition);            // sets the servo position according to the scaled value
}

//--------------------------------------------------------------------
// Center the servo

void servoCenter()
{
  servoAuto = false;
  servoMiddle = true;
  servo.write(180 - ((servoMinAngle + servoMaxAngle) / 2));
  potTotal = 90;
  potCount = 1;
  potHead = 1; 
  potTail = 0;
  potValues[0] = 90;
}

//--------------------------------------------------------------------
// Reset servo parameters to their defaults

void resetServoParameters()
{
  servoMinAngle = 0;
  servoMaxAngle = 180;
  servoMiddle = false;
  servoAuto = false;
  ampTotal = 0;
  ampCount = 0;
  ampHead = 0; 
  ampTail = 0;
  ampMaximum = 0;
  potTotal = 0;
  potCount = 0;
  potHead = 0; 
  potTail = 0;
}

//-------------------------------------------------------------
// Read the current
// Add to the running total
// Returns current running average in mA

int readCurrent()
{
  // taking the average for stable readings
  unsigned long current = 0;

  for (int i=0; i<16; i++) 
  {
    current = current + analogRead(A_PIN);
  }
  current = (current >> 4);
  int amp = round(current * 5.5); // calibration value,to be changed according to components used
  
  if (ampCount == AMP_SIZE)
  {
    //Remove oldest value from running total
    ampTotal = ampTotal - ampValues[ampTail];
    ampTail = (ampTail + 1) & AMP_MASK;
    ampCount--;
  }
  //Add new value to running total
  ampValues[ampHead] = amp;
  ampHead = (ampHead + 1) & AMP_MASK;
  ampTotal = ampTotal + amp;
  ampCount++;
  
  ampMaximum = max(ampMaximum, (uint16_t)amp);
  return (int)round(ampTotal / ampCount);
}

//-------------------------------------------------------------
// Reads the Potentiometer value
// Returns current running average (0 to 1023)

int readPot()
{
  // taking the average for stable readings
  int pot = analogRead(POT_PIN);
  pot = map(pot, 1022, 0, 180, 0);    // scale it to use it with the servo (value between 0 and 180)
  if (potCount == POT_SIZE)
  {
    //Remove oldest value from running total
    potTotal = potTotal - potValues[potTail];
    potTail = (potTail + 1) & POT_MASK;
    potCount--;
  }
  //Add new value to running total
  potValues[potHead] = pot;
  potHead = (potHead + 1) & POT_MASK;
  potTotal = potTotal + pot;
  potCount++;
  return (int)round(potTotal / potCount);
}