113 lines
2.9 KiB
C++
113 lines
2.9 KiB
C++
#define encoderPinA 22 // Pin für das Signal A
|
|
#define encoderPinB 23 // Pin für das Signal B
|
|
|
|
volatile int encoderPos = 0; // Variable zur Speicherung der relativen Position
|
|
int lastEncoded = 0; // Variable zur Speicherung des letzten Encodierten Zustands
|
|
|
|
int SerIn = 0;
|
|
float torque = 0;
|
|
float offset = 0;
|
|
float setpoint = 0;
|
|
unsigned int valA1 = 0;
|
|
unsigned int valA0 = 0;
|
|
|
|
void setup() {
|
|
// Initialize the DAC pin
|
|
analogWrite(DAC0, 0);
|
|
pinMode(DAC0, OUTPUT);
|
|
analogWrite(DAC1, 0);
|
|
pinMode(DAC1, OUTPUT);
|
|
|
|
pinMode(A0, INPUT);
|
|
pinMode(A1, INPUT);
|
|
|
|
pinMode(encoderPinA, INPUT);
|
|
pinMode(encoderPinB, INPUT);
|
|
attachInterrupt(digitalPinToInterrupt(encoderPinA), updateEncoder, CHANGE);
|
|
attachInterrupt(digitalPinToInterrupt(encoderPinB), updateEncoder, CHANGE);
|
|
|
|
analogWriteResolution(12); // Set the resolution to 12 bits (0-4095)
|
|
analogReadResolution(12);
|
|
|
|
Serial.begin(9600);
|
|
|
|
//delay(10000); // Evtl. vorhandene Spulenspannung senken.
|
|
}
|
|
|
|
void loop() {
|
|
if (Serial.available() > 0) {
|
|
delay(100);
|
|
SerIn = Serial.read();
|
|
if (SerIn == 't') {
|
|
tare();
|
|
} else {
|
|
SerIn = (SerIn - '0') * 10;
|
|
analogWrite(DAC1, SerIn * 40.95);
|
|
Serial.println(SerIn * 0.26);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
torque = getActTorque();
|
|
for (int i = 0; i < 9; i++) {
|
|
torque += getActTorque();
|
|
}
|
|
torque = torque * 0.1;
|
|
|
|
Serial.print("torque: ");
|
|
Serial.println(abs(torque + offset), 1);
|
|
analogWrite(DAC0, abs(torque + offset) * 81.9);
|
|
|
|
|
|
|
|
// Beispiel: Ausgabe der relativen Position
|
|
Serial.print("Relative Position: ");
|
|
Serial.println(encoderPos);
|
|
|
|
// Berechnung des relativen Winkels
|
|
float angle = (encoderPos / 20000.0) * 360.0;
|
|
Serial.print("Relative Angle: ");
|
|
Serial.println(angle, 1); // Ausgabe mit einer Nachkommastelle
|
|
|
|
delay(1000);
|
|
}
|
|
|
|
void 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 = (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) encoderPos++;
|
|
if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderPos--;
|
|
|
|
lastEncoded = encoded; // Aktualisieren des letzten Zustands
|
|
}
|
|
|
|
unsigned int VoltToInt(unsigned int voltage) {
|
|
return (voltage - 550.0) * 1.86136364; // (x - 550) * 4095/2200
|
|
}
|
|
|
|
float getActTorque() {
|
|
valA0 = analogRead(A0);
|
|
return (valA0 - 2047.5) * 0.024420024; // (x - 2047.5) * 100/4095
|
|
//return ((4095 * 1.00) - 2047.5) * 0.024420024;
|
|
}
|
|
|
|
float getSetPoint() {
|
|
return (analogRead(A1) * 0.002442002); //x * 10/4095
|
|
}
|
|
|
|
void tare() {
|
|
torque = getActTorque();
|
|
for (int i = 0; i < 99999; i++) {
|
|
torque += getActTorque();
|
|
}
|
|
torque = torque * 0.00001;
|
|
offset = 0 - torque;
|
|
|
|
encoderPos = 0;
|
|
} |