Code powering my PCB Laminator

PCB_Laminator_v1.5.ino

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include <Encoder.h>
#include <PID_v1.h>

//display stuff
#define FRAME_RATE 130 //in ms, takes about 112ms actually
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
char degreeC[] = {(char)247, 'C', '\0'}; //°C
unsigned long display_tim; //tim = previous time
unsigned int frame = 0;

//motor stuff
Servo myServ;
Encoder myEnc(3, 2);
#define SERVO_PIN A3
#define SERVO_MIN 50
#define SERVO_STOP 92
#define SERVO_MAX 137
#define SERVO_SLOW_FORWARD 67 //for initial pcb insertion
#define SERVO_REAL_SLOW_FORWARD 84 //even out the heat
#define SERVO_SOFT_RANGE 20 //limit rate of change of motor to reduce inrush current
//PI constants
#define motor_Kp 4
#define motor_Ki 1
#define motor_KI_WINDUP 5
long motor_Ki_error = 0;
//target stuff, back and forth
unsigned long motor_tim = 0;
bool motor_dir = true;
int motor_period = 0;
long t1 = 0;
long t2 = 0;
#define MAX_VELOC 51 // ms/step
#define TIME_PAD 1000 //550 //ms
long target_pos = 0;
long actual_pos = 0;
int motor_pos = 0;

//heater stuff
#define HEATER_PIN 5
#define HEATER_MIN 100.0 //temperature in C
#define HEATER_MAX 220.0
int heaterCount = 0;
volatile float freq = 0.00;
volatile unsigned long freq_tim = 0;
//thermistor
#define THERMISTORPIN A0
#define THERMISTORNOMINAL 100000      
#define TEMPERATURENOMINAL 25
#define NUMSAMPLES 30
#define BCOEFFICIENT 3950
#define SERIESRESISTOR 100300
uint16_t samples[NUMSAMPLES];
volatile double actual_temp = 0.0;
volatile double target_temp = 0.0;
//PID stuff
#define HEATER_CYCLE 2080
#define HEATER_RES 255 //cycles to wait to turn heater on off
#define Kp 57.619
#define Ki 1.188
#define Kd 698.625
volatile double heater_duty = 0.0;
PID myPID(&actual_temp, &heater_duty, &target_temp, Kp, Ki, Kd, DIRECT);

//user input stuff
#define BUTTON A1
#define TEMP_POT A2

void setup() {
  OSCCAL = 0xAA; //device specific value for the internal RC oscillator
  Serial.begin(115200);

  //motor setup
  myServ.attach(SERVO_PIN);
  myServ.write(SERVO_STOP);

  //heater setup
  PCMSK2 |= bit(PCINT22); //for zcd, pinchange interrupt, digital pin 6
  PCICR |= bit(PCIE2);
  pinMode(5, OUTPUT);
  
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0, HEATER_RES);
  myPID.SetSampleTime(HEATER_CYCLE);

  //user input setup
  pinMode(BUTTON, INPUT_PULLUP);

  //display setup
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(10, 24);
  display.print(F("PCB Laminator V1.5"));
  display.setCursor(19, 32);
  display.print(F("by Matthew Tran"));
  display.display();
  delay(420);

  getMotorTargets();
  
  display_tim = millis();
}

void loop() {
  if (!digitalRead(BUTTON)) {
    delay(25);
    while(!digitalRead(BUTTON));
    getMotorTargets();
    display_tim = millis();
  }
  //heater stuff
  target_temp = mapdouble(analogRead(TEMP_POT), 0, 1023, HEATER_MIN, HEATER_MAX);
  actual_temp = getTemp();
  myPID.Compute();

  //motor stuff (might want to move this stuff into an interrupt run every 50ms)
  if (millis() - motor_tim > motor_period) {
    motor_dir ^= 1;
    target_pos = motor_dir ? t1 : t2;
    motor_tim = millis();
  }

  actual_pos = myEnc.read();
  long error = actual_pos - target_pos;

  motor_Ki_error = (error == 0) ? 0 : motor_Ki_error + error; //shouldn't actually set to 0 but it works so...
  motor_Ki_error = constrain(motor_Ki_error, -motor_KI_WINDUP, motor_KI_WINDUP);

  motor_pos = SERVO_STOP + motor_Kp * error + motor_Ki * motor_Ki_error;
  motor_pos = constrain(motor_pos, myServ.read() - SERVO_SOFT_RANGE, myServ.read() + SERVO_SOFT_RANGE);
  motor_pos = constrain(motor_pos, SERVO_MIN, SERVO_MAX);
  myServ.write(motor_pos);
  
  //display/serial stuff
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print(F("Target Temp: ")); display.print(target_temp); display.println(degreeC);
  display.print(F("Actual Temp: ")); display.print(actual_temp); display.println(degreeC);
  display.print(F("Target Pos: ")); display.println(target_pos);
  display.print(F("Actual Pos: ")); display.println(actual_pos);
  display.print(F("Freq: ")); display.print(freq); display.println(F("Hz"));
  display.print(F("Heater %: ")); display.println((int) heater_duty);
  display.print(F("Motor %: ")); display.println(motor_pos);
  display.print(F("Framerate: ")); display.print(frame); display.println(F("ms"));
  display.display();

  Serial.print(heater_duty);
  Serial.print(",");
  Serial.print(actual_temp);
  Serial.print(",");
  Serial.println(target_temp);
  
  while (millis() - display_tim < FRAME_RATE);
  frame = millis() - display_tim;
  display_tim = millis();
}

ISR(PCINT2_vect) {
  if(digitalRead(6)) {
    freq = 500000.0 / (micros() - freq_tim);
    freq_tim = micros();
    heaterCount++;
    if (heaterCount > HEATER_RES) {
      heaterCount = 0;
    }
    digitalWrite(HEATER_PIN, heaterCount <= (int) heater_duty && (int) heater_duty > 0);
  }
}

double getTemp() {
  uint8_t i;
  double average;
  for (i=0; i< NUMSAMPLES; i++)
   samples[i] = analogRead(THERMISTORPIN);
  average = 0;
  for (i=0; i< NUMSAMPLES; i++)
     average += samples[i];
  average /= NUMSAMPLES;
  average = 1023 / average - 1;
  average = SERIESRESISTOR / average;
  double steinhart;
  steinhart = average / THERMISTORNOMINAL;     // (R/Ro)
  steinhart = log(steinhart);                  // ln(R/Ro)
  steinhart /= BCOEFFICIENT;                   // 1/B * ln(R/Ro)
  steinhart += 1.0 / (TEMPERATURENOMINAL + 273.15); // + (1/To)
  steinhart = 1.0 / steinhart;                 // Invert
  steinhart -= 273.15;                         // convert to C
  return steinhart;
}

double mapdouble(double x, double in_min, double in_max, double out_min, double out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void getMotorTargets() {
  myServ.write(SERVO_REAL_SLOW_FORWARD);
  //push button to start target acquisition
  do {
    gettingTargetsResetDisplay();
    display.print(F("Push button to start target acquisition..."));
    gettingTargetsUpdateHeater();
  } while(digitalRead(BUTTON));
  myEnc.write(0);
  myServ.write(SERVO_SLOW_FORWARD);
  do {
    gettingTargetsResetDisplay();
    display.println(F("Started! Please release button..."));
    gettingTargetsUpdateHeater();
  } while(!digitalRead(BUTTON));
  delay(25); //debounce

  //push button to set first target
  while(digitalRead(BUTTON)) {
    gettingTargetsResetDisplay();
    display.println(F("Push button again to set first target..."));
    gettingTargetsUpdateHeater();
  }
  t1 = myEnc.read();
  myServ.write(SERVO_STOP); //provide feedback for button press
  unsigned long temp = millis();
  while(!digitalRead(BUTTON) || millis() - temp < 420) {
    gettingTargetsResetDisplay();
    display.print(F("Noted! First target set to "));
    display.println(t1);
    display.println(F("Please release button..."));
    gettingTargetsUpdateHeater();
  }
  delay(25);
  myServ.write(SERVO_SLOW_FORWARD);

  //push button to set second target
  while(digitalRead(BUTTON)) {
    gettingTargetsResetDisplay();
    display.println(F("Push button again to set second target..."));
    gettingTargetsUpdateHeater();
  }
  t2 = myEnc.read();
  myServ.write(SERVO_STOP);
  target_pos = t1;
  motor_period = (t2 - t1) * MAX_VELOC + TIME_PAD;
  do {
    gettingTargetsResetDisplay();
    display.print(F("Gotcha! Second target set to "));
    display.println(t2);
    display.println(F("Release button to complete target acquisition..."));
    gettingTargetsUpdateHeater();
  } while(!digitalRead(BUTTON));
  motor_tim = millis();
}

void gettingTargetsResetDisplay() {
  display_tim = millis();
  display.clearDisplay();
  display.setCursor(0, 0);
}

void gettingTargetsUpdateHeater() {
  target_temp = mapdouble(analogRead(TEMP_POT), 0, 1023, HEATER_MIN, HEATER_MAX);
  actual_temp = getTemp();
  myPID.Compute();
  display.setCursor(0, 48);
  display.print(F("Target Temp: ")); display.print(target_temp); display.println(degreeC);
  display.print(F("Actual Temp: ")); display.print(actual_temp); display.println(degreeC);
  display.display();

  Serial.print(heater_duty);
  Serial.print(",");
  Serial.print(actual_temp);
  Serial.print(",");
  Serial.println(target_temp);
}