Lineær aktuator med ESP32

// Strøm motor:

getAverageReadings(); // Gjennomsnitt av "rå" verdier fra ADC. 0 - 4095
vDrop = average / (4095 / 3.3); // Beregner spenningsfall over motstand. 1240.9 verdier per volt.
motor_current = (vDrop / resistor) / ampGain; // Ohms Lov

void moveActuator(bool dir, int speed) { // dir 0 = inn, dir 1 = ut. Speed: 0-255

  timeSinceStart = curTime;     // Setter tid siden start til gjeldende tid

  if (dir == 1) {               // Dersom retning er UT....
    analogWrite(sw2, 0);        // Slå av sw2
    digitalWrite(sw4, LOW);     // Slå av sw4
    delay(10);                  // Ventetid for å lade ut transistorer
    analogWrite(sw1, speed);    // Skriv PWM signal med hastighet "speed" til sw1
    digitalWrite(sw3, HIGH);    // Slå på sw3
    movingTo = 2;               // Sett status "kjører ut"
    delay(1);
  }
  else {
    analogWrite(sw1, 0);
    digitalWrite(sw3, LOW);
    delay(10);
    analogWrite(sw2, speed);
    digitalWrite(sw4, HIGH);
    movingTo = 1;
    delay(1);
  }
}

// Stopp aktuator ved høy strøm
if ((motor_current >= curLimit) && !overloaded) {
      timeSinceOverload = curTime;  // Setter tid når overlast skjedde til gjeldende tid
      overloaded = 1;               // Setter overload til TRUE
      position = 0;                 // Setter posisjon til ukjent
      moveActuator(0, 255);         // Kjør aktuatoren ut
    }
    
    // Stopp aktuatoren når den har kjørt ut litt etter en overlast skjedde.
    if ((curTime - timeSinceOverload >= 500) && overloaded && !trigger) {
      haltActuator();               // Stopp aktuator
    }

void onTriggerChange()  {
  if (trigger && !overloaded) {

    if (position <= 1) {
      // Aktuator er inne eller status er ukjent.
      moveActuator(1, 255);
    }
    else {
      // aktuator er ute. Kjør aktuator inn.
      moveActuator(0, 255);
    }
  }
  else if (trigger && overloaded) {
    // Tving aktuator ut med redusert hastighet dersom trigger holdes inne
    moveActuator(1, slowSpeed);
  }
}

/*
Laget av Koen Van Eck - 2023
*/

#include "thingProperties.h"

// IO
const int currentPin = 35;      // Inngang som leser av strøm
const int sw1 = 18;             // NPN 1 (TIP 120)
const int sw2 = 19;             // NPN 2 (TIP 120)
const int sw3 = 23;             // PNP 2 (TIP 125)
const int sw4 = 22;             // PNP 1 (TIP 125)

// Setup
const float resistor = 0.47;    // Motstand verdi i ohm
const int slowSpeed = 120;      // Hastighet etter aktuator stopper på overlast. 0-255
const int smoothReadings = 10;  // Antall målinger som for å beregne gjennomsnitt
const long interval = 500;      // Ventetid før overlastbeskyttelse aktiveres i ms

// Variabler
int currentRaw = 0;             // 0 - 4095
float vDrop = 0;                // 0 - 3,3
int position = 0;               // 0 = Ukjent, 1 = inne, 2 = ute
int movingTo = 0;               // 0 = ingen bevegelse, 1 = inn, 2 = ut.
bool overloaded = 0;            // 0 = vanlig drift, 1 = slowSpeed
int ampGain = 2;                // Forsterkning av spenningsfall over motstand via op-amp

// Smoothing
int readings[smoothReadings];   // Array med størrelse på "smoothReadings (10)"
int readIndex = 0;              // Index
int total = 0;
int average = 0;

// Timer
unsigned long curTime = 0;              // Gjeldende tid

unsigned long timeSinceStartMoving = 0; // Tid siden aktuator begynte å bevege
unsigned long timeSinceOverload = 0;    // Tid siden overbelastning


void setup() {
  pinMode(sw1, OUTPUT);
  pinMode(sw2, OUTPUT);
  pinMode(sw3, OUTPUT);
  pinMode(sw4, OUTPUT);

  // Åpne serial port
  Serial.begin(9600);
  delay(1500);

  // Defined in thingProperties.h
  initProperties();

  // Connect to Arduino IoT Cloud
  ArduinoCloud.begin(ArduinoIoTPreferredConnection);
  
  setDebugMessageLevel(2);
  ArduinoCloud.printDebugInfo();
  
  // Reset alle verdier for gjennomsnittsmåling
  for (int thisReading = 0; thisReading < smoothReadings; thisReading ++) {
    readings[thisReading] = 0;
  }

}

