357 lines
10 KiB
C++
357 lines
10 KiB
C++
#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.");*/
|
|
} |