DIY Servo

[Geistesblitz]

Naja, man kann die Dynamik eines Elektromotors mittels Differentialgleichungen beschreiben, und wenn man die passend umstellt, kann man aus dem Zeitverlauf von Sollposition, -geschwindigkeit und -beschleunigung den Spannungsverlauf berechnen, der genau zur gewünschten Motorbewegung führt. Die Regelung ist dann nur noch zum Ausgleichen von Modellabweichungen und Störungen da. Bringt allerdings nur etwas, wenn sich die Sollposition nicht nur sprunghaft ändert, sondern einen halbwegs glatten Verlauf zeigt. Wenn man nämlich einen Sprung versucht zu differenzieren, bekommt man nur einen kurzen Puls. Da geht der Motor durch die Regelung eh in die Begrenzung. Der Sollwertverlauf sollte also so geschaffen werden, dass der Motor da auch hinterherkommt, ich möchte eben eine gute Folgeregelung erzielen. Für das Anfahren von diskreten Positionen reicht der Regler an sich aus.

Ja, der Regler befindet sich in der ISR. Da wird im Prinzip nur der Winkel (Angle) mittels SPI vom Sensor eingelesen, die Position (Pos) für eben mehrere Umdrehungen berechnet und mit dem Sollwert (Adcwert) verglichen. Aus dem Vergleich resultiert die Abweichung (E), von der dann Differenz (De) und Integral (E_int) gebildet werden, jeweils mit einer Verstärkung multipliziert und in der Stellgröße (Tast) gespeichert. Anschließend noch ein wenig Anpassung wegen der Begrenzung, da Tast eben größer werden kann als die maximalen 1023 für die PWM.

Das Programm ist zugegebenermaßen nicht wirklich übersichtlich und es hat sich seit ich es hier reingestellt hab auch schon wieder an vielen Stellen verändert. Wahrscheinlich schreib ich es nochmal neu, wenn alles weitestgehend passt. Die Vorsteuerung hab ich zwar schon versucht zu implementieren, allerdings bekomm ich das Modell des Motors nicht zufriedenstellend aufgestellt.

[Der Einsteiger]

Hi
Pünktlich zum Jahresbeginn gibt es von mir ein Update
Ich habe es in den Ferien endlich mal wieder geschafft, etwas für mein Hobby zu tun....
Dabei herausgekommen ist ein neues und auch viel stärkeres Servo. :



Wer sich für den Code interessiert, der jetzt auch schon ganz gut funktioniert (für Arduino):

const int ledPin = 13; //LED connected to digital pin 13
const int clockPin = 7; //output to clock
const int CSnPin = 6; //output to chip select
const int inputPin = 2; //read AS5040

int inputstream = 0; //one bit read from pin
long packeddata = 0; //two bytes concatenated from inputstream
long angle = 0; //holds processed angle value
long anglemask = 65472; //0x1111111111000000: mask to obtain first 10 digits with position info
long statusmask = 63; //0x000000000111111; mask to obtain last 6 digits containing status info
long statusbits; //holds status/error information
int DECn; //bit holding decreasing magnet field error data
int INCn; //bit holding increasing magnet field error data
int OCF; //bit holding startup-valid bit
int COF; //bit holding cordic DSP processing error data
int LIN; //bit holding magnet field displacement error data
int debug = 1; //SET THIS TO 0 TO DISABLE PRINTING OF ERROR CODES
int shortdelay = 80; // this is the microseconds of delay in the data clock
int longdelay = 8; // this is the milliseconds between readings


int pwm_pin = 3;  //Motor pwm
int in_1 = 4;   //Motor Richtung
int in_2 = 5;   //Motor Richtung
String msg;
char a;
int soll_wert;
int ist_wert;
int e;
int Winkel;
int Winkel_alt;
int Winkel_abweichung;
int Um;
int esum;
int ealt;
int yaus;
int y_max = 128;
int y_min = 0;
int esum_max = 700;

float y;
float i;
float d;
float Kp = 2.0;  //P
float Ki = 3.0;  //I
float Kd = 0.5;  //D
float Ta = 0.02;  //Abtast

void setup()
{
Serial.begin(9600);
pinMode(ledPin, OUTPUT); // visual signal of I/O to chip
pinMode(clockPin, OUTPUT); // SCK
pinMode(CSnPin, OUTPUT); // CSn -- has to toggle high and low to signal chip to start data transfer
pinMode(inputPin, INPUT); // SDA

pinMode(in_1, OUTPUT);
  pinMode(in_2, OUTPUT);
  pinMode(pwm_pin, OUTPUT);
  
  digitalWrite(in_1, HIGH);  
  digitalWrite(in_2, LOW);
  analogWrite(pwm_pin, 0);

  // CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
digitalWrite(CSnPin, HIGH); // CSn high
digitalWrite(clockPin, HIGH); // CLK high
delay(longdelay);
digitalWrite(ledPin, HIGH); // signal start of transfer with LED
digitalWrite(CSnPin, LOW); // CSn low: start of transfer
delayMicroseconds(shortdelay);  // delay for chip -- 1000x as long as it needs to be
digitalWrite(clockPin, LOW); // CLK goes low: start clocking
delayMicroseconds(shortdelay);  // hold low for 10 ms
for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
{
digitalWrite(clockPin, HIGH); //clock goes high
delayMicroseconds(shortdelay); // wait 10ms
inputstream =digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC);
packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
digitalWrite(clockPin, LOW);
delayMicroseconds(shortdelay); // end of one clock cycle
}
// end of entire clock cycle
//Serial.println(" ");
digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
// Serial.println(packeddata,BIN);
angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
angle = angle * 0.3515; // angle * (360/1024) == actual degrees
//Serial.print("angle: "); // and, finally, print it.


Winkel_alt = int(angle);

//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
if (debug)
{
statusbits = packeddata & statusmask;
DECn = statusbits & 2; // goes high if magnet moved away from IC
INCn = statusbits & 4; // goes high if magnet moved towards IC
LIN = statusbits & 8; // goes high for linearity alarm
COF = statusbits & 16; // goes high for cordic overflow: data invalid
OCF = statusbits & 32; // this is 1 when the chip startup is finished.
if (DECn && INCn) { Serial.println("magnet moved out of range"); }
else
{
if (DECn) { Serial.println("magnet moved away from chip"); }
if (INCn) { Serial.println("magnet moved towards chip"); }
}
if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
}

packeddata = 0; // reset both variables to zero so they don't just accumulate
angle = 0;  
}







void loop()
{

//unsigned long startmicros=micros();

  if(Serial.available() > 0)
  {
    msg = "";
    while(Serial.available() > 0){
      a = Serial.read();
      msg = msg + String(a);      
      delay(250);      
    }
    soll_wert = msg.toInt();
    Serial.println(soll_wert);
  }  
    
      
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
digitalWrite(CSnPin, HIGH); // CSn high
digitalWrite(clockPin, HIGH); // CLK high
delay(longdelay);
digitalWrite(ledPin, HIGH); // signal start of transfer with LED
digitalWrite(CSnPin, LOW); // CSn low: start of transfer
delayMicroseconds(shortdelay);  // delay for chip -- 1000x as long as it needs to be
digitalWrite(clockPin, LOW); // CLK goes low: start clocking
delayMicroseconds(shortdelay);  // hold low for 10 ms
for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
{
digitalWrite(clockPin, HIGH); //clock goes high
delayMicroseconds(shortdelay); // wait 10ms
inputstream =digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC);
packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
digitalWrite(clockPin, LOW);
delayMicroseconds(shortdelay); // end of one clock cycle
}
// end of entire clock cycle
//Serial.println(" ");
digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
// Serial.println(packeddata,BIN);
angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
angle = angle * 0.3515; // angle * (360/1024) == actual degrees
//Serial.print("angle: "); // and, finally, print it.


//Serial.println(angle, DEC);
Winkel = int(angle);

//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
if (debug)
{
statusbits = packeddata & statusmask;
DECn = statusbits & 2; // goes high if magnet moved away from IC
INCn = statusbits & 4; // goes high if magnet moved towards IC
LIN = statusbits & 8; // goes high for linearity alarm
COF = statusbits & 16; // goes high for cordic overflow: data invalid
OCF = statusbits & 32; // this is 1 when the chip startup is finished.
if (DECn && INCn) { Serial.println("magnet moved out of range"); }
else
{
if (DECn) { Serial.println("magnet moved away from chip"); }
if (INCn) { Serial.println("magnet moved towards chip"); }
}
if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
}

packeddata = 0; // reset both variables to zero so they don't just accumulate
angle = 0;

regler();

//unsigned long endmicros=micros();

//Serial.println(endmicros-startmicros,DEC);

}


void regler()  {
  
Winkel_abweichung = Winkel_alt - Winkel;
if(Winkel_abweichung > 180)  {
  Um = Um + 1;
}

if(Winkel_abweichung < -180)  {
  Um = Um - 1;
}

ist_wert = 360 * Um;
ist_wert = Winkel + ist_wert;

Winkel_alt = Winkel;
Serial.println(ist_wert);

      e = soll_wert - ist_wert;
      e = abs(e);            
      esum = esum + e;
      if(esum > esum_max)  {
        esum = esum_max;
      }
      
      i = float(esum) * Ta;
      i = i * Ki;
      
      d = float(e) - float(ealt);
      d = d / Ta;
      d = d * Kd;      
      
      y = Kp * e;
      y = y + i;
      y = y + d;
      
      yaus = int(y);
      //Serial.println(y);
      ealt = e;
      
      if(yaus > y_max)  {
        yaus = y_max;
      }
      
      if(yaus < y_min)  {
        yaus = y_min;
      }
      
      if(ist_wert > soll_wert)  {
        digitalWrite(in_1, HIGH);  
        digitalWrite(in_2, LOW);
        analogWrite(pwm_pin, yaus);
      }
      
      if(ist_wert < soll_wert)  {
        digitalWrite(in_1, LOW);  
        digitalWrite(in_2, HIGH);
        analogWrite(pwm_pin, yaus);
      }
      
      if(ist_wert == soll_wert)  {
        digitalWrite(in_1, LOW);  
        digitalWrite(in_2, LOW);
        analogWrite(pwm_pin, 0);
        esum = 0;
        ealt = 0;
      }
             
}