void loop() {
  ArduinoCloud.update();

  // timer
  curTime = millis();

  // Strøm motor
  getAverageReadings();                         // Leser "rå" verdier fra ADC. 0 - 4095 og lager gjennomsnittverdi
  vDrop = average / (4095 / 3.3);               // Beregner spenningsfall over motstand. 1240.9 verdier per volt.
  motor_current = (vDrop / resistor) / ampGain; // Bruker ohms lov til å beregne strøm gjennom kretsen
  Serial.println(analogRead(currentPin));       // For debugging
  delay(1);

  // Ventetid slik at strømmen får stabilisere seg
  if (curTime - timeSinceStartMoving >= interval) {
    // Stopp aktuator ved høy strøm
    if ((motor_current >= curLimit) && !overloaded) {
      timeSinceOverload = curTime;  // Setter tid når overlast skjedde til gjeldende tid
      overloaded = 1;               // Setter overload til TRUE
      position = 0;                 // Setter posisjon til ukjent
      moveActuator(0, 255);         // Kjør aktuatoren ut
    }
    
    // Stopp aktuatoren når den har kjørt ut litt etter en overlast skjedde.
    if ((curTime - timeSinceOverload >= 500) && overloaded && !trigger) {
      haltActuator();               // Stopp aktuator
    }

    // Oppdater posisjon
    if (movingTo == 1 && motor_current <= 0.01) {
      haltActuator();
      position = 1;
    }
    else if (movingTo == 2 && motor_current <= 0.01) {
      haltActuator();
      position = 2;
      overloaded = 0;
    }
  }
  
  // Oppdater status tekst
  if (overloaded) {
    status = "Overbelastet!";
  }
  else if (movingTo == 1) {
    status = "Kjører inn";
  }
  else if (movingTo == 2) {
    status = "Kjører ut";
  }
  else if (position == 0) {
    status = "Posisjon ukjent";
  }
  else if (position == 1) {
    status = "Aktuator inne";
  }
  else if (position == 2) {
    status = "Aktuator ute";
  }
  else {
    status = "Ukjent";
  }

}

// Funksjon for å ta gjennomsnittet av analoge målinger
void getAverageReadings() {
  total = total - readings[readIndex];          // Fjern forrige måling med samme index fra totalen   

  readings[readIndex] = analogRead(currentPin); // Skriv gjeldende analoge verdi til reading arrayen

  total = total + readings[readIndex];          // Legg måleverdien til totalen

  readIndex = readIndex + 1;                    // Øk index

  if (readIndex >= smoothReadings) {            // Dersom indexen er blir høyere enn antall målinger, sett index til 0.
    readIndex = 0;
  }

  average = total / smoothReadings;             // Rekn ut gjennomsnitt
}

void moveActuator(bool dir, int speed) { // dir 0 = inn, dir 1 = ut. Speed: 0-255

  timeSinceStartMoving = curTime;     // Setter tid siden start til gjeldende tid

  if (dir == 1) {               // Dersom retning er UT....
    analogWrite(sw2, 0);        // Slå av sw2
    digitalWrite(sw4, LOW);     // Slå av sw4
    delay(10);                  // Ventetid for å lade ut transistorer
    analogWrite(sw1, speed);    // Skriv PWM signal med hastighet "speed" til sw1
    digitalWrite(sw3, HIGH);    // Slå på sw3
    movingTo = 2;               // Sett status "kjører ut"
    delay(1);
  }
  else {
    analogWrite(sw1, 0);
    digitalWrite(sw3, LOW);
    delay(10);
    analogWrite(sw2, speed);
    digitalWrite(sw4, HIGH);
    movingTo = 1;               // Sett status "kjører inn"
    delay(1);
  }
}

// Funksjon for å stoppe aktuator
void haltActuator() {
  analogWrite(sw1, 0);
  analogWrite(sw2, 0);
  digitalWrite(sw3, LOW);
  digitalWrite(sw4, LOW);
  movingTo = 0;                 // Sett status "posisjon ukjent"
}

// Funksjon som kjøres når knapp på HMI trykkes på
void onTriggerChange()  {
  if (trigger && !overloaded) {

    if (position <= 1) {
      // Aktuator er inne eller status er ukjent.
      moveActuator(1, 255);
    }
    else {
      // aktuator er ute. Kjør aktuator inn.
      moveActuator(0, 255);
    }
  }
  else if (trigger && overloaded) {
    // Tving aktuator ut med redusert hastighet dersom trigger holdes inne
    moveActuator(1, slowSpeed);
  }

}

void onCurLimitChange() {
  
}