Alle Libraries eingefügt.

Libraries/PRG_342/prg_342.cpp

@@ -0,0 +1,357 @@
+#include "prg_342.h"
+
+volatile int PRG_342::encoderPos = 0;  // Initialisierung der statischen Variablen
+volatile int PRG_342::lastEncoded = 0; // Initialisierung der statischen Variablen
+volatile int PRG_342::ANGLE = 0;   // Initialisierung der statischen Variablen
+const int PRG_342::EncoderPinA;         // Definition der statischen Variablen
+const int PRG_342::EncoderPinB;         // Definition der statischen Variablen
+const int PRG_342::EncoderPinN;         // Definition der statischen Variablen
+
+const int PRG_342::DMS_PIN;         // Definition der statischen Variablen
+const int PRG_342::EXTSET_PIN;         // Definition der statischen Variablen
+const int PRG_342::TORQUE_OUT_PIN;         // Definition der statischen Variablen
+const int PRG_342::ACT_OUT_PIN;         // Definition der statischen Variablen
+int PRG_342::valA0;         // Definition der statischen Variablen
+int PRG_342::valA1;         // Definition der statischen Variablen
+int PRG_342::filteredValA1;
+
+volatile bool PRG_342::isRunning;
+
+PRG_342::PRG_342(){
+    this->valA0 = 0;
+    this->valA1 = 0;
+    this->filteredValA1 = 0;
+    this->TORQUE = 0;
+    this->OFFSET = 0;
+    this->SETPOINT = 0;
+    this->ANGLE = 0;
+    this->K_FAKTOR = 1.0;
+
+    lastSpeedCalcTime = millis();
+    lastAngle = this->getAngle();
+
+    analogWrite(TORQUE_OUT_PIN, 0);
+    analogWrite(ACT_OUT_PIN, 0);
+    analogWrite(DMS_PIN, 0);
+    analogWrite(EXTSET_PIN, 0);
+    digitalWrite(EncoderPinA, LOW);
+    digitalWrite(EncoderPinB, LOW);
+
+    pinMode(DMS_PIN, INPUT);
+    pinMode(EXTSET_PIN, INPUT);
+    pinMode(EncoderPinA, INPUT);
+    pinMode(EncoderPinB, INPUT);
+    pinMode(TORQUE_OUT_PIN, OUTPUT);
+    pinMode(ACT_OUT_PIN, OUTPUT);
+    
+    analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
+    analogReadResolution(12);
+
+    attachInterrupt(digitalPinToInterrupt(EncoderPinA), PRG_342::updateEncoder, CHANGE);
+    attachInterrupt(digitalPinToInterrupt(EncoderPinB), PRG_342::updateEncoder, CHANGE);
+}
+
+PRG_342::PRG_342(float K_FAKTOR) {
+    this->valA0 = 0;
+    this->valA1 = 0;
+    this->TORQUE = 0;
+    this->OFFSET = 0;
+    this->SETPOINT = 0;
+    this->ANGLE = 0;
+    this->K_FAKTOR = K_FAKTOR;
+
+    lastSpeedCalcTime = millis();
+    lastAngle = this->getAngle();
+
+    analogWrite(TORQUE_OUT_PIN, 0);
+    analogWrite(ACT_OUT_PIN, 0);
+    analogWrite(DMS_PIN, 0);
+    analogWrite(EXTSET_PIN, 0);
+    digitalWrite(EncoderPinA, LOW);
+    digitalWrite(EncoderPinB, LOW);
+
+    pinMode(DMS_PIN, INPUT);
+    pinMode(EXTSET_PIN, INPUT);
+    pinMode(EncoderPinA, INPUT);
+    pinMode(EncoderPinB, INPUT);
+    pinMode(TORQUE_OUT_PIN, OUTPUT);
+    pinMode(ACT_OUT_PIN, OUTPUT);
+    
+    analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
+    analogReadResolution(12);
+
+    attachInterrupt(digitalPinToInterrupt(EncoderPinA), PRG_342::updateEncoder, CHANGE);
+    attachInterrupt(digitalPinToInterrupt(EncoderPinB), PRG_342::updateEncoder, CHANGE);
+}
+
+PRG_342::~PRG_342() {
+    detachInterrupt(digitalPinToInterrupt(EncoderPinA));
+    detachInterrupt(digitalPinToInterrupt(EncoderPinB));
+}
+
+void PRG_342::updateValA0(){
+    this->valA0 = analogRead(DMS_PIN);
+}
+
+void PRG_342::updateValA1(){
+    this->valA1 = analogRead(EXTSET_PIN);
+    this->filteredValA1 = this->exponentialFilter(this->valA1, this->filteredValA1, 0.01);
+}
+
+float PRG_342::exponentialFilter(float currentValue, float previousFilteredValue, float alpha) {
+  return alpha * currentValue + (1.0 - alpha) * previousFilteredValue;
+}
+
+/*void PRG_342::setOutput(unsigned int value){
+    if(value <= 0){
+        value = 50;
+    } else if (value > 4095){
+        value = 4095;
+    }
+
+    analogWrite(TORQUE_OUT_PIN, value);
+}*/
+
+/*void PRG_342::setOutput(unsigned int value, bool noLimits, bool onlyWhenRunning){
+    if(value <= 0 && !noLimits){
+        value = 50;
+    } else if (value > 4095 && !noLimits){
+        value = 4095;
+    }
+
+    if(onlyWhenRunning){
+        if(this->isRunning){
+            analogWrite(TORQUE_OUT_PIN, value);
+        }
+    } else {
+        analogWrite(TORQUE_OUT_PIN, value);
+    }
+    this->lastOutput = value;
+}*/
+
+void PRG_342::setOutput(unsigned int value, bool onlyWhenRunning){
+    value = constrain(value, 0, 4095);
+    if(onlyWhenRunning){
+        if(this->isRunning){
+            if(abs(this->getRotationSpeed()) >= 400){
+                analogWrite(TORQUE_OUT_PIN, value);
+                this->ActOut = value;
+            }
+        }
+    } else {
+        analogWrite(TORQUE_OUT_PIN, value);
+        this->ActOut = value;
+    }
+}
+
+void PRG_342::setOutput(int startValue, int endValue, int max_mV_per_ms){
+    endValue = constrain(endValue, 0, 4095);
+    int actValue = startValue;
+    int changeValue;
+    while(abs(actValue - endValue) > max_mV_per_ms + 1){
+        changeValue = endValue - startValue;
+        changeValue = constrain(changeValue, max_mV_per_ms * -1, max_mV_per_ms);
+        actValue += changeValue;
+        delayMicroseconds(1000);
+    }
+    analogWrite(TORQUE_OUT_PIN, actValue);
+    this->ActOut = actValue;
+}
+
+void PRG_342::updateActOut(int actTorque){
+    analogWrite(ACT_OUT_PIN, abs(actTorque) * 0.0819);
+}
+
+int PRG_342::getActTorque(){
+    this->updateValA0();
+    return ((this->valA0 * 24.4200244) - 50000) * this->K_FAKTOR;
+    //return this->valA0;
+}
+
+unsigned int PRG_342::getSetPoint(unsigned char format){
+    this->updateValA1();
+    if(format == 0){
+        return this->filteredValA1;
+    } else if (format == 1){
+        return (this->filteredValA1 * 2.44200244);
+    }
+}
+
+unsigned int PRG_342::getOutVolt(){
+    return this->ActOut * 6.34920635;
+}
+
+void PRG_342::tareTorque(){
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->TORQUE = this->getTorque(false, 0.01);
+    this->OFFSET = 0 - this->TORQUE; 
+}
+
+void PRG_342::tareAngle(){
+    this->ANGLE = 0;
+    this->lastEncoded = 0;
+    this->encoderPos = 0;
+}
+
+void PRG_342::updateActTorqueOut(){
+    this->TORQUE = this->getActTorque();
+    analogWrite(this->TORQUE_OUT_PIN, abs(this->TORQUE + this->OFFSET) * 0.0819);
+}
+
+void PRG_342::updateEncoder(){
+    int MSB = digitalRead(EncoderPinA);  // Most Significant Bit
+    int LSB = digitalRead(EncoderPinB);  // Least Significant Bit
+
+    int encoded = (MSB << 1) | LSB;          // Kombinieren der beiden Bits
+    int sum = (PRG_342::lastEncoded << 2) | encoded;  // Kombinieren mit dem letzten Zustand
+
+    // Bestimmen der Richtungsänderung und Aktualisierung der Position
+    if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) PRG_342::encoderPos++;
+    if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) PRG_342::encoderPos--;
+
+    PRG_342::lastEncoded = encoded;  // Aktualisieren des letzten Zustands
+
+    PRG_342::ANGLE = PRG_342::encoderPos * 18;
+
+    isRunning = true;
+}
+
+int PRG_342::getTorque(int offset){
+    if(offset){
+        return this->getActTorque() + this->OFFSET; 
+    } else {
+        return this->getActTorque();
+    }
+}
+
+/*int PRG_342::getTorque(int offset, unsigned int COUNTS){
+    this->TORQUE = this->getActTorque();
+    for (int i = 0; i < COUNTS-1; i++) {
+        this->TORQUE += this->getActTorque();
+    }
+    this->TORQUE = (float)this->TORQUE / COUNTS;
+    if(offset){
+        return this->TORQUE + this->OFFSET; 
+    } else {
+        return this->TORQUE;
+    }
+}*/
+
+int PRG_342::getTorque(int offset, float ALPHA){
+    this->TORQUE = ALPHA * (float)this->getActTorque() + (1.0 - ALPHA) * (float)this->TORQUE;
+    if(offset){
+        return this->TORQUE + this->OFFSET; 
+    } else {
+        return this->TORQUE;
+    }
+}
+
+int PRG_342::getAngle(){
+    //this->updateEncoder();
+    return this->ANGLE;
+}
+
+void PRG_342::safeShutdown(){
+    this->setOutput(4095);
+    for (int i = 0; i < 4065; i++){
+        this->setOutput(4095 - i);
+        delay(10);
+    }
+}
+
+void PRG_342::safeShutdown(unsigned int time){
+    unsigned int DELAY = time * 244.200244;
+    this->setOutput(4095);
+    for (int i = 0; i < 4065; i++){
+        this->setOutput(4095 - i);
+        delayMicroseconds(DELAY);
+    }
+}
+
+void PRG_342::safeShutdown(unsigned int time, unsigned int value){
+    unsigned int DELAY = time * 244.200244;
+    value += 100;
+    this->setOutput(value);
+    for (int i = 0; i < value + 1; i++){
+        this->setOutput(value - i);
+        delayMicroseconds(DELAY);
+    }
+}
+
+void PRG_342::DynDecog(unsigned int actValue){
+    this->setOutput(4095 * 0.5);
+    delay(500);
+    this->setOutput(4095 * 0.1);
+    delay(500);
+    this->setOutput(4095 * 0.5);
+    delay(500);
+    this->setOutput(actValue);
+}
+
+float PRG_342::getRotationSpeed() {
+    // Aktuelle Zeit und Winkel holen
+    unsigned long currentTime = millis();
+    int currentAngle = this->getAngle();
+
+    // Zeitdifferenz und Winkeldifferenz berechnen
+    unsigned long timeDelta = currentTime - lastSpeedCalcTime;
+    int angleDelta = currentAngle - lastAngle;
+
+    // Aktualisieren der letzten Winkel- und Zeitwerte
+    lastSpeedCalcTime = currentTime;
+    lastAngle = currentAngle;
+
+    // Verhindern einer Division durch Null
+    if (timeDelta == 0) return 0;
+
+    // Drehgeschwindigkeit in Grad pro Sekunde berechnen
+    float speed = (angleDelta) * (1000.0 / timeDelta);
+
+    return speed;
+}
+
+void PRG_342::AutoDecog(){
+    for(float k = 10; k > 0; k--){
+        for(int j = 0; j < 100; j++){
+            float samples = 1000.0;
+            for (float i = 0; i < samples; i++){
+                analogWrite(TORQUE_OUT_PIN, sin(2.0 * PI * (i / samples)) * 4095.0 * 0.5 * (k / 100) + 4095 * 0.5 );
+            }
+        }
+    }
+
+    /*this->setOutput(4095 * 0);
+    delay(100);
+    for(float k = 0; k < 0.5; k = k + 0.05){
+        this->setOutput(4095 * k);
+        while(this->getRotationSpeed() < 400){}
+        delay(100);
+        Serial.print("output: ");
+        Serial.println(4095 * k);
+        Serial.print("isRunning: ");
+        Serial.println(isRunning);
+        Serial.print("getRotationSpeed(): ");
+        Serial.println(getRotationSpeed());
+    }
+    delay(100);
+    for(float i = 0.5; i > 0.0; i = i - 0.05){
+        this->setOutput(4095 * i);
+        while(this->getRotationSpeed() < 400){}
+        delay(100);
+        Serial.print("output: ");
+        Serial.println(4095 * i);
+        Serial.print("isRunning: ");
+        Serial.println(isRunning);
+        Serial.print("getRotationSpeed(): ");
+        Serial.println(getRotationSpeed());
+    }
+    Serial.println("AutoDecog ended.");*/
+}

Libraries/PRG_342/prg_342.h

@@ -0,0 +1,73 @@
+#ifndef PRG_342_H
+#define PRG_342_H
+
+#include "Arduino.h"
+
+class PRG_342 {
+public:
+    PRG_342();  // Standardkonstruktor
+    PRG_342(float k_faktor);
+    ~PRG_342(); // Standarddestruktor
+
+    unsigned int ActOut;
+    static const int TORQUE_OUT_PIN = DAC1;
+
+    unsigned int getSetPoint(unsigned char format);
+    unsigned int getOutVolt();
+    void tareTorque();
+    void tareAngle();
+    void updateActOut(int actTorque);
+    int getTorque(int offset = true);
+    //int getTorque(int offset, unsigned int COUNTS);
+    int getTorque(int offset, float ALPHA);
+    int getAngle();
+    static volatile bool isRunning;
+    //void setOutput(unsigned int value);
+    //void setOutput(unsigned int value, bool noLimits = false, bool onlyWhenRunning = false);
+    void setOutput(unsigned int value, bool onlyWhenRunning = false);
+    void setOutput(int startValue, int endValue, int mA_per_ms);
+    void safeShutdown();
+    void safeShutdown(unsigned int time);
+    void safeShutdown(unsigned int time, unsigned int value);
+    void DynDecog(unsigned int actValue);
+    void AutoDecog();
+
+    float getRotationSpeed();
+
+private:
+    int getActTorque();
+    void updateActTorqueOut();
+    void updateValA0();
+    void updateValA1();
+    static void updateEncoder(); // Statische ISR-Funktion
+    float K_FAKTOR;
+
+    static volatile int encoderPos; // Encoderposition
+    static volatile int lastEncoderPos; // Encoderposition
+    static volatile int lastEncoded; // Letzter enkodierter Zustand
+    static volatile int ANGLE; // Winkel
+
+    static const int EncoderPinA = 22;
+    static const int EncoderPinB = 23;
+    static const int EncoderPinN = 24;
+    
+    static const int DMS_PIN = A0;
+    static const int EXTSET_PIN = A1;
+    static const int ACT_OUT_PIN = DAC0;
+
+    static int valA0;
+    static int valA1;
+
+    static int filteredValA1;
+
+    float exponentialFilter(float currentValue, float previousFilteredValue, float alpha);
+
+    int TORQUE;
+    int OFFSET;
+    unsigned int SETPOINT;
+    unsigned long lastSpeedCalcTime;
+    int lastAngle;
+
+};
+
+#endif