; ; Projekt 108/S ( 08. Mai 2008 ) ; ; ;============================================================================== ; ; <<<<<<<<<< S W I T C H S E R V >>>>>>>>>> ; ;============================================================================== ; ; SERVOERSATZSCHALTUNG für Schalterservos (Version für 12F675 & 12C672) ; ; Programm erzeugt aus EIN/AUS Schaltersignalen PWM-Signale zur Ansteuerung ; des angeschlossenen Servomotors, wobei die jeweilige Servoposition mittels ; Servopotis bestimmt wird. ; Verschiedene, über Konstante "method" vorgebbare Auslenkmethoden sind ; möglich : _____ ; * kurzer Halbausschlag aus Mitte (Trigger) I o I ; * beidseitiger Halbausschlag aus Mitte VDD-I I-VSS ; * Vollausschlag mit einstellbarer Geschwindigkeit I I ; * Step by step Vollausschlag, d.h. Servo rückt S1-I I-PWM-1 ; bei kurzen EIN-Schaltungen jeweils nur I I ; ein kurzes Stück weiter. Nach längerer Trimmer-I I-PWM-2 ; Stellung auf EIN (Variable "ontime") I I ; erfolgt dann kontinuierlicher Vollaus- S2-I I-S-Poti ; schlag nach vorheriger Methode. I_____I ; * SWITCHSERV funktioniert entweder mit einem ; Schalter abwechselnd oder mit je einem Schalter für jede ; Richtung. ; * Bei Schalter AUS ist sowohl ein Auslaufen in die Endstellung als ; auch ein Sofortstop möglich. ; Einer der Parameter Auslenkgeschwindigkeit, Servomitte oder Ausschlag- ; amplitude kann mittels externem Trimmer eingestellt werden; Vorgabe ; der Parameters via Konstante "trimsel". ; Die Logik der Schaltersignale (aktiv HIGH bzw. LOW) kann mit Parameter "L" ; von im Programmkopf definiert werden; im Falle von negativer Logik ; ist S1 intern über Widerstände mit dem Pluspol verbunden (pull ups). ; ; Die Schaltung ersetzt die analoge Elektronik eines kommerziellen Servos. ; ; Eingebaut in : ; ; [ SSRV-03/08 ] LHD-7 Iwo-Jima ( 05.03.2008 ) ; [ SSRV-05/08 ] LHA-5 Peleliu ( 07.05.2008 ) ; ; D. Lübbesmeyer ; ;============================================================================== ; LIST P=12F675 ; PROZESSOR-Definition (12F675 oder 12C672) ; ;============================================================================== ; ; IFDEF __12F675 ERRORLEVEL -302 ; __config H'3F8C' ; interner Oszillator, WDT=on, Brown out=off, MCRL=off ; __MAXRAM H'FF' __BADRAM H'06'-H'09', H'0D', H'11'-H'18', H'1A'-H'1D', H'60'-H'7F' __BADRAM H'86'-H'89', H'8D', H'8F', H'91'-H'94', H'97'-H'98', H'E0'-H'FF' ; IFNDEF __12F675 MESSG "Processor-header file mismatch. Verify selected processor." ENDIF ELSE ERRORLEVEL -302 __config H'3F6C' ; Code Protection=off, MCRL=intern, PWRT=on, ; WDT=on, interner RC-Oszillator ohne Ausgabe __MAXRAM H'FF' __BADRAM H'06'-H'09', H'0D'-H'1D' __BADRAM H'86'-H'89', H'8D', H'90'-H'9E', H'C0'-H'EF' ; IFNDEF __12C672 MESSG "Processor-header file mismatch. Verify selected processor." ENDIF ENDIF ; ; ;-----------------------------------------------------------------------------! ; ! ; ======== PROGAMM - OPTIONEN ======== ! ; ! #DEFINE SPINDPOT ; Servo mit Spindel-Potentiometer ! #UNDEFINE SPINDPOT ; Servo mit Trimmer-Potentiometer ! ; ! ;-----------------------------------------------------------------------------! ; #DEFINE RESFLAG FLAG,6 ; Kopie der POR-Flagge ; ;------ Konstanten Definition --------------------------------------------- w EQU H'00' ; Ziel W f EQU H'01' ; F z EQU H'02' ; ==== STATUS Bits c EQU H'00' rp0 EQU H'05' ; ;----- Anwender Werte -------------------------------------------------------- ; IFDEF __12F675 adc00 EQU B'00001001' ; ADFM=0, VCFG=0, A/D-Konversion, Kanal AN2 adc01 EQU B'00001101' ; " " " , Kanal AN3 adwgo EQU H'01' ; ADC-Bit für AD-go ELSE adc00 EQU B'01010001' ; A/D Konversion FOSC/8, Kanal GP2 adc01 EQU B'01011001' ; A/D Konversion FOSC/8, Kanal GP4 adwgo EQU H'02' ; ADC-Bit für AD-go ENDIF ; ;----- vom Benutzer zu definierende Werte ------------------------------------ ; ; ------- AUSSCHLAGSSTEUERUNG ------ method EQU B'00001010' ; Methodencode : ; [Lba00] ---> kurzer, beidseitiger Ausschlag ; [Lba01] ---> beidseitiger Ausschlag ; [Lba10] ---> Vollausschlag einstellbarer Geschwind. ; [Lba11] ---> Step by step Vollausschlag ; a = 0 --> 1 Schalter (abwechselnd, bei [Lba11] ; nicht möglich) ; = 1 --> 2 Schalter ; b = 0 --> Servo in Endstellung bei Schalter-AUS ; = 1 --> sofortiger Servostop " " " ; wird für [Lba00] und [Lba01] ignoriert ; L = Schaltersignallogik : 0 = positiv, ; 1 = neagtiv trimsel EQU H'03' ; [ 0 ] = Trimmer nicht verwendet, ; [ 1 ] = Servomittelstellung, ; [ 2 ] = Ausschlagsamplitude, ; [ 3 ] = Methoden-abhängig Slowmove-Laufzeit oder ; Ausschlags-Haltezeit. step EQU H'02' ; Increment bei step by step ontime EQU H'F0' ; Step by step EIN-Zeit bis Umschalten in den ; Move-Mode (= sqrt(mSek)) cSHI EQU H'01' ; Wert von dSHI slowfak EQU H'00' ; Faktor auf Servolaufzeit ; (Laufzeit = vortl*szeit/(2**Slowfak) ; * [servmax-servmin] ; pwmeps EQU H'08' ; minimale Impulslänge rglstop EQU H'10' ; Abbruch der Regelung nach .. schritten mit ; maximalem PWM thalt EQU H'F0' ; normale Schalterhaltezeit thalt_d EQU H'14' ; " bei Direktstop saus EQU H'36' ; Schalter-AUS-Zeit vor SLEEP (=sqrt(1000*Sekunden)) servmin EQU H'19' ; minimale Servostellung (Potistellungsbegrenzung) servmid EQU H'7F' ; mittlere " servmax EQU H'EA' ; maximaler " (Potistellungsbegrenzung) strtpos EQU H'00' ; Servo-Startposition (0=min oder 1=max) srvspan EQU H'66' ; maximale Servo-Auschlagsamplitude szeit EQU H'30' ; Servolaufzeit bzw. Haltezeit bei getriggertem Servo- ; ausschlag ;----- Register Files ---------------------------------------------------- ; cblock H'00' ; Bank -0- INDF,TMR0,PCL,STATUS,FSR,GPIO endc PCLATH EQU H'0A' INTCON EQU H'0B' CMCON EQU H'19' ADRESH EQU H'1E' ADCON0 EQU H'1F' ; cblock H'20' FLAG ; Flaggen (Bit_0 = First-ON-Flagge, ; _1 = Trigger-Flagge, ; _2 = Step by step Flagge, ; _6 = Kopie der POR-Flagge I ; Laufvariable SHALT,SS,SS_o,SSS ; Schalter-Haltezeit & Schalterstellungen RICHT ; Servolaufrichtung (bei automatischem ; Richtungswechsel mit einem Schalter (S1)) MTHS ; Methodenselektion SRVTIM,MOVTIM,TRIGT:2 ; Auslenkzeit & Servo-Laufvariable & Triggerhaltezeit SHIB,dSHI,SHIL ; Servo- Sollstellung, SHIB-Increment & Servo- Limit SVMD,SVMI,SVMX,SVAMP ; Servosignalwerte (Mitte, Minimum, Maximum, Amplitude) PWM:2,PWMo,NPWMIN ; PWM Impulslängen, Zähler für minimale Impulslänge TREF,GIOH ; Ref-Zeit & HIGH Var. zum GPIO-ANDden TAUS:2,TON:2 ; Schalter-AUS Durchläufe, ON-Time Zähler (step-Mode) TRIMMo ; vorheriges Ergebnis der Trimmer-AD-Wandlung endc II EQU I ; Gleichsetzen von I und II ; OPTION_REG EQU H'81' ; Bank -1- TRISIO EQU H'85' PCON EQU H'8E' IFDEF __12F675 OSCCAL EQU H'90' ELSE OSCCAL EQU H'8F' ENDIF WPU EQU H'95' IOC EQU H'96' EEDATA EQU H'9A' EEADR EQU H'9B' EECON1 EQU H'9C' EECON2 EQU H'9D' ANSEL EQU H'9F' OPTVAL EQU H'A0' ; ;============================================================================== ; ; -------- Standard-Macros : c_add_x macro const,X,d ; addiert Konstante zu X mit Destination d movlw const addwf X,d endm c.eq.0 macro const ; nächsten Befehl ausführen wenn : const = 0 clrw addlw const btfsc STATUS,z endm c.eq.n macro const,zahl ; nächsten Befehl ausführen wenn : const = Zahl movlw const sublw zahl btfsc STATUS,z endm c_sub_x macro const,X,d ; subtrahiert Konstante von X mit Destination d movlw const subwf X,d endm c_to_io macro const,msk,Y ; Kopiert mit "msk* maskierte Bit von const in "Y" movf Y,w ; liest Y andlw msk ; setzt maskiert Bits null iorlw const ; OR-Verknüpfung mit const movwf Y ; Ergebnis zurück nach Y endm c_to_x macro const,X ; X = Konstante; Läd Konstante in X movlw const movwf X endm min_x_c macro X,const ; Minimum von X und Konstante movlw const ; wenn const überschritten, wird subwf X,w ; X = const gesetzt movlw const btfsc STATUS,c movwf X endm max_x_c macro X,const ; Maximum von X und Konstante movlw const ; wenn const unterschritten, wird subwf X,w ; X = const gesetzt movlw const btfss STATUS,c movwf X endm min_x_y macro X,Y ; Minimum von X und Y movf Y,w ; wenn Y überschritten, wird subwf X,w ; X = Y gesetzt movf Y,w btfsc STATUS,c movwf X endm max_x_y macro X,Y ; Maximum von X und Y movf Y,w ; wenn Y unterschritten, wird subwf X,w ; X = Y gesetzt movf Y,w btfsc STATUS,c movwf X endm neg macro X ; X = -X comf X,f incf X,f endm toggl_m macro X,mask ; Invertierung mehrerer mit "mask" movlw mask ; definierter Bits von X xorwf X,f endm w.eq.0 macro ; nächsten Befehl ausführen wenn : W = 0 andlw H'FF' btfsc STATUS,z endm w.ne.0 macro ; nächsten Befehl ausführen wenn : w # 0 andlw H'FF' btfss STATUS,z endm x.eq.0 macro X,d ; nächsten Befehl ausführen wenn : X = 0 movf X,d btfsc STATUS,z endm x.eq.c macro X,const ; nächsten Befehl ausführen wenn : X = const movlw const subwf X,w btfsc STATUS,z endm x.eq.y macro X,Y ; nächsten Befehl ausführen wenn : X = Y movf X,w subwf Y,w btfsc STATUS,z endm x.eq.yR macro X,Y ; nächsten Befehl ausführen wenn : X = Y movf X,w subwf Y,f ; und Laden von X in Y movwf Y btfsc STATUS,z endm x.ge.c macro X,const ; nächsten Befehl ausführen wenn : X > const movlw const subwf X,w btfsc STATUS,c endm x.ge.y macro X,Y ; nächsten Befehl ausführen wenn : X > Y movf Y,w subwf X,w btfsc STATUS,c endm x.lt.c macro X,const ; nächsten Befehl ausführen wenn : X < const movlw const subwf X,w btfss STATUS,c endm x.lt.y macro X,Y ; nächsten Befehl ausführen wenn : X < Y movf Y,w subwf X,w btfss STATUS,c endm x.ne.0 macro X,d ; nächsten Befehl ausführen wenn : X # 0 movf X,d btfss STATUS,z endm x_to_io macro X,msk,Y ; Kopiert mit "msk* maskierte BitS von X in "Y" movf Y,w ; liest Y andlw msk ; setzt maskiert Bits null iorwf X,w ; OR-Verknüpfung mit X movwf Y ; Ergebnis zurück nach Y endm y_add_x macro Y,X,d ; addiert Y zu X mit Destination d movf Y,w addwf X,d endm abs.g.e macro Y,X,eps ; führt folgendes Statement aus wenn : y_sub_x Y,X,w ; ---- abs(Y - X) > eps (eps = 2**n) btfss STATUS,c sublw H'FF' andlw H'100'-eps btfss STATUS,z endm abs.l.e macro Y,X,eps ; führt folgendes Statement aus wenn : y_sub_x Y,X,w ; ---- abs(Y - X) < eps (eps = 2**n) btfss STATUS,c sublw H'FF' andlw H'100'-eps btfsc STATUS,z endm y_sub_x macro Y,X,d ; subtrahiert Y von X mit Destination d movf Y,w subwf X,d endm y_to_x macro Y,X ; X = Y ; Läd Y in X movf Y,w movwf X endm adw macro adco ; ------ AD-WANDLUNG des Kanals "adco" ------- c_to_x adco,ADCON0 c_to_x H'07',II ; Wartezeit 3*II + 2 us decfsz II,f goto $-1 bsf ADCON0,adwgo ; Start AD-Wandler btfsc ADCON0,adwgo ; mit Test ob AD-Wandlung beendet goto $-1 bcf ADCON0,0 ; ADW abschalten endm ; stpstp macro fkt ; Methodentest step by step movlw method andlw H'03' sublw H'03' btfsc FLAG,2 bcf STATUS,z fkt STATUS,z endm ; ; ; ;================================================================================ ; ; org H'00' goto START org H'04' retfie ; Interrupt-Adresse [004] ; START call GLBDEF c_to_x H'01',TRIMMo btfsc RESFLAG ; wenn WDT- oder Brownout- Restart (RESFLAG=H) goto PWMSIG ; ---> Initialisierung überspringen call SETVAR clrf FLAG c_to_x cSHI,dSHI c_to_x strtpos,RICHT ; Startwert von RICHT (min oder max) incf RICHT,w btfss MTHS,1 ; wenn Halbausschlag ---> SSS=RICHT+1 clrw ; sonst SSS=0 movwf SSS stpstp btfsc ; wenn Step to step Methode [ba11] (z=1) goto STRT_0 btfsc MTHS,1 ; wenn kein Halbausschlag btfss MTHS,3 goto STRT_1 STRT_0 adw adc00 ; ADFM=0, VCFG=0, A/D-Konversion, Kanal AN2 y_to_x ADRESH,SHIB ; SHIB = ADRESH goto STRT_2 STRT_1 movf SSS,w ; Bestimmung von SHIL durch indirekte addlw SVMD ; Adressierung von SVMD, SVMI und SVMX movwf FSR ; SHIL = SVMD (SSS=0), = SVMI (SSS=1) und y_to_x INDF,SHIL ; = SVMX (SSS=2) movwf SHIB STRT_2 clrf SS clrf SS_o clrf PWM clrf PWMo ; ; >>>>>>>>>>> PWM - SIGNALGENERIERUNG <<<<<<<<<<<< ; Hauptschleife PWMSIG movf PWM,w sublw imax ; PWM+1 = imax - PWM movwf PWM+1 btfsc STATUS,z ; wenn PWM+1 = 0 (LOW-Anteil) goto PWMhi ; ---> direkt zum HIGH-Anteil PWMlo movf GPIO,w ; PWM-OUTPUT : liest GPIO andlw H'3C' ; und setzt erste beiden Bits null movwf GPIO ; Ergebnis zurück nach GPIO y_add_x TMR0,PWM+1,w movwf TREF x.lt.c PWM+1,imax/2+1 ; wenn LOW-Zykluslänge < imax/2+1 goto PWMlo_0 ; ---> keine Neuwertberechnung call RGLSTK ; Bestimmung der Regelstrecke w.ne.0 goto PWMlo_0 call SWITCH ; Bestimmung der Schalterstellung call SETVAR ; " der Parameter w.eq.0 call SRVSOL ; ---> Berechnung der Servo-Sollposition PWMlo_0 clrwdt abs.g.e TMR0,TREF,H'02' ; wenn abs(TMR0-TREF) > 2 goto PWMlo_0 ; ---> LOW-Output halten ; ; ----- HIGH-Zyklus ----- PWMhi x.eq.0 PWM,f ; wenn PWM = 0 (HIGH-Anteil) goto PWMSIG ; ---> zurück zum Schleifenbeginn x_to_io GIOH,H'3C',GPIO ; Kopie von GIOH auf GPIO,0 & 1 y_add_x TMR0,PWM,w movwf TREF x.ge.c PWM,imax/2+1 ; wenn HIGH-Zykluslänge > imax/2+1 call RGLSTK ; ---> Bestimmung der Regelstrecke PWMhi_0 clrwdt abs.g.e TMR0,TREF,H'02' ; wenn abs(TMR0-TREF) > 2 goto PWMhi_0 ; ---> HIGH-Output halten goto PWMSIG ; ; ;------------------------------------------------------------------------------ ; ; subroutine << SKNLI >> erzeugt eine nichtlineare Funktion Impulslänge zu ; PWM duty cycle, um den unteren Drehzahlbereich des ; des Servomotors anzuheben. ; ; ------- MOTORSTEUERUNG ------ imax EQU H'7D' ; maximale Impulslänge (PWM-Frequenz = 1/(imax*vortl) f_on EQU H'02' ; Totzeit um Nullpunkt ( 6% von PWM+1max) IFDEF SPINDPOT max_F EQU H'04' ; maximale Fahrsignallänge (= Signal/14 us) ; SKNLI clrf PCLATH decf PWM,w addwf PCL,f ; ===== TABELLE für Kennlinie ===== DT H'60', H'74', H'7D', H'7D', H'7D' ELSE max_F EQU H'0F' ; maximale Fahrsignallänge (= Signal/14 us) ; ; { y = 430. * (exp[ 0.01*x] - 1) + 75.0 } ; SKNLI clrf PCLATH decf PWM,w addwf PCL,f ; ===== TABELLE für Kennlinie ===== DT H'4B', H'4F', H'54', H'58', H'5D', H'61', H'66', H'6A' DT H'6F', H'74', H'78', H'7D', H'7D', H'7D' ENDIF ; ; ;------------------------------------------------------------------------------ ; ; subroutine << GLBDEF >> Initialisierung des Ports sowie einiger Register ; GLBDEF IFDEF __12F675 call H'3FF' ; Laden der Oszillator-Kalibrierung aus Chip ELSE call H'07FF' ; Laden der Oszillator-Kalibrierung aus Chip ; movlw H'88' ; Laden der Calibrierung für Fenster-Chip nach ; Löschung des JW-Chips ENDIF bsf STATUS,rp0 movwf OSCCAL clrw btfsc PCON,1 ; Kopie von PCON,1 via W auf RESFLAG movlw H'01' ; über Bankgrenze hinweg bsf PCON,1 bcf STATUS,rp0 clrwdt clrf INTCON btfsc INTCON,7 goto $-H'03' bsf RESFLAG ; W=1 ---> WDT oder Brown-out : RESFLAG = H w.eq.0 bcf RESFLAG ; W=0 ---> Neustart : RESFLAG = L c_to_x method,MTHS ; Methodenselektor (in Definitionen vorgeben) clrf TMR0 IFDEF __12F675 c_to_x H'07',CMCON ; Komparator abschalten bsf STATUS,rp0 c_to_x H'C2',OPTVAL ; TMR0-Def.: GPPU=1 (kein pull up), INTEGD=1, ; T0CS=0 (intern), T0SF=0, PSA=0 (TMR0), ; PS2,PS1,PS0=0,1,0 (Vorteiler=8) movlw method andlw H'10' ; Abfrage des Signallogikparamters "L" btfsc STATUS,z ; wenn L = 1 (negative Signallogik) goto $+H'03' ; ---> WPU = H'20' (pull-up von GPIO,5), bcf OPTVAL,7 ; sonst WPU = 0 c_to_x H'20',WPU c_to_x H'28',IOC ; Interrupt auf Wechsel von GPIO,3 bzw. GPIO,5 c_to_x H'1C',ANSEL ; Analog-Kanäle GP2 und GP4, FOSC/8 ELSE bsf STATUS,rp0 clrf ANSEL ; alles Analog-Kanäle, V_ref=VDD ENDIF y_to_x OPTVAL,OPTION_REG ; Timer-0 starten c_to_x H'3C',TRISIO ; Pin-Definition : ; Pin7=PWM-1, Pin6=PMW-2 (output), ; Pin5=Servopoti, Pin4=S1, ; Pin3=Einstellung, Pin2=S2 (input) bcf STATUS,rp0 c_to_x saus,TAUS ; TAUS setzen movwf TAUS+1 return ; ; ;------------------------------------------------------------------------------ ; ; subroutine << RGLSTK >> Bestimmung des PWM-HIGH-Anteils des Motorsignals. ; Zunächst wird mittels AD-Wandlung der Servo-Poten- ; tiometerstellung der Servo-Istwert bestimmt und ; dann aus der Differenz die Regelgrösse ermittelt, ; aus der sich schliesslich die Laufrichtung und die ; Drehzahl des Servomotors ergeben. Ist ausgeregelt, ; wird beim Rücksprung W=0 gesetzt, sonst W=1. ; Bleibt Servo bei PWM>0 hängen, wird mit pwmexps ; Zyklen Regelung mit maximalem PWM fortgesetzt ; RGLSTK adw adc00 ; ADFM=0, VCFG=0, A/D-Konversion, Kanal AN2 y_sub_x ADRESH,SHIB,w ; Regelabweichungsberechnung ( PWM=SHIB-Servopos.) movwf PWM movlw H'01' ; GIOH vorwärts btfsc STATUS,c goto $+4 ; wenn Vorzeichen negativ neg PWM ; ---> PWM rückwärts c_to_x H'02',GIOH ; GIOH rückwärts min_x_c PWM,max_F ; positiv : BEGRENZUNG von PWM auf max_F x.ge.c PWM,f_on ; Definition der Fahrsignal-ON-Schwelle goto $+H'03' ; ---> Motordrehzahlberechnung clrf PWM retlw H'00' call SKNLI ; Korrektur mit nichtlinearer Kennlinie movwf PWM sublw imax btfss STATUS,z goto $+H'04' c_to_x imax,PWMo retlw H'01' abs.l.e PWM,PWMo,pwmeps ; wenn abs(PWM-PWMo) > pwmeps goto $+H'03' c_to_x rglstop,NPWMIN ; ---> NPWMIN zurücksetzen y_to_x PWM,PWMo decfsz NPWMIN,f ; wenn NPWMIN - 1 > 0 retlw H'01' ; ---> normale Regelung incf NPWMIN,f c_to_x imax,PWM ; sonst ---> Abbruch der Regelung retlw H'01' ; mit maximalem Strom ; ; ;------------------------------------------------------------------------------ ; ; subroutine << SWITCH >> Wertet die beiden Schaltereingänge (GPIO,3 und ; GPIO,5) unter Beiziehung des Methodenselektors ; aus. ; Sind beide Schalter aus, wird der Prozessor ; in Sleep-mode versetzt. ; SWITCH clrf I movlw method andlw H'10' ; Abfrage des Signallogikparamters "L" btfss STATUS,z ; wenn L = 1 goto $+H'08' ; ---> negative Signallogik SW_Lpos btfsc GPIO,5 ; wenn Schalter -1- gesetzt : bsf I,0 ; ---> I,0 setzen btfss MTHS,2 ; nur bei MTHS,2 = 1 goto SW_1 btfsc GPIO,3 ; wenn Schalter -2- gesetzt : bsf I,1 ; ---> I,1 setzen goto SW_0 SW_Lneg btfss GPIO,5 bsf I,0 btfss MTHS,2 ; nur bei MTHS,2 = 1 goto SW_1 btfss GPIO,3 bsf I,1 SW_0 x.eq.c I,H'03' ; wenn I = 3 (beide Schalter gesetzt) clrf I ; ---> I = 0 SW_1 x.eq.yR I,SS_o ; wenn I # SS_o goto SW_2 movlw thalt ; wenn Schalter = OFF und Direktstop gesetzt x.ne.0 I,f ; ---> SHALT = thalt_d btfsc MTHS,3 ; sonst ---> SHALT = thalt c_to_x thalt_d,SHALT SW_2 x.ne.0 SHALT,f ; Gültigkeitsüberprüfung neu starten decfsz SHALT,f ; wenn SS während SHALT gleich return bcf FLAG,0 y_to_x I,SS ; ---> Speicherung des Neuwerts in SS. btfsc STATUS,z ; wenn Schalter=0 goto uPSLEEP ; ---> Schalter AUS stpstp btfss ; wenn Schalter#0 & step by step Mode return c.eq.n ontime,H'F0' return decfsz TON,f ; ---> Abarbeiten der EINschaltzeit ontime*ontime return c_to_x ontime,TON decfsz TON+1,f return bsf FLAG,2 y_to_x SRVTIM,MOVTIM ; starten der Zeitverzögerung c_to_x cSHI,dSHI ; Definition des Increments btfsc SSS,1 ; Bestimmung der Laufrichtung return ; wenn SSS < 2 d.h. Minimum neg dSHI ; ---> Vorzeichen von dSHI = negativ return ; (SHIB läuft von Maximum nach Minimum) ; ; ------ SCHALTER-AUS und PROZESSORRUHE uPSLEEP decfsz TAUS,f ; 16 Bit TAUS abarbeiten return c_to_x saus,TAUS decfsz TAUS+1,f return clrwdt clrf TMR0 bsf STATUS,rp0 ; Umschaltung des TMR0- Prescalers auf WDT c_to_io H'0F',H'0F',OPTION_REG ; WDT- Periode ~ 2.3 s bcf STATUS,rp0 uPcont clrwdt bcf GPIO,0 bcf GPIO,1 c_to_x H'08',INTCON ; Definition von Interrupt bei Pegelwechsel sleep ; SLEEP; Aufwachen durch WDT nop ; oder Schalter EIN nop btfss STATUS,4 ; wenn WDT-wake up goto uPcont ; ---> weiter schlafen clrf INTCON clrf TMR0 bsf STATUS,rp0 c_to_io H'04',H'0F',OPTION_REG ; Timer-0 starten bcf STATUS,rp0 c_to_x saus,TAUS ; wenn TAUS abgearbeitet movwf TAUS+1 ; ---> Prozessor abschalten goto SWITCH return ; ; ;------------------------------------------------------------------------------ ; ; subroutine << SETVAR >> Bestimmung eines Parameters (Servomittelstellung, ; Ausschlagsamplitude, Slowmove-Laufzeit oder Aus- ; schlagshaltezeit) durch extern justierbaren ; Trimmer nach Vorgabe des Parameters "trimsel". ; trimsel : [ 0 ] = keine Trimmerverwendung ; [ 1 ] = Servomittelstellung ; [ 2 ] = Ausschlagsamplitude ; [ 3 ] = Methoden-abhängig Slowmove- ; Laufzeit oder Ausschlags-Halte- ; zeit. ; Der Einstellwert wird aus einer Spannungsmessung am ; Eingang GPIO,4 (Trimmer zwischen VSS und VDD) er- ; mittelt. ; ** Die Mittelstellung (SVMD) entspricht direkt dem ; Trimmerwert ; ** Die Servoendstellungen (SVMI und SVMX) werden ; mit der Trimmer-einstellbare Ausschlags- ; Amplitude (SVAMP) bestimmt zu : ; SVMI = SVMD - SVAMP/2 ( > servmin ) ; SVMX = SVMD + SVAMP/2 ( < servmax ) ; ** Servolaufdauer bzw. der Triggerhaltezeit . ; -- Für die Servolaufdauer gilt : ; Laufzeit = 1.028 ms * (SRVTIM(/2**slowfak)) ; * (servmax-servmin) ; -- Für die Triggerhaltezeit gilt : ; Haltezeit = 1.028 ms * (SRVTIM/(2**slowfak))^2 ; SETVAR c.eq.0 trimsel ; ===== SETZEN der AUSSCHLAGSPARAMETER ===== goto SET_VAL ; ---> keine AD-Wandlung adw adc01 ; ADFM=0, VCFG=0, A/D-Konversion, Kanal AN3 x.eq.yR ADRESH,TRIMMo ; bei unverändertem ADRESH (TRIMMo = ADRESH) retlw H'00' ; mit gleichen Werten weiterarbeiten c_to_x servmid,SVMD ; Voreinstellungen : Servo-Mittelstellung c_to_x srvspan,SVAMP ; Servo-Ausschlagsamplitude c_to_x szeit,SRVTIM ; Trigger- oder Ausschlagszeit c_to_x szeit,TRIGT+1 c_to_x H'01',PCLATH movlw trimsel addwf PCL,f ; Verteilung als Funktion von "trimsel" goto SET_VAL goto SET_MID goto SET_SPN goto SET_TIM ; ----- SERVO-Mittelstellung (Trimmer) ----- SET_MID y_to_x TRIMMo,SVMD goto SET_VAL ; ----- SERVO-Ausschlagsamplitude ----- SET_SPN bcf STATUS,c rrf TRIMMo,w movwf SVAMP ; SVAMP = Trimmereinstellung/2 goto SET_VAL ; ------ SERVO-Ausschlagsgeschwindigkeit ------ SET_TIM btfsc MTHS,1 goto $+H'04' ; Trigger oder Stellgeschwindigkeit ? y_to_x TRIMMo,TRIGT+1 goto SET_VAL y_to_x TRIMMo,SRVTIM c.eq.0 slowfak ; SRVTIM (Zeitverzögerung für goto $+H'06' ; Ausschlag oder Triggerhaltezeit) movwf I bcf STATUS,c rrf SRVTIM,f ; SRVTIM = SRVTIM / 2**Slowfak decfsz I,f goto $-H'03' max_x_c SRVTIM,H'01' ; SET_VAL y_sub_x SVAMP,SVMD,w movwf SVMI ; SVMI = SVMD - SVAMP max_x_c SVMI,servmin ; begrenzt auf "servmin" y_add_x SVAMP,SVMD,w movwf SVMX ; SVMX = SVMD + SVAMP min_x_c SVMX,servmax ; begrenzt auf "servmax" ; adw adc00 ; ---- Ueberprüfung der Servogrenzen ---- x.lt.y ADRESH,SVMX ; wenn Servoposition > eingestelltes SVMX goto $+H'04' y_to_x SVMX,SHIB ; ---> SHIB = SVMX retlw H'01' ; und Rückregelung auf diesen Wert x.ge.y ADRESH,SVMI ; kleiner eingestelltes SVMI retlw H'00' y_to_x SVMI,SHIB ; ---> SHIB = SVMI retlw H'01' ; und Rückregelung auf diesen Wert ; ; ;------------------------------------------------------------------------------ ; ; subroutine << SRVSOL>> Berechnung des jeweiligen Servo-Sollwert. ; Bei aktiver Schalterstellung wird zunächst mittels ; Auswertmetode MTHS bestimmt, welche Auslenkungsart ; (Trigger, Halbausschlag, Vollausschlag oder step by ; step) vorgesehen ist und dann nach entsprechender ; Methode die Servo- Sollstellung (SHIB) ermittelt, ; wobei die Ausschlaggrenzen, die Triggerzeit sowie ; die Auslenkgeschwindigkeit als Eingabe definiert ; wurden. SRVSOL x.ne.0 SS,f ; wenn beide Schalter AUS goto SRV_ON ; ----- SCHALTER AUS ----- SRV_OFF btfsc MTHS,1 ; wenn Halbseitenmethode, d.h. MTHS,1=0 goto $+H'05' btfss MTHS,3 btfsc MTHS,0 ; wenn [L1a0x] goto SRV_MIT ; ---> Servo in Mittelstellung goto SRV_TRG ; sonst ---> Trigger abarbeiten ; btfsc FLAG,2 ; wenn step by step btfsc MTHS,3 ; oder Sofort-Stop-Flagge gesetzt [b=1] return ; ---> Ende goto SRV_VOLA ; ----- SCHALTER EIN ----- SRV_ON c_to_x H'01',PCLATH btfsc FLAG,0 ; wenn First-ON-Flagge gestrichen goto SRV_DIS bsf FLAG,0 ; ---> Initialisierung movf SS,w btfsc MTHS,2 goto $+H'04' toggl_m RICHT,H'01' ; invertieren von RICHT incf RICHT,w ; wenn MTHS,2=L ( gerade) ---> SSS=RICHT+1 movwf SSS ; ,0=H (ungerade) ---> SSS=SS addlw SVMD ; Bestimmung von SHIL durch indirekte movwf FSR ; Adressierung von SVMD, SVMI und SVMX y_to_x INDF,SHIL ; mit Hilfe der Schalterstellung (0, 1 oder 2) movlw method ; --> Adresse je nach Schalter SVMD bis SVMX andlw H'03' addwf PCL,f ; Methodenwahl nach Vorgabe von MTHS,0&1 goto SRV_IT goto SRV_LIM goto SRV_IVL SRV_IST bcf FLAG,2 ; Methodencode [a11] (Step by Step) c_to_x ontime,TON movwf TON+1 ; ON-time Zähler setzen btfss SSS,1 ; goto $+H'09' c_add_x step+f_on,SHIB,f ; positive Richtung min_x_y SHIB,SVMX ; Begrenzung auf SVMX return c_sub_x step+f_on,SHIB,f ; negative Richtung max_x_y SHIB,SVMI ; Begrenzung auf SVMI return SRV_IVL bsf FLAG,2 ; Methodencode [a10] (Vollausschlag) y_to_x SRVTIM,MOVTIM c_to_x cSHI,dSHI ; ---> starten der Zeitverzögerung btfsc SSS,1 ; Bestimmung der Laufrichtung goto SRV_VOLA ; wenn SSS < 2 d.h. Minimum neg dSHI ; ---> Vorzeichen von dSHI = negativ goto SRV_VOLA ; (SHIB läuft von Maximum nach Minimum) SRV_IT bsf FLAG,1 ; Methodencode [a00] (kurzer Ausschlag) y_to_x SRVTIM,TRIGT SRV_LIM y_to_x SHIL,SHIB return ; SRV_DIS movlw method andlw H'03' addwf PCL,f ; Methodenwahl nach Vorgabe von MTHS,0&1 goto SRV_TRG ; Methodencode [a00] (Trigger- Halbausschlag) return ; " [a01] (beideitiger Halbausschlag) goto SRV_VOLA ; " [a10] (Vollausschlag) btfss FLAG,2 ; " [a11] (Step by step) return goto SRV_VOLA SRV_TRG btfss FLAG,1 goto SRV_MIT decfsz TRIGT,f ; wenn Trigger-Flagge gesetzt return ; Haltezeit des Servo-Ausschlags (16 Bit) y_to_x SRVTIM,TRIGT decfsz TRIGT+1,f return bcf FLAG,1 ; Trigger-Flagge streichen SRV_MIT y_to_x SVMD,SHIB return ; ----- VOLLAUSSCHLAG ---- SRV_VOLA x.eq.y SHIL,SHIB ; wenn SHIL = SHIB return ; ---> fertig decfsz MOVTIM,f ; Zeitverzögerung return ; ---> direkt zum Signal y_to_x SRVTIM,MOVTIM ; Laden der neuen Zeitverzögerung y_add_x dSHI,SHIB,f ; sonst : SHIB = SHIB + dSHI y_sub_x SHIL,SHIB,w ; Ausschlagsbegrenzung movf SHIL,w btfss dSHI,7 ; wenn dSHI : goto $+H'04' btfss STATUS,c ; neg.: SHIB < SHIL (c=1) ---> Ausschlagende goto $+H'03' return btfsc STATUS,c ; pos.: SHIB > SHIL (c=1) ---> Ausschlagende movwf SHIB return ; ;----------------------------------------------------------------------- END