So, nach ein paar Stunden frustrierender Tests, bin ich nun zu dem Entschluss gekommen, dass ich einfach mit dem RB-35 nicht weiterkommen werde, da dieser eine sehr große Massenträgheit besitzt und deshalb nicht genau anzusteuern ist (jedenfalls für mich)...
Da ich jedoch dieses Thema sehr spannend finde gebe ich so schnell nicht auf.
Deshalb habe ich mir jetzt ein billiges Servo aus der Bastelkiste geschnappt und das Poti entfernt. Dann habe ich mir eine Adapterwelle gedreht, welche auf der einen Seite eine Abflachung besitzt, sodass die Welle genau in das Abtriebszahnrad passt, in dem vorher das Poti steckte. Auf der anderen Seite ragt nun ein Wellenstummel mit einem Magneten an der Spitze hinaus ( Dieser Aufbau soll nur zu Testzwecken dienen...).
Nun konnte ich bereits mit einem abgewandelten Programm akzeptable Ergebnisse erzielen.
Doch stellt sich für mich jetzt eine andere Frage. Wie kann ich verhindern, dass mein Servo spinnt, sobald es über Null fährt (denn es bekommt ja dann einen ungewollten Sprung...) ?
Hier ist noch mal mein neuer Code (wieder mit float, da es für mich keinen Unterschied gemacht hat)
Ich hoffe ihr versteht meine Frage und kennt eine LösungCode: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 esum; int ealt; int yaus; int y_max = 45; 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 regler() { if(soll_wert >= 10) { if(soll_wert <= 350) { 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; } } } }
Der Einsteiger
Weiss ehrlich gesagt nicht, wer da auf das Servo springt. Hat es was mit deinem Magneten zu tun?
Mit den Dingern hab ich noch nix gemacht (auch sowas, dass ich schon lange vor hab).
Du zählst doch da Impulse, oder?
Dann musst du doch wissen, wenn ne volle Umdrehung um ist, und fängst dann halt einfach von vorn an zu zählen (ggf. kannst du ja _dieses_ Ereignis auch noch speichern, dann weisst du später auch, wieviele Umdrehungen es waren).
Grüssle, Sly
..dem Inschenör ist nix zu schwör..
Ne, es ist ein bisschen anders... Also der AS5040 wird über den ssi Bus (glaube, der heißt so) ausgewertet. Diesen Programmteil habe ich nur von Google übernommen...Weiss ehrlich gesagt nicht, wer da auf das Servo springt. Hat es was mit deinem Magneten zu tun?
Mit den Dingern hab ich noch nix gemacht (auch sowas, dass ich schon lange vor hab).
Du zählst doch da Impulse, oder?
Dann musst du doch wissen, wenn ne volle Umdrehung um ist, und fängst dann halt einfach von vorn an zu zählen (ggf. kannst du ja _dieses_ Ereignis auch noch speichern, dann weisst du später auch, wieviele Umdrehungen es waren).
Also am Ende werden mir auf jeden Fall 0-360° ausgegeben (das funktioniert ja auch prima). Wenn ich also weiter drehe fängt er wieder mit Null an und so weiter...
Und genau das ist das spinnen, was ich meinte. Nehmen wir als Beispiel an ich möchte von 150° auf 1° positionieren. Dann kann es durch Schwingung und so weiter dazu kommen, dass meine Achse nun ein bisschen weiter dreht und dann 355° anzeigt wird. In diesem Fall dreht dann nämlich mein Servo nicht ein kleines Stück zurück, sondern macht fast eine ganze Umdrehung...
Wie kann ich das nun beheben (im Programmcode) ??
Hehe, damit plag ich mich auch gerade herum. Ich versuch das so zu lösen, dass ich nebenher in einer weiteren Variable die Umdrehungen zähle. Dazu berechne ich die Differenz zwischen dem aktuellen und dem vorherigen Winkel und wenn dieser über 180° ist, wird eine Umdrehung raufgezählt und bei kleiner -180° einen runter. Dann kommt alles in eine neue Variable, meinetwegen
Winkel=360*Umdrehungen+gemessener Winkel
Du dürftest allerdings nicht wirklich ° herausbekommen, sondern Werte von 0 bis 4096, oder? Weiß gerade nicht, ob der AS5040 12-Bit Auflösung hatte. Ich hab jedenfalls bei mir einen AS5048A drin, der hat 14 Bit, also muss ich da mit 16384 pro Umdrehung rechnen. Allerdings haut das bei mir mit den ganzen Datentypen noch nicht so ganz hin.
Und dein Getriebemotor hat ein zu hohes Trägheitsmoment? Ich hab bei mir auch einen Motor im RB-35-Format drin (angegeben mit 76 U/min im Leerlauf bei 12V) und jetzt, wo ich den PID-Regler ganz gut abgestimmt bekommen hab, ist zumindest mit dem Auge kaum noch irgendwelches Schwingen zu erkennen und die Bewegung ist auch ziemlich fix. Beim PID-Regler muss man übrigens auch teilweise ganz schön auf die Wertebereiche der Variablen achten, gerade der Integratoranteil macht gerne mal Mist.
AI - Artificial Idiocy
Na das ist doch schön zu hörenHehe, damit plag ich mich auch gerade herum. Ich versuch das so zu lösen, dass ich nebenher in einer weiteren Variable die Umdrehungen zähle. Dazu berechne ich die Differenz zwischen dem aktuellen und dem vorherigen Winkel und wenn dieser über 180° ist, wird eine Umdrehung raufgezählt und bei kleiner -180° einen runter. Dann kommt alles in eine neue Variable, meinetwegen
Werde ich mal Morgen testen.
Nein, ich bekomme natürlich eigentlich 0 bis 1023 raus. Aber das Arduino Beispielprogramm macht halt daraus gleich ganze Grad.
Was haut denn bei dir mit den Datetypen nicht so hin ?
Naja, ob das nun wirklich am Motor lag, kann ich nicht genau sagen. Es kann natürlich auch an meinen mangelnden Programmierkünsten liegen.
Dürfte ich vielleicht dein Programm sehen? Das würde mich sehr interessieren...
Hast du auch eine Seite, wo mehr über dein Projekt steht, oder youtube ?
Leider hab ich keine Seite und bei Youtube hab ich schon ewig nix hochgeladen. An sich wär das aber was, was ich mal gebrauchen könnte. Aber erstmal egal.
Hmm, wenn du mit dem Code was anfangen kannst ... ist in Bascom geschrieben
Die Probleme, die ich da derzeit habe, hat mit der Größe der Zahlen zu tun. Der Sensor gibt wie gesagt 14 Bit aus, mit Integer kann ich also gerade einmal 4 Umdrehungen abdecken (wobei das je nach Anwendung auch schon reichen kann). Nagut, wöllte ich die Auflösung nicht nutzen, könnte ich ganz leicht mittels Bitshift auf weniger Bit runterskalieren, aber ich wills ja gerne so präzise wie möglich. Jedenfalls hab ich damit erst recht Probleme beim Integrator, da dieser ja in jedem Reglertakt die Abweichung aufaddiert. Bei einem relativ großen Sprung kommt da wirklich viel zusammen und wenn man dann versucht, mit diesem riesigen Wert weiterzurechnen, bekommt man erst recht Probleme. Da verschwinden dann gerne mal Bits und der Regler dreht durch. Hatte zwischendurch schon paar Mal Versionen gehabt, da hat der Motor nicht aufgehört zu drehen. Jedenfalls ist mein nächster Schritt, mal eine vernünftige Kommunikation mit dem Rechner dazu zu basteln, um dann mal den Motor ordentlich modellieren und simulieren zu können. Ich arbeite bei Regelungstechnik doch lieber mit Übertragungsfunktionen, als nur herum zu probieren. Bei meinem letzten Experiment hatte die Verwendung einer Vorsteuerung schon eine enorme Verbesserung gebracht. Die sorgt nämlich dafür, dass der Motor hauptsächlich den Sollwerten entsprechend so angesteuert wird, dass er schon ohne Regelung etwa die Sollbahn abfahren würde. Dann braucht der Regler nur noch die dabei auftretende Abweichung ausregeln.Code:$regfile = "m32def.dat" $framesize = 32 $swstack = 32 $hwstack = 32 $crystal = 16000000 $baud = 9600 Declare Sub Serial0charmatch() Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , Polarity = Low , Phase = 1 , Clockrate = 128 Config Adc = Single , Prescaler = Auto , Reference = Avcc Config Timer1 = Pwm , Pwm = 10 , Compare A Pwm = Clear Up , Compare B Pwm = Clear Up , Prescale = 1 Config Timer0 = Timer , Prescale = 1024 On Timer0 Isr0 Config Lcdpin = Pin , Db4 = Portc.3 , Db5 = Portc.2 , Db6 = Portc.1 , _ Db7 = Portc.0 , E = Portc.4 , Rs = Portc.5 Config Lcd = 20 * 4 Cursor Off Cls Config Porta.0 = Input Config Porta.1 = Output Config Porta.2 = Output Config Portb.3 = Output Portb.3 = 1 Spiinit Config Serialin = Buffered , Size = 30 , Bytematch = 13 Dim Incoming_str As String * 4 Dim Angle As Integer Dim Angle_vor As Integer Dim Angle_low As Byte At Angle Overlay Dim Angle_high As Byte At Angle + 1 Overlay Dim Anglereg(2) As Byte Dim Errorreg(2) As Byte Dim Adcwert As Long Dim Tast As Integer Dim Temp As Long Dim E As Long Dim E_vor As Long Dim E_int As Long Dim De As Long Dim Count As Byte Dim Pos As Long Dim Turns As Integer Turns = 0 E_vor = 0 E_int = 0 De = 0 Count = 0 Anglereg(1) = &HFF Anglereg(2) = &HFF Errorreg(1) = &H40 Errorreg(2) = &H01 Wait 1 Porta.1 = 1 Porta.2 = 1 Portb.3 = 0 Waitus 10 'Angle_high = Spimove(errorreg(1) , 1) 'Angle_low = Spimove(errorreg(2) , 1) 'Waitus 10 Angle_high = Spimove(anglereg(1) , 1) Angle_low = Spimove(anglereg(2) , 1) Waitus 10 Portb.3 = 1 Angle_high = Angle_high And &B00111111 Adcwert = Angle Enable Interrupts Enable Timer0 Waitms 500 Do Count = Count + 1 If Count = 20 Then Count = 0 If Adcwert = 1000 Then Adcwert = 17384 Else Adcwert = 1000 End If End If Cls Locate 1 , 1 Lcd Angle 'anzeigen Istwinkel Locate 2 , 1 Lcd Adcwert 'anzeigen Sollwinkel Locate 2 , 10 Lcd E Locate 1 , 10 Lcd E_int Locate 3 , 1 Lcd Tast 'anzeigen PWM-Wert Locate 4 , 10 Lcd Pos Locate 4 , 1 Lcd Turns Waitms 100 Loop Sub Serial0charmatch() Input Incoming_str Noecho Print Angle End Sub Isr0: Load Timer0 , 30 Portb.3 = 0 Waitus 5 Angle_high = Spimove(anglereg(1) , 1) Angle_low = Spimove(anglereg(2) , 1) Waitus 5 Portb.3 = 1 Angle_high = Angle_high And &B00111111 Temp = Angle - Angle_vor If Temp < -8192 Then Turns = Turns + 1 Elseif Temp > 8192 Then Turns = Turns - 1 End If Pos = 16384 * Turns Pos = Pos + Angle Angle_vor = Angle ' Start Adc ' Adcwert = Getadc(0) ' Adcwert = Adcwert * 16 E = Adcwert - Pos De = E - E_vor If De > 400 Then De = 400 Elseif De < -400 Then De = -400 End If E_vor = E Tast = E * 2 Temp = E_int / 128 Tast = Tast + Temp Temp = De * 40 Tast = Tast + Temp If Tast > 0 Then Porta.1 = 1 Porta.2 = 0 Else Porta.1 = 0 Porta.2 = 1 End If Tast = Abs(tast) Tast = Tast + 400 If Tast > 1023 Then Tast = 1023 Else E_int = E_int + E End If Compare1a = Tast Return
AI - Artificial Idiocy
So, ich bin es mal wieder. Hatte leider die Tage doch keine Zeit. Also jetzt:
Erstmal vielen Dank an dich GeistesblitzJetzt kann mein Servo endlich 360° ohne Probleme drehen ....
Nur die Regelung muss ich noch ein bisschen verbessern, aber erstmal egal.
Also Bascom benutze ich normalerweise auch, doch steige ich durch dein Programm trotzdem nicht durch (im Thema Software bin ich eben noch ein EinsteigerHmm, wenn du mit dem Code was anfangen kannst ... ist in Bascom geschrieben).
Ist das Programm komplett mit Regelung, oder fehlt da noch was ?
Also möchtest du eher ein endlos Servo bauen (vgl. mit Schrittmotor...) ?Der Sensor gibt wie gesagt 14 Bit aus, mit Integer kann ich also gerade einmal 4 Umdrehungen abdecken (wobei das je nach Anwendung auch schon reichen kann).
Mein Servo soll übrigens eher eine Art Eigenbau eines Dynamixels sein. Aber das nimmt sich ja nicht so viel.
Wie genau soll das mit einer Vorsteuerung funktionieren ?Bei meinem letzten Experiment hatte die Verwendung einer Vorsteuerung schon eine enorme Verbesserung gebracht. Die sorgt nämlich dafür, dass der Motor hauptsächlich den Sollwerten entsprechend so angesteuert wird, dass er schon ohne Regelung etwa die Sollbahn abfahren würde. Dann braucht der Regler nur noch die dabei auftretende Abweichung ausregeln.
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.
AI - Artificial Idiocy
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):
Code: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; } }
Berechtigungen
Zitieren
Lesezeichen