Beginners mini-cursus (deel 8)

Mini-cursus deel 8 gaat over het aansturen van servomotoren.
Voor PIC Basic programmeurs zonder bezit van een servomotor is het toch verstandig onderstaande eens door te nemen omdat hier ook diverse nieuwe programmeermethoden worden behandeld.
Bovendien is de SERVO instructie ook voor andere doeleinden te gebruiken.
Nieuwe servomotoren zijn al te koop voor 6 Euro.


SERVO

Servomotoren aansturen

De syntaxis
 SERVO Pin, Rotatie waarde 

De instructie SERVO genereert een puls op poort Pin, met een pulsbreedte afhankelijk van Rotatie waarde.

Pin - Is een Port . Pin constante die verbonden is met de servomotor (zie schema).
Dit mag elke poort zijn die als uitgang is in te stellen.
Rotatie waarde - Is een 16-bit (0 ... 65535) waarde dat de positie aangeeft van de motor.
Dit hoeft geen vast getal (constante) te zijn, maar mag ook de waarde van een WORD variabele of een berekening zijn (zie zodadelijk voorbeelden).
Rotatie waarde wordt opgegeven in eenheden van microseconden (µs).
Deze is oscillator onafhankelijk en zal dus altijd in eenheden van 1 µSec zijn, wat voor kristal frequentie er ook wordt gebruikt.

 

Servomotoren aansturen met PIC Basic is heel eenvoudig.
De SERVO instructie genereert een puls die bestaat uit eenheden van 1µSec (microseconde).
Om een puls van bijvoorbeeld 1,75mSec op PORTB.0 te geven schrijf je SERVO PORTB.0, 1750
De 1750 staat voor 1750µSec, oftewel 1,75mSec.
 

Servomotor
In de servomotor zelf zit een printplaatje ingebouwd.
De PIC stuurt een signaal naar de ingebouwde print van de servo met bijvoorbeeld de opdracht om de as 45 graden te verdraaien.
De print van de servo stuurt zijn motor aan en de interne potmeter van de servo meet de uitslag van de as.
Zodra deze uitslag 45 graden vanaf de beginpositie is stopt de print van de servo de motor.

Het grote voordeel van de servomotor is dat de PIC zo precies aan de servomotor kan doorgeven in welke stand deze zijn as moet zetten.

(Voor de duidelijkheid: een servomotor is geen stappenmotor).

Servo's worden gebruikt om verschillende bewegingen in de modelbouw of robotica te controleren.
In de robotica om bijvoorbeeld de beweging van een been of arm te veroorzaken, in de modelbouw om te kunnen sturen (roer of wielen) of gas te geven (brandstofmotoren).

(Mono uitvoering)

Ikzelf gebruik het om een potmeter aan te drijven voor de volumeregeling van de luidsprekers in de badkamer.
Dit is een groot formaat stereo potmeter die rechtstreeks tussen versterker en luidsprekers zit.
De PIC weet zo altijd in welke stand de grote potmeter zich bevind.
Zo kan als de PIC wordt opgestart eerst het volume naar een vooraf ingestelde volumesterkte worden gebracht en dan pas worden ingeschakeld zodat de luidsprekers niet uit het plafond knallen omdat het volume nog op loeihard stond van de vorige keer.
 

Een ander project waar een servomotor in zit is de beamerkast om de spiegel te openen en te sluiten.


De spiegel wordt geopend en gesloten door een servo.

Meer info hierover


Een servomotor vergelijkt constant zijn opgegeven positie (in verhouding met de pulsbreedte) met de werkelijke positie (in verhouding met de weerstand van de potmeter, die intern mechanisch verbonden is met zijn as).
Als er meer dan iets verschil zit tussen deze twee, zal de interne print van de servo onmiddellijk ingrijpen en proberen het verschil op te heffen door de motor in de opgegeven stand te zetten.
Een signaal met een andere pulsbreedte zal de as dus doen verdraaien.
Maar ook als mechanische krachten van buiten af proberen de as weg te draaien van zijn opgegeven positie, zal de servomotor dit proberen tegen te houden.

De meeste servo's kunnen hun as maximaal een halve slag (= 180°) verdraaien.
Er zijn echter ook bijzondere varianten.
De zogenaamde zeilservo's 20 91 50 en 20 91 54 van Conrad bijvoorbeeld kunnen meerdere omwentelingen maken.
Bij elke 500µSec langer draait de as van deze servomotor een hele omwenteling rechtsom.
Een kwart omwenteling naar links wordt dus bereikt door de pulstijd met 125 µSec (= ¼ van 500) te verkorten.
(Erg nauwkeurig vond ik dit type motor tijdens de test overigens niet, maar heb er niet lang mee geëxperimenteerd).

Wanneer een servo geen voedingsspanning heeft of geen positioneringsignaal ontvangt, is zijn as makkelijk met de hand te verdraaien.
Maar als er voedingsspanning op staat en positioneringsignalen ontvangt, zal het niet wijken van zijn positie.

Dit alles doet dus de print in de servo zelf en hoeven wij ons daar tenminste niet druk over te maken.
Wij hoeven alleen maar aan deze interne print op te geven in welke stand de motor-as moet komen te staan.

WHILE 1 = 1              ;Oneindige lus
  SERVO PORTA.6, 1500    ;Puls 1,5mSec, komt (meestal) overeen met de motor-as centreren 
  DELAYMS 20             ;Zorgt ervoor dat 50x per seconde het signaal verzonden wordt
WEND                     ;Terug naar WHILE

De DELAYMS 20 zorgt ervoor dat het signaal met de standaard frequentie van 50Hz wordt verzonden.
Dit kan echter bij sommige servomotoren langer of korter zijn, dan even de beschrijving van de servomotor doorlezen of experimenteren.
Erg kritisch is deze tijd niet.
 

Aan een servomotor zitten 3 draden, een - draad, een + draad en een signaaldraad die van de PIC komt.
De print in de servo heeft een constante spanning nodig, meestal 4,8 ... 6V (5V kan dus ook) die minstens één ampère piekstroom kan leveren.
Het 5V positioneringssignaal (de derde draad) moet meestal een pulsbreedte hebben van tussen de 1mSec en 2mSec lang, met een frequentie van ongeveer 50Hz.
Met 50Hz betekent het dat er dus elke 20mSec een pulsje van 1 ... 2mSec komt.

De pulsbreedte bepaalt nu in welke stand de servomotor zijn as zet.
Omdat er heel veel verschillende soorten servomotoren zijn kunnen er modellen zijn die van het standaard protocol afwijken, maar over het algemeen zet de servo zijn as in het midden als het een signaal ontvangt met pulsjes van 1,5mSec (= 1500µSec).

Om de as dus in het midden te zetten door aansturing met PIC Basic geef je op: SERVO PORTA.0, 1500.
Moet de as een hoek van 90° naar links draaien, dan moet de pulsbreedte 1,25mSec zijn en geef je dus het commando SERVO PORTA.0, 1250 en om een hoek van 90° naar rechts te maken dan geef je het commando SERVO PORTA.0, 1750, wat neer komt op een puls van 1,75mSec.
Het mag duidelijk zijn dat alle tussenliggende waarden overeenkomen met een hoek van de motor-as.

Als de motor-as bij 1,5mSec niet precies in het midden staat kan dat komen door toleranties in de servomotor zelf, maar vooral door afwijking van de interne oscillator van de PIC.
De interne oscillator van een PIC is makkelijk, maar niet erg nauwkeurig.
Een opgegeven waarde in de SERVO instructie van bijvoorbeeld 1500 kan in de ene PIC afwijken naar bijvoorbeeld 1,6mSec, terwijl een andere PIC dit in 1,4mSec doet.
Hierdoor kan hetzelfde Basic programma met dezelfde motor door de ene PIC de motor-as onder een (iets) andere hoek zetten dan door een andere PIC.
Voor één project is dat niet zo erg.
Je past gewoon het programma zo aan dat bijvoorbeeld waarde 1400 de neutraalstand is.
Maak je nu een serieproduct, dan zou je steeds bij elke PIC het programma moeten ijken.
Dan kun je dus beter de PIC op een kristal laten draaien, waardoor SERVO 1500 inderdaad overeenkomt met 1,5mSec (= 1500µSec).
 


Conrad verkoopt al servo's vanaf zo'n 6,49 Euro (Conrad bestelnr's. 23 37 51 (normaal) en 23 05 00 (mini uitvoering)).
Alle instellingswaarden in onderstaande voorbeelden van de instructie SERVO zijn gebaseerd op zo'n goedkope servomotor.
Hoewel standaard 0° moet worden bereikt bij 1250µSec, heeft deze servo het bij mij op 700µSec.
De maximale uitslag (180°) is standaard 1750µSec, bij deze servo is dat op 2300µSec.
Maar de PIC Basic cursus zou net zo goed moeten werken bij andere (duurdere) servomodellen, alleen moet je dan zelf de waarden aanpassen als dat nodig mocht zijn (experimenteren!).


Sluit eerst de componenten aan volgens het schema.
Servomotoren veroorzaken behoorlijke stoorsignalen op de voedingsspanning door de pieken in de stroomopname.
In de voorbeelden wordt de servomotor gevoed uit dezelfde voeding als van de PIC, maar het is natuurlijk veel beter om de servo een eigen voeding of 5V stabilisator (7805) te geven.
Een grote buffer elco wordt hier gebruikt voor de totale voeding, de PIC krijgt daarna nog een eigen elco van 47µF.

De 100n ontkoppelcondensator zoals gebruikelijk zo dicht mogelijk bij de PIC plaatsen.
Een Schottky diode (zoals de BAT85) zorgt ervoor dat de servo geen stroom van de 47µF elco snoept en de PIC van slag zou raken:


Het signaal voor de servo komt uit PORTA.0 (rood aangegeven).
Neem voor de buffer elco een waarde van minimaal 1000µF en 10V (of hogere spanning).
Om storingen van de servomotor op de PIC te voorkomen moet er een Schottky diode in de voeding van de PIC komen.
Dit voorkomt dat de servomotor stroom snoept van de 47µF elco.

 

Het eerste programma laat de servomotor, aangesloten op PORTA.0, langzaam heen en weer bewegen:

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Variabele declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle 8 bits van poortregister PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
WHILE 1 = 1                   ;Oneindige lus
  FOR Positie = 1000 TO 2000 STEP 20  ;Omhoog tellen met stapjes van 20uSec
    SERVO PORTA.0, Positie    ;Stuur 'Positie' pulsjes naar PORTA.0
    DELAYMS 20                ;20mSec = 50Hz, eventueel wijzigen bij ander type motor 
  NEXT

  FOR Positie = 2000 TO 1000 STEP -20 ;Terug tellen met stapjes 20uSec
    SERVO PORTA.0, Positie    ;Stuur 'Positie' pulsjes naar PORTA.0
    DELAYMS 20                ;20mSec = 50Hz, eventueel wijzigen bij ander type motor 
  NEXT
WEND                          ;Terug naar WHILE

Weet dat de pulsbreedte groter is dan 255µSec, dus de variabele 'Positie' moet niet als BYTE, maar als WORD worden gedeclareerd.

Een servo moet constant zijn pulsjes krijgen, wil je zeker zijn dat de servomotor (probeert) zijn as op de ingestelde positie te zetten.
De eerste FOR ... NEXT lus begint met 1000µSec en laat de servomotor rechtsom draaien, omdat de lus-teller 'Positie' steeds groter wordt met stapjes van 20, en dus de pulsbreedte steeds met 20µSec wordt vergroot.
De stapgrootte bepaalt hierdoor de snelheid van de servomotor.
Zodra de pulsjes 2000µSec groot zijn, wordt deze FOR ... NEXT lus beëindigd.

In de tweede FOR ... NEXT lus wordt teruggeteld (STEP -20) van 2000µSec naar 1000µSec, waardoor de servomotor linksom zal draaien, omdat de pulsjes met stapjes van 20µSec kleiner worden.


In het volgende voorbeeld reageert de motor direct op de potmeter, die op de PIC is aangesloten (zie schema).
Door op de as van de 25k potmeter een stuurtje te monteren, kun je de servomotor sturen.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL Potmeter     = PORTA.1 ;Hierop zit de potmeter om de servo te "sturen"

;Variabele declareren
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
WHILE 1 = 1                   ;Oneindige lus
  Positie = 700 + (5 * POT Potmeter, 140) ;Meet potmeter, reken om en geef aan 'Positie' 

  SERVO Motor, Positie        ;Stuur 'Positie' pulsjes naar poort 'Motor'
  DELAYMS 10                  ;10mSec, de functie POT neemt namelijk ook 10mSec in 
WEND                          ;Terug naar WHILE

De waarde voor Schaal van de functie POT is hier op 140 gezet.
Vul eventueel de door jouw gemeten waarde in, zie cursus deel 5.

Het meten van de potmeter met POT, de omrekening van de waarde die de functie POT geeft (0 ... 255) naar de waarde die de instructie SERVO nodig heeft voor de goedkope servo (700 ... 2300) en de uitkomst aan de variabele 'Positie' geven staat allemaal in één programmaregel.

Je moet er wel rekening mee houden dat als er veel opdrachten in de lus staan die relatief veel tijd innemen, zoals bijvoorbeeld de instructie POT en CLS, dat je dan de DELAYMS tijd korter maakt.
De functie POT neemt minimaal 10mSec tijd in om de condensator die in serie met de potmeter is geschakeld op te laden en tijdens meten weer te ontladen.
Deze tijd moet van de pulspauze tijd worden afgehaald, zodat deze nu nog maar 10mSec hoeft te zijn.
De testopstelling bij me thuis werkte zelfs nog toen ik de hele DELAYMS instructie weghaalde.


In het volgende voorbeeld reageert de motor opnieuw direct op de potmeter die op de PIC is aangesloten.
Maar het programma heeft hier een voorziening dat in een vrij groot bereik in het midden van de potmeter de servomotor in de neutraalstand blijft staan.
Zodoende is de neutraalstand van de servomotor makkelijker te vinden op de potmeter.
Een modelauto bijvoorbeeld zal hierdoor makkelijker te besturen zijn, omdat de stuurwielen op een vrij groot gedeelte van de potmeterslag in het midden blijven staan (gecentreerd).
De bestuurder hoeft zo het stuur (gemonteerd op de potmeter) maar "ongeveer" in het midden te zetten, waardoor het programma ervoor zorgt dat de stuurwielen toch recht staan.

In tegenstelling tot het vorige programma kun je hier bij SYMBOL bovenin het programma, ingeven wat de µSec waarde moet zijn als de motor op 0° staat en wat de µSec waarde moet zijn als de motor op 180° staat.
In het voorbeeld zijn deze ingesteld op 700 en 2300µSec (eventueel aanpassen).
De compiler en de PIC rekenen dan zelf uit wat de neutraalstand is en meer van dat soort berekeningen.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Algemene constanten 
SYMBOL MaxPosInstel = 2300    ;uSec: (meest rechtsom 180o)
SYMBOL MinPosInstel = 700     ;usec: (meest linksom 0o)
SYMBOL PotNeutraal  = 200     ;Waarde bepaalt hoe groot neutrale gedeelte is van potmeter

;Berekende constanten door de compiler
SYMBOL NeutraalHalf = PotNeutraal / 2       ;Bereken de helft van PotNeutraal
SYMBOL MaxPos       = MaxPosInstel + NeutraalHalf
SYMBOL MinPos       = MinPosInstel - NeutraalHalf
SYMBOL Midden       = (MaxPos + MinPos) / 2 ;Waarde in uSec voor neutraalstand servomotor 
SYMBOL Verschil     = MaxPos - MinPos       ;Verschil in uSec tussen minimaal en maximaal

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL Potmeter     = PORTA.1 ;Hierop zit de potmeter om de servo te "sturen"

;Variabelen declareren
;DWORD
DIM DWoord          AS DWORD  ;Uitkomst berekening van potmeter kan boven 65535 uitkomen
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
WHILE 1 = 1                   ;Oneindige lus
  ;Berekeningen door de PIC
  DWoord = Verschil * POT Potmeter, 140
  Positie = MinPos + (Dwoord / 255)

  SELECT CASE Positie         ;Test waarde van uitgerekende potmeterinstelling 'Positie'
    CASE < Midden - NeutraalHalf ;Potmeter lager dan de helft (min halve neutraalstand)
      Positie = Positie + NeutraalHalf

    CASE > Midden + NeutraalHalf ;Potmeter hoger dan de helft (plus halve neutraalstand) 
      Positie = Positie - NeutraalHalf

    CASE ELSE                 ;Overige waarden de servomotor in de neutraalstand zetten
      Positie = Midden
  END SELECT

  SERVO Motor, Positie        ;Stuur 'Positie' pulsjes naar poort 'Motor'
  DELAYMS 10                  ;10mSec, de functie POT neemt namelijk ook 10mSec in
WEND

Er moeten drie waarden worden opgegeven in dit programma:

1.  Het aantal µSec voor de meest linkse stand (0°), in dit programma 700.
2. Het aantal µSec voor de meest rechtse stand (180°), in dit programma 2300.
3. De grootte van het middengebied van de potmeter waar de servo in neutraalstand moet blijven staan.
De verhouding ligt aan de waarden die je bij de eerste 2 punten hebt opgegeven.
In het voorbeeld is 200 opgegeven.
Een modelauto zal makkelijker te besturen zijn, omdat de stuurwielen op een groot gedeelte van de potmeterslag in het midden blijven staan.

 

 
Berekeningen door de compiler
Er worden in dit voorbeeld door de Positron PIC Basic compiler (dus niet door de PIC!) eerst een paar berekeningen gedaan.
Achter SYMBOL kun je namelijk ook berekeningen met getallen en constanten (dus niet van variabelen!) plaatsen zoals hier is gedaan.

Het voordeel hiervan is dat de PIC die berekeningen straks tijdens uitvoeren van het programma niet hoeft te doen, omdat deze dan al door de compiler zijn gedaan.
Bovendien scheelt het snelheid en geheugen van de PIC.
Je kunt alleen getallen en constanten berekenen met de compiler, variabelen gaat natuurlijk niet, omdat nu nog niet bekend is wat de waarde van een variabele straks kan zijn (zoals bijvoorbeeld de stand van de potmeter, die kan elke keer anders zijn).
De compiler kan moeilijk gaan rekenen met iets dat nog niet bekend is.
De uitkomst van een berekening met de compiler is dan ook altijd een constante, vaste waarde.
Berekeningen met variabelen zal niet de compiler, maar de PIC zelf moeten doen.

Er worden hier vijf berekeningen door de compiler gedaan.
Allereerst wordt de helft van de opgegeven neutraalstand berekent door deze simpel door twee te delen.
De uitkomst wordt aan de nieuwe constante met de naam 'NeutraalHalf' gegeven.
In het voorbeeld is 'PotNeutraal' 200, dus 'NeutraalHalf' is 100 (= 200 / 2).

De ene helft van 'PotNeutraal' wordt aan 'MaxPos' toegevoegd, de andere helft wordt van 'MinPos' afgehaald.
Dat is nodig omdat de servomotor in het neutrale gedeelte van de potmeter niet draait en dat zou anders betekenen dat de motor dan niet helemaal op de opgegeven 700 (= 0°) en 2300 (= 180°) zou kunnen komen.
Door de waarde die wordt ingevuld bij 'PotNeutraal' over beide helften te verdelen komen we weer op de juiste waarden uit.

Daarna wordt door de compiler het midden (oftewel de neutraalstand) berekent door 'MinPos' en 'MaxPos' bij elkaar op te tellen en de uitkomst te delen door twee.
In het voorbeeld is dat (600 + 2400) / 2, de uitkomst 1500 wordt in de constante 'Midden' gezet.
De waarde 1500 is nu de neutraalstand van de servomotor.

Tot slot wordt nog de waarde van 'Verschil' uitgerekend.
'Verschil' bevat het verschil tussen potmeter helemaal open en potmeter helemaal dicht (in microseconden) door 'MinPos' van 'MaxPos' af te halen, de uitkomst is hier 2400 - 600 = 1800µSec.

Je moet bij berekeningen door de compiler er wel voor zorgen dat de berekeningen in de juiste volgorde staan.
'NeutraalHalf' moet hoger in de SYMBOL lijst staan dan 'MaxPos' en 'MinPos', omdat deze twee constanten een berekening uitvoeren met deze 'NeutraalHalf'.
Anders heeft 'NeutraalHalf' nog geen waarde gekregen en krijg je een foutmelding als je gaat compileren.
En de compilerberekeningen voor 'Midden' en 'Verschil' moeten weer onder 'MaxPos' en 'MinPos' staan omdat de berekeningen voor 'Midden' en 'Verschil' met deze twee constanten gebeurt.
Hierdoor kan het wel gebeuren dat je de constantenamen voor de uitkomsten van de compilerberekeningen niet goed op alfabetische volgorde kan zetten.
 

Nog even voor de duidelijkheid:

SYMBOL Constante = 1 + 2

DIM    Variabele AS BYTE
Variabele = 4 + 3

PRINT AT 1, 1, DEC Constante
PRINT AT 2, 1, DEC Variabele 

In dit voorbeeldje wordt de eerste berekening (1 + 2) door de compiler gedaan en wordt de uitkomst (3) hier in de constante met de naam 'Constante' gezet.
Als je gaat compileren wordt overal waar de naam 'Constante' in je programma staat hiervoor het getal 3 in de plaats gezet.

De tweede berekening (4 + 3) wordt door de PIC zelf uitgevoerd en wordt pas uitgerekend als de PIC zijn programma afdraait.
De uitkomst (7) wordt door de PIC in de variabele met de naam 'Variabele' gezet.

'Constante' zal hier in de PIC altijd de waarde 3 behouden.
Je kunt 'Constante' bijvoorbeeld niet verhogen met "INC Constante", want dat krijg je een foutmelding tijdens het compileren.


Sluit nu twee pulstoetsen en een rode LED aan op de PIC.


Vergeet de BAT85 Schottky diode niet.
 

Met het volgende programma is met de twee toetsen de servomotor te besturen:

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal

;Algemene constanten 
SYMBOL Snelheid     = 30      ;Snelheid waarmee de motor verdraaid

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL LinksOm      = PORTB.0 ;Toets voor motor linksom
SYMBOL RechtsOm     = PORTB.1 ;Toets voor motor rechtsom

;Variabelen declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie = 1500                ;Eerst de servo centreren

WHILE 1 = 1                   ;Oneindige lus
  IF RechtsOm = LAAG AND Positie < 2300 THEN Positie = Positie + Snelheid
  IF LinksOm  = LAAG AND Positie > 700  THEN Positie = Positie - Snelheid

  SERVO Motor, Positie        ;Stuur 'Positie' pulsjes naar poort 'Motor'
  DELAYMS 20                  ;20mSec = 50Hz, eventueel wijzigen bij ander type motor 
WEND                          ;Terug naar WHILE

Het meeste is bekend.
In het hoofdprogramma krijgt de variabele 'Positie' eerst een waarde van 1500 toegewezen (1500 = 1,5mSec), zodat bij PIC opstart bijvoorbeeld de stuurwielen van een modelauto of het roer van een boot of vliegtuig recht worden gezet.
Een signaal met 1,5mSec pulsen zal de meeste servomotoren in het midden (= neutraalstand, 90°) plaatsen.
Als de servomotor niet precies in het midden staat, moet de waarde 1500 wat verhoogt of verlaagt worden.

Door op toets 'RechtsOm' te drukken wordt de pulsbreedte vergroot waardoor de as van de servomotor rechtsom naar de nieuwe positie zal draaien en met toets 'LinksOm' verklein je de pulsbreedte, waardoor de as linksom naar de nieuw opgegeven positie zal draaien.
De variabele 'Positie' wordt hier met de grootte van de constante 'Snelheid' verandert.
De naam 'Snelheid' is gekozen omdat de stapgrootte de snelheid van de servomotor bepaalt.
Als 'Snelheid' bijvoorbeeld maar 1 is, zal de pulsbreedte maar heel langzaam steeds langer (of korter) worden waardoor de as van de servomotor maar heel langzaam verdraaid.

Er zit echter een klein foutje in bovenstaand programma en wel in dit gedeelte:

  IF RechtsOm = LAAG AND Positie < 2300 THEN Positie = Positie + Snelheid
  IF LinksOm  = LAAG AND Positie > 700  THEN Positie = Positie - Snelheid

De minimale positie staat op 700 en de maximale positie staat op 2300 ingesteld, maar zoals het hierboven staat beschreven is er toch een mogelijkheid dat deze waarden worden overschreden.

De stapgrootte 'Snelheid' is bij SYMBOL op 30 ingesteld.
Nadat 'Positie' (die bij start op 1500 (neutraalstand) is gezet) 26 maal is verhoogt met de stapgrootte 30, is de waarde van 'Positie' 2280 geworden (= 1500 + (26 × 30)).
Kijk wat er gebeurt als je opnieuw op de toets 'RechtsOm' drukt.
Het programma bekijkt of toets 'RechtsOm' laag is (en dat is hij want je drukt op de toets).
én hij bekijkt of 'Positie' kleiner is dan de maximale waarde van 2300 (en dat is ook waar want 'Positie' is op dit moment 2280).
Dus het programma zegt, beide beweringen zijn waar dus tel er bij 'Positie' weer 30 (de waarde van 'Snelheid') op.
En... 2280 + 30 = 2310, de maximale waarde is hierbij tóch overschreden.

En nu is dit nog maar een kleine overschrijding.
Als 'Positie' op een gegeven moment (bij andere startwaarden) 2295 was en de stapgrootte 'Snelheid' bijvoorbeeld 70, dan zou de verhoging ook uitgevoerd worden waardoor 2295 + 70 = 2365, de maximale waarde van 2300 met maarliefst 65 zou zijn overschreden!

Dus moet je zoiets anders aanpakken:

  IF RechtsOm = LAAG THEN     ;Als toets 'RechtsOm' is ingedrukt, dan...
    IF (Positie + Snelheid) < 2300 THEN ;Bekijk of nog met 'Snelheid' kan worden verhoogt 
      Positie = Positie + Snelheid ;Servo positie verhogen met waarde 'Snelheid'
    ELSE                      ;...anders...
      Positie = 2300          ;Positie van servo op maximaal zetten (meest rechtsom)
    ENDIF
  ENDIF

  IF LinksOm  = LAAG THEN     ;Als toets 'LinksOm' is ingedrukt, dan...
    IF (Positie - Snelheid) > 700 THEN ;Bekijk of nog met 'Snelheid' kan worden verlaagt 
      Positie = Positie - Snelheid ;Servo positie verlagen met waarde 'Snelheid'
    ELSE                      ;...anders...
      Positie = 700           ;Positie van servo op minimaal zetten (meest linksom)
    ENDIF
  ENDIF

Als je bovenstaande deel hiervoor in de plaats zet, wordt de waarde niet overschreden.

Eerst maak je een IF ... THEN blok die kijkt of toets 'RechtsOm' is ingedrukt of niet.
Als deze toets niet is ingedrukt, wordt er verder ook helemaal niets gewijzigd.
In dat IF ... THEN blok plaats je opnieuw een (ander) IF ... THEN blok.
Deze telt alvast 'Snelheid' bij 'Positie' op en bekijkt dan of de uitkomst onder de waarde 2300 blijft.
Als blijkt dat dat zo is, dan kan 'Positie' gerust verhoogt worden.
En anders (ELSE) wordt 'Positie' gewoon op het maximum (= 2300) gezet.

Voor de toets 'LinksOm' werkt het in principe net zo, alleen dan omgekeerd.

Het onderstaande programma is aangepast en nu wél goed.
Tevens hebben de maximale- en minimale positie een constantenaam gekregen, namelijk 'MaxPos' en 'MinPos':

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal

;Algemene constanten 
SYMBOL MaxPos       = 2300    ;Maximale uitslag van de servomotor (180o)
SYMBOL MinPos       = 700     ;Minimale uitslag van de servomotor (0o)
SYMBOL Snelheid     = 30      ;Snelheid waarmee de motor verdraaid

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL LinksOm      = PORTB.0 ;Toets voor motor linksom
SYMBOL RechtsOm     = PORTB.1 ;Toets voor motor rechtsom

;Variabelen declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie = 1500                ;Eerst de servo centreren

WHILE 1 = 1                   ;Oneindige lus
  IF RechtsOm = LAAG THEN     ;Als toets 'RechtsOm' is ingedrukt, dan...
    IF (Positie + Snelheid) < MaxPos THEN ;Bekijk of nog met 'Snelheid' kan worden verhoogt 
      Positie = Positie + Snelheid ;Servo positie verhogen met waarde 'Snelheid'
    ELSE                      ;...anders...
      Positie = MaxPos        ;Positie van servo op maximaal zetten (meest rechtsom)
    ENDIF
  ENDIF

  IF LinksOm  = LAAG THEN     ;Als toets 'LinksOm' is ingedrukt, dan...
    IF (Positie - Snelheid) > MinPos THEN ;Bekijk of nog met 'Snelheid' kan worden verlaagt 
      Positie = Positie - Snelheid ;Servo positie verlagen met waarde 'Snelheid'
    ELSE                      ;...anders...
      Positie = MinPos        ;Positie van servo op minimaal zetten (meest linksom)
    ENDIF
  ENDIF

  SERVO Motor, Positie        ;Stuur 'Positie' pulsjes naar poort 'Motor'
  DELAYMS 20                  ;20mSec = 50Hz, eventueel wijzigen bij ander type motor 
WEND                          ;Terug naar WHILE

In het voorbeeld hierna is overigens een andere manier te zien die er voor zorgt dat de minimum en maximum waarden niet worden overschreden.

Omdat de SERVO instructie in een oneindige WHILE ... WEND lus staat, zal het programma constant pulsjes naar de servomotor sturen, of er nu een toets is ingedrukt of niet.
Dit betekent dat de motor constant zijn positie blijft behouden en stroom op zal nemen.
Het is ook mogelijk dat de motor geen stroom opneemt als de motor eenmaal op z'n positie is.


Het volgende programma zal de servomotor verdraaien naar de opgegeven positie en daarna nog zo'n 2 seconden zijn positie vasthouden.
Daarna vallen de pulsjes weg en zal de servomotor geen stroom meer opnemen.
De motor is dan met handkracht te verdraaien.
Ikzelf heb dit gebruikt voor de aandrijving van een grote volumepotmeter.
Als de volumepotmeter eenmaal in de juiste stand staat zal deze toch niet uit zichzelf gaan verdraaien dus kan de servomotor ook inactief worden gemaakt.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL HOOG         = 1       ;Hoog signaal
SYMBOL LAAG         = 0       ;Laag signaal
SYMBOL TRUE         = 1       ;Waar = 1

;Algemene constanten
SYMBOL MaxPos       = 2300    ;Maximale uitslag van de servomotor (180o)
SYMBOL MinPos       = 700     ;Minimale uitslag van de servomotor (0o)
SYMBOL Snelheid     = 20      ;Snelheid waarmee de servomotor verdraaid

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL Volume       = PORTB.0 ;Toets voor volume

;Variabelen declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as
;BYTE
DIM BD1             AS BYTE   ;Byte Dummy 1
;BIT
DIM VolumeUp        AS BIT    ;Is TRUE (= waar) bij volume harder en FALSE bij zachter

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie = MinPos + 200        ;Eerst het volume laag zetten (200 boven het minimum)
FOR BD1 = 1 TO 80             ;80 pulsjes de tijd om dat te doen
  SERVO Motor, Positie        ;Stuur servomotor pulsjes
  DELAYMS 20                  ;20mSec per puls = 50Hz
NEXT

WHILE 1 = 1                   ;Oneindige lus
  WHILE Volume = HOOG : WEND  ;Als volumetoets niet is ingedrukt dan wachten op volumetoets 

  IF VolumeUp = TRUE THEN     ;Als volume harder moet, dan... ('Positie' verhogen)
    Positie = Positie + Snelheid ;Verhogen met 'Snelheid'
  ELSE                        ;...anders... (volume zachter, dus 'Positie' verlagen)
    Positie = Positie - Snelheid ;Verlagen met 'Snelheid'
  ENDIF

  IF Positie > MaxPos THEN Positie = MaxPos ;Als 'Positie' boven maximum uitkomt...
  IF Positie < MinPos THEN Positie = MinPos ;Als 'Positie' onder minimum uitkomt...

  FOR BD1 = 1 TO 100          ;Nog 100 pulsjes geven als volumetoets wordt losgelaten
    SERVO Motor, Positie      ;Stuur 'Positie' pulsjes naar poort 'Motor'
    DELAYMS 20                ;20mSec = 50Hz, eventueel wijzigen bij ander type motor 
    IF Volume = LAAG THEN BREAK ;FOR-NEXT lus meteen verlaten als volumetoets is ingedrukt 
  NEXT
  IF BD1 > 1 THEN VolumeUp = ~VolumeUp ;Volume richting omkeren (Harder <-> Zachter)

WEND                          ;Terug naar WHILE

Hier is een andere manier gebruikt die er voor zorgt dat de 'MinPos' en 'MaxPos' waarden niet worden overschreden.
Als 'Positie' verstelt wordt, wordt meteen daarna gekeken of de minimale of maximale waarde is overschreden, en als dat zo is, dan wordt 'Positie' teruggebracht naar de minimale of maximale waarde.
Pas daarna wordt de instructie SERVO uitgevoerd.

De snelheid van de servomotor wordt beperkt door de constante 'Snelheid'.
Wanneer je deze echter te hoog instelt, kan de servomotor het niet bijhouden.
Met de goedkope servomotor van Conrad kun je tot 'Snelheid = 70' gaan, een grotere waarde zal de servomotor niet sneller laten draaien.

In het hoofdprogramma wordt eerst de volume naar "zacht" gedraaid.
Er worden 80 pulsjes met de waarde van 'MinPos' + 200 naar de servo gestuurd, dit is de stand van het opstartvolume.
Binnen die 80 pulsjes moet de servomotor de volumepotmeter naar "zacht" zien te draaien.
'MinPos' is de volume op 0, maar omdat er 200 bij op worden geteld wordt de volume "zacht" gezet.

De onderste FOR ... NEXT lus stuurt de servo nog 100 pulsjes na, zodra de toets 'Volume' (op PORTB.0) wordt losgelaten.
Dit wordt bereikt door de lus meteen te verlaten (met BREAK) zolang de 'Volume' toets wel is ingedrukt.
Wordt de toets op een gegeven moment losgelaten, dan wordt de lus dus niet verlaten en zal de FOR ... NEXT lus de SERVO instructie nog 100 keer uitvoeren.

Na de NEXT wordt de draairichting alvast omgekeerd voor de volgende toetsindruk, door de bitvariabele 'VolumeUp' van toestand te veranderen (0 wordt 1 of 1 wordt 0).
Lus-teller 'BD1' van de laatste FOR ... NEXT wordt nooit hoger dan 1 zolang de toets 'Volume' is ingedrukt.
De volumerichting mag dan ook pas worden omgedraaid, als 'BD1' groter is dan 1, want anders zou de richting ook steeds worden omgedraaid als de toets is ingedrukt, en dat hoort natuurlijk niet.


Met het volgende programma worden zowel de potmeter als beide toetsen gebruikt.
Eerst wordt de servomotor op 0° gezet.
Bij een druk op S2 zal de servo naar een met de potmeter ingestelde hoek gaan.
Door een druk op S1 draait de servo weer terug naar 0°.

De stand van de servomotor is eenvoudig in te stellen door S2 ingedrukt te houden en dan de potmeter in te stellen.
Als de juiste stand eenmaal is ingesteld, dan zal de servo hier steeds naar toe gaan als op S2 wordt gedrukt.

In dit voorbeeld wordt meteen de absolute positie aan de servo opgegeven, er wordt dus niet langzaam naar toe gewerkt.
Hierdoor zal de servomotor op zijn snelst naar de gevraagde positie gaan.
De snelheid van de servomotor is hier dus niet in te stellen.
Het programma wordt hierdoor wel eenvoudiger.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal

;Algemene constanten
SYMBOL MinPos       = 700     ;Minimale uitslag van de servomotor (0o)

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servomotor uitkomen
SYMBOL Potmeter     = PORTA.1 ;Potmeter bepaalt hoever de servo verdraaid na druk op S2
SYMBOL S1           = PORTB.0 ;S1 zet de servo terug naar beginstand (linksom, 0o)
SYMBOL S2           = PORTB.1 ;S2 zet de servo in de met de potmeter ingestelde positie

;Variabelen declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de servomotor

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie = MinPos              ;Startwaarde, servomotor in beginstand zetten (linksom, 0o)

WHILE 1 = 1                   ;Oneindige lus
  IF S1 = LAAG THEN           ;Als op S1 wordt gedrukt, dan...
    Positie = MinPos          ;Servo in beginstand zetten
  ELSEIF S2 = LAAG THEN       ;...anders, als op S2 wordt gedrukt, dan...
    Positie = MinPos + (6 * POT Potmeter, 140) ;Meet potmeter, reken om, geef aan 'Positie' 
  ENDIF

  SERVO Motor, Positie        ;Stuur 'Positie' uSec pulsjes naar poort 'Motor'
  DELAYMS 20                  ;Om de 20mSec pulsjes = 50Hz
WEND

De Schaal van de functie POT weer aanpassen aan je eigen waarden, in het voorbeeld staat deze op 140.

Het voorbeeld hierboven is aardig eenvoudig maar onnauwkeurig.
'MaxPos' kan hier niet worden opgegeven maar wordt berekent door de waarde van de functie POT met 6 te vermenigvuldigen.
Als de potmeter helemaal open staat dan geeft POT de waarde 255.
Dit wordt dus vermenigvuldigd met 6, waarna 'MinPos' (met waarde 700) er nog bij op wordt geteld.
De uitkomst voor de maximum positie is 700 + (6 × 255) = 2230, terwijl het 2300 mag zijn.
De onnauwkeurigheid zit hem in de vermenigvuldigingsfactor.
De waarde 6 blijft de hoogste uitkomst onder de maximumwaarde 2300 houden, maar zit er met 2230 wel erg ruim onder.
Als je met 7 vermenigvuldigd, kom je 255 hoger te liggen en zal je dus bóven de maximumwaarde uit komen.

Bedenk dat alleen hele getallen mogelijk zijn.
Je kunt dit dus niet oplossen door de POT met 6,275 te vermenigvuldigen, want de compiler rond het naar beneden af, dus wordt het toch weer een 6.
Er is wel een mogelijkheid voor drijvende komma getallen, maar die zijn nog niet behandeld in de cursus en dat vreet bovendien programmageheugen van de PIC.


Onderstaand programma doet hetzelfde als de bovenste maar doet dit veel nauwkeuriger.
De truc is hier om de waarden eerst veel groter te maken (door vermenigvuldiging), dan te gaan berekenen en dan weer terug te brengen (door deling) naar de normale waarden.
Hier is bovenin het programma zowel 'MinPos' voor de minimale positie (= 0°) en 'MaxPos' voor de maximale positie (= 180°) op te geven.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal

;Algemene constanten
SYMBOL MaxPos       = 2300    ;Maximale uitslag van beide servomotoren (180o)
SYMBOL MinPos       = 700     ;Minimale uitslag van beide servomotoren (0o)

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servomotor uitkomen
SYMBOL Potmeter     = PORTA.1 ;Potmeter bepaalt hoever de servo verdraaid na druk op S2
SYMBOL S1           = PORTB.0 ;S1 zet de servo terug naar beginstand (linksom, 0o)
SYMBOL S2           = PORTB.1 ;S2 zet de servo in de met de potmeter ingestelde positie

;Variabelen declareren
;DWORD
DIM DWoord          AS DWORD  ;Uitkomst berekening van potmeter kan boven 65535 uitkomen
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de servomotor

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie = MinPos              ;Startwaarde, servomotor in beginstand zetten (linksom, 0o)

WHILE 1 = 1                   ;Oneindige lus
  IF S1 = LAAG THEN           ;Als op S1 wordt gedrukt, dan...
    Positie = MinPos          ;Servo in beginstand zetten
  ELSEIF S2 = LAAG THEN       ;...anders, als op S2 wordt gedrukt, dan...
    DWoord = (MaxPos - MinPos) * POT Potmeter, 140 ;Schaal waarde 140 eventueel aanpassen 
    Positie = MinPos + (Dwoord / 255) ;Reken de instelling van potmeter om naar uSec
  ENDIF

  SERVO Motor, Positie        ;Stuur 'Positie' uSec pulsjes naar poort 'Motor'
  DELAYMS 20                  ;Om de 20mSec pulsjes = 50Hz
WEND

Zoals gebruikelijk eerst weer eventueel de 'MinPos' en 'MaxPos', alsook de Schaal van de functie POT aanpassen aan je eigen waarden.

In het hoofdprogramma wordt eerst de motor op 0° gezet met Positie = MinPos.

Dan wordt in de WHILE ... WEND lus constant de SERVO instructie uitgevoerd, die de servomotor op de waarde van de WORD variabele 'Positie' houdt.

Zodra er op S2 wordt gedrukt, wordt eerst de potmeter uitgelezen met POT.
In deze regel wordt ook het eerste deel van de berekening gedaan die de gemeten potmeterstand moet omzetten naar een microseconde waarde, die voor de servo geschikt is.
Eerst wordt hiervoor de 'MinPos' waarde van 'MaxPos' afgehaald, zodat bekend is hoeveel microseconden verschil er zit tussen de potmeter op minimum en maximum, in het voorbeeld is dat dus 2300 - 700 = 1600.
Door deze waarde met de gemeten potmeterstand te vermenigvuldigen kun je een hele grote waarde krijgen die zelfs niet in een WORD variabele past.
Ga maar na als de potmeter op maximaal staat, dan geeft de functie POT een waarde van 255.
En 255 × 1600 = 408000, en dat past niet in een WORD want die kan maximaal 65535 bevatten, dus wordt de uitkomst in een DWORD variabele gezet, die we hier maar de naam 'Dwoord' hebben gegeven.

De regel erna doet het tweede deel van de berekening, waarvan de uitkomst een geschikte waarde voor de SERVO instructie is.
Eerst wordt het grote getal in 'Dwoord' door 255 gedeeld en daarna 'MinPos' er weer bijgeteld die in de eerste berekening eraf was gehaald.

Om te kijken of de berekening klopt gaan we uit van de uiterste twee mogelijkheden (potmeter helemaal dicht en helemaal open) en kijken dan of de uitkomst klopt.

Eerst als de potmeter helemaal dicht staat.
De functie POT geeft dan de waarde 1.
In het eerste deel van de berekening MaxPos - MinPos is de uitkomst dus 1600.
Dit getal wordt daarna vermenigvuldigd met de potmeterstand en die is hier als voorbeeld 1, de uitkomst in 'Dwoord' is dan 1600 × 1 = 1600.
In de regel daarna wordt de waarde voor 'Positie' berekent door de waarde in 'Dwoord' door 255 te delen, dus: 1600 ÷ 255 = 6,275.
We werken hier echter met hele getallen dus dit wordt afgerond naar 6.
Tot slot wordt 'MinPos' er weer bij opgeteld en heeft 'Positie' voor de servo dus de uitkomst 700 + 6 = 706µSec als de potmeter helemaal dicht staat.
Bij SERVO wordt de servomotor dus helemaal naar links (0°) gedraaid.
Nou, 706 ligt dicht bij de opgegeven 700, dus dat klopt wel zo'n beetje.

Nu het andere uiterste, de potmeter helemaal open gedraaid, om te kijken of de berekening ook dan nog klopt.
De functie POT geeft nu de waarde 255.
In het eerste deel van de berekening MaxPos - MinPos is de uitkomst nog steeds 1600.
Dit getal wordt weer vermenigvuldigd met de potmeterstand en die is nu als voorbeeld 255, de uitkomst in 'Dwoord' is 1600 × 255 = 408000.
In de regel daarna wordt de waarde voor 'Positie' berekent door deze uitkomst door 255 te delen: 408000 ÷ 255 = 1600.
Tot slot wordt 'MinPos' er weer bij opgeteld en heeft 'Positie' voor de servo dus de uitkomst 700 + 1600 = 2300µSec als de potmeter volledig open staat.
Bij SERVO wordt de servomotor dus helemaal naar rechts (180°) gedraaid.
En 2300 is precies de waarde die we bij 'MaxPos' hebben opgegeven.

Alle andere tussenliggende waarden van de potmeter komen overeen met de stand van de servomotor.
Je kunt nog een extra controle doen met de potmeter halfweg, zodat POT de waarde 127 geeft, reken maar na of het klopt.


Zonder DWORD
Bovenstaand voorbeeld maakt gebruik van de 32-bit DWORD variabele.
De precisie is hoog, maar het vreet programma geheugen.
Onderstaande programma doet opnieuw hetzelfde, maar zorgt ervoor dat de uitkomst van berekeningen nooit boven de 65535 uitkomen, zodat we met maar één WORD variabele ('Positie') werken en de DWORD variabele kan vervallen.
Het programma is er maar iets minder nauwkeurig door geworden.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal

;Algemene constanten
SYMBOL MaxPos       = 2300    ;Maximale uitslag van beide servomotoren (180o)
SYMBOL MinPos       = 700     ;Minimale uitslag van beide servomotoren (0o)

;Berekende constanten door de compiler
SYMBOL Verschil     = MaxPos - MinPos
SYMBOL Factor       = (Verschil / 255) + 1 ;Omdat afronding naar beneden, moet er 1 bij op
SYMBOL VerschilFact = Verschil / Factor

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servomotor uitkomen
SYMBOL Potmeter     = PORTA.1 ;Potmeter bepaalt hoever de servo verdraaid na druk op S2
SYMBOL S1           = PORTB.0 ;S1 zet de servo terug naar beginstand (linksom, 0o)
SYMBOL S2           = PORTB.1 ;S2 zet de servo in de met de potmeter ingestelde positie

;Variabelen declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de servomotor

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie = MinPos              ;Startwaarde, servomotor in beginstand zetten (linksom, 0o)

WHILE 1 = 1                   ;Oneindige lus
  IF S1 = LAAG THEN           ;Als op S1 wordt gedrukt, dan...
    Positie = MinPos          ;Servo in beginstand zetten
  ELSEIF S2 = LAAG THEN       ;...anders, als op S2 wordt gedrukt, dan...
    Positie = MinPos + ((VerschilFact * POT Potmeter, 140) / 255) * Factor) ;Schaal 140 
  ENDIF

  SERVO Motor, Positie        ;Stuur 'Positie' uSec pulsjes naar poort 'Motor'
  DELAYMS 20                  ;Om de 20mSec pulsjes = 50Hz
WEND

De formule achter 'Positie' kan ik zelf ook niet echt meer volgen () nadat ik de berekening heb vereenvoudigd door berekeningen van meerdere regels voor de lol eens in één regel te proppen.
De compiler berekent de vermenigvuldigingsfactor 'Factor' zo groot mogelijk voor de nauwkeurigheid, maar zorgt er wel voor dat de uitkomst van een vermenigvuldiging met dit getal nooit boven de 65535 uit komt.
Hierdoor kunnen we met een WORD variabele werken en is geen DWORD variabele nodig.


Het volgende voorbeeld wordt gebruikt om een theezakje een aantal maal in de thee dippen, en daarna weer uit de thee te halen.

   
Testopstelling: De hefboom op de as is verlengd met een strip gemaakt van een oud stukje printplaat.
Het touwtje van het theezakje wordt bevestigd door deze in een zaagsnede in de hefboom te leggen.

 

Als op toets 1 wordt gedrukt zal het theezakje 10x worden gedipt, als op toets 2 wordt gedrukt dan zal wat vaker in de thee worden gedipt, omdat het theezakje ook al voor het eerste bakkie is gebruikt.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal
SYMBOL OFF          = 0       ;UIT
SYMBOL ON           = 1       ;AAN

;Algemene constanten 
SYMBOL AantalDip_1  = 10      ;BYTE: Aantal dat gedipt moet worden voor een nieuw zakje
SYMBOL AantalDip_2  = 20      ;BYTE: Aantal dat gedipt moet worden voor een gebruikt zakje
SYMBOL BovenDeMok   = 1100    ;uSec: Waarde bepaalt hoogte dat theezakje boven de mok hangt
SYMBOL BoveninMok   = 1650    ;uSec: Waarde bepaalt hoogte dat theezakje bovenin mok zit
SYMBOL OnderinMok   = 1850    ;uSec: Waarde bepaalt hoogte dat theezakje onderin mok zit
SYMBOL PulsPauzeTijd= 20      ;mSec: Om de 20mSec een puls (= 50Hz)
SYMBOL Snelheid     = 5       ;Snelheid waarmee het zakje in de thee dipt (hoger = sneller)

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL LED          = PORTA.2 ;Rode LED, deze brandt als het apparaat in werking is
SYMBOL EersteBakje  = PORTB.0 ;Toets voor een vers bakkie thee (theezakje is nieuw)
SYMBOL TweedeBakje  = PORTB.1 ;Toets voor een tweede bakkie thee (hetzelfde theezakje)

;Variabelen declareren
;WORD
DIM WD1             AS WORD   ;Word Dummy 1
;BYTE
DIM DipAantal       AS BYTE   ;Bevat het gekozen aantal keer dat er gedipt moet worden
DIM DipTeller       AS BYTE   ;Teller die telt hoevaak er is gedipt tijdens thee dippen

;        76543210
PORTA = %00000100             ;Maak alle poorten van PORTA laag behalve PORTA.2 (rode LED)
TRISA = %11111010             ;PORTA.0 is uitgang voor de servomotor en PORTA.2 voor LED

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
WHILE 1 = 1                   ;Oneindige lus
  FOR WD1 = 1 TO 50           ;50 pulsjes de tijd om naar de bovenste stand te gaan
    SERVO Motor, BovenDeMok   ;Zet de hefboom in de stand die is opgegeven in 'BovenDeMok'
    DELAYMS PulsPauzeTijd     ;'PulsPauzeTijd' is bovenin het programma opgegeven
  NEXT

  LED = OFF                   ;Rode LED is uit als apparaat in rust is

  WHILE PORTB & %00000011 = %00000011 : WEND          ;Wacht tot een toets wordt ingedrukt
  DELAYMS 5                                           ;Tijd tegen contactdender van toetsen 
  IF EersteBakje = LAAG THEN DipAantal = AantalDip_1  ;Is het S1, dip dan AantalDip_1 keer
  IF TweedeBakje = LAAG THEN DipAantal = AantalDip_2  ;Is het S2, dip dan AantalDip_2 keer 

  LED = ON                                            ;Rode LED aan, apparaat is bezig

  FOR DipTeller = 1 TO DipAantal                      ;Dip theezakje gekozen aantal keer
    FOR WD1 = BoveninMok TO OnderinMok STEP Snelheid  ;Van bovenin mok tot bodem van mok
      SERVO Motor, WD1                                ;Verdraai motor volgens WD1 waarde
      DELAYMS PulsPauzeTijd
    NEXT
    IF DipTeller = 1 AND DipAantal = AantalDip_1 THEN DELAYMS 10000 ;Wacht bij eerste dip

    FOR WD1 = OnderinMok TO BoveninMok STEP -Snelheid ;Van bodem van mok tot bovenin mok
      SERVO Motor, WD1                                ;Verdraai motor volgens WD1 waarde
      DELAYMS PulsPauzeTijd
    NEXT
  NEXT
WEND                          ;Terug naar WHILE

Dit moet nu toch aardig te volgen zijn, lees anders de REM regels die achter de programmaregels staan.
Denk eraan dat dummy 'WD1' van de FOR ... NEXT teller een WORD variabele is, een BYTE is te klein!

Alle poorten van poortregister A (PORTA), worden laag gemaakt, behalve PORTA.2, die wordt hoog gemaakt zodat de rode LED meteen brandt als met TRISA de poort wordt omgeschakeld als uitgang.
Verderop in het programma, als de servo in positie en het apparaat in rust is, wordt de LED pas uitgezet.

Wanneer het hoofdprogramma begint, wordt eerst de hefboom in de hoogste positie gezet door de eerste FOR ... NEXT lus.
De servo krijgt van het programma 50 pulsen (× 20mSec = 1 seconde) de tijd om dat te doen.
De servomotor ligt op zijn kant, dus staat het armpje hoog boven de theemok.
Een nieuw theezakje wordt bevestigd door het touwtje in de zaagsnede van de hefboom te leggen (zie foto's) en er moet een mok met heet water onder worden geplaatst (misschien eerst droog proberen?).

Een druk op S1 zal de rode LED aanzetten, het theezakje (snel) in de mok doen zakken en daarna eerst 10 seconden wachten zodat het theezakje zich kan vullen met het water (het nieuwe, droge theezakje blijft namelijk eerst drijven).
Dit kun je bepalen door naar 2 dingen te kijken:

1.  Staat de 'DipTeller' op 1? Zo ja, dan is het de eerste dip van een (opgegeven) aantal.
2. Is 'DipAantal' gelijk aan 'AantalDip_1'?   Zo ja, dan is net op S1 gedrukt, de toets voor een nieuw zakje.

Beide beweringen (= AND) moeten waar zijn (= TRUE), alleen dán wordt de 10 seconden wachttijd uitgevoerd.
Als bewering 1 niet waar is (= FALSE), dan is het niet de eerste dip.
Als dit er niet stond dan zou het theezakje bij elke dip 10 seconden wachten.
Als bewering 2 niet waar is, dan is er niet op S1 gedrukt.
Als dit er niet stond dan zou het theezakje ook wachten als op S2 was gedrukt.

Dan wordt het theezakje langzaam (waarde van 'Snelheid') het opgegeven aantal maal (waarde van 'AantalDip_1) in de thee gedipt.
Wanneer het aantal is bereikt wordt het zakje weer (snel) uit de thee gehaald en blijft boven de kom hangen.
Nu wordt de LED weer uitgezet; thee is klaar, apparaat in rust, het wacht weer op een toetsindruk.

Als er van hetzelfde theezakje een tweede mok thee moet worden gezet, dan moet op S2 worden gedrukt.
Het theezakje is nu al nat dus zal er nu geen 10 seconden gewacht worden.
Omdat de thee in het gebruikte zakje niet zo sterk meer is, moet het wat vaker worden gedipt dan de eerste keer, S2 zal dus vaker laten dippen dan wanneer er op S1 is gedrukt.

De waarden van de constanten 'BovenDeMok', 'BoveninMok' en 'OnderinMok' kunnen bij andere servomotoren afwijken, dus die zul je naar eigen omstandigheden misschien moeten aanpassen.
En als je de thee wat meer of minder sterk wilt hebben moet je de constanten 'AantalDip_1' en 'AantalDip_2' even aanpassen naar smaak.
Plakje cake?

Dit voorbeeld van de theezetter is uitgebreid met een aanraaksensor en te bekijken als volledig project op deze website.


Met display
Voor de volgende paar voorbeelden moet het HD44780 display worden aangesloten.


Het HD44780/A00 display is volgens de standaard PIC Basic manier aangesloten.
Denk aan de Schottky diode.

 

Onderstaand voorbeeld laat het aantal microseconden op het display zien, dat aan de SERVO instructie wordt gegeven:

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal

;Algemene constanten 
SYMBOL MaxPos       = 2300    ;Maximale uitslag van de servomotor (180o)
SYMBOL MinPos       = 700     ;Minimale uitslag van de servomotor (0o)
SYMBOL Snelheid     = 30      ;Snelheid waarmee de motor verdraaid

;Berekende constante door de compiler
SYMBOL Midden       = (MaxPos + MinPos) / 2 ;Waarde in uSec voor neutraalstand servomotor 

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servo uitkomen
SYMBOL LinksOm      = PORTB.0 ;Toets voor motor linksom
SYMBOL RechtsOm     = PORTB.1 ;Toets voor motor rechtsom

;Variabelen declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen
DELAYMS 500                   ;LCD stabilisering


;Hoofdprogramma
Positie = Midden              ;Eerst de servo centreren

CLS                           ;Wis displayscherm
PRINT AT 1, 6, 228, "Sec."    ;Plaats alvast tekst

WHILE 1 = 1                   ;Oneindige lus
  IF RechtsOm  = LAAG THEN Positie = Positie + Snelheid ;Servo positie verhogen
  IF Positie > MaxPos THEN Positie = MaxPos             ;Als 'Positie' boven maximum uitkomt

  IF LinksOm   = LAAG THEN Positie = Positie - Snelheid ;Servo positie verlagen 
  IF Positie < MinPos THEN Positie = MinPos             ;Als 'Positie' onder minimum uitkomt

  IF LinksOm   = LAAG AND RechtsOm = LAAG THEN ;Als beide toetsen tegelijk ingedrukt zijn...
    Positie = Midden                           ;In de neutraalstand zetten
    WHILE PORTB & %00000011 < %00000011 : WEND ;Wacht tot beide toetsen zijn losgelaten
  ENDIF

  PRINT AT 1, 1, DEC4 Positie ;Plaats het aantal microseconden op het display

  SERVO Motor, Positie        ;Stuur 'Positie' pulsjes naar poort 'Motor'
  DELAYMS 20                  ;20mSec = 50Hz, eventueel wijzigen bij ander type motor 
WEND                          ;Terug naar WHILE

Met CLS wordt eerst het display gewist.
Daarna wordt op regel 1, kolom 6 eerst het 'µ' teken geplaatst, met meteen daarachter Sec.
Het 'µ' teken heeft ASCII waarde 228, als je tenminste een HD44780/A00 type display hebt.
Het woord µSec. blijft ongewijzigd, dus die hoeft niet in de WHILE ... WEND lus te staan.
Door dit in kolom 6 van het display te zetten, kunnen de microseconden er zodadelijk voor worden geplaatst.

Hier is een andere variant van programmeren, die controleert of 'Positie' niet over de ingestelde grenswaarden 'MinPos' en 'MaxPos' gaat.
De 'Positie' wordt sowieso verhoogt of verlaagt als de toets wordt ingedrukt, maar meteen in de IF ... THEN regel daarna wordt gecontrolleerd of de grens wordt overschreden en zo ja, dan terugbracht naar de grenswaarde.

Door beide toetsen tegelijk in te drukken wordt de servomotor weer in de neutraalstand gezet.
Dit gebeurt overigens pas als beide toetsen weer zijn losgelaten.


In onderstaand voorbeeld kun je de hoek van de as van de servomotor aflezen op het display.
De waarden in het voorbeeld zijn weer gebaseerd op de goedkope servomotor van Conrad.
Voor andere modellen kan dit afwijken en zul je het moeten aanpassen.
Met S1 en S2 is de servo te verdraaien.

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Poortnamen
SYMBOL Motor        = PORTA.0 ;Poort waar de pulsjes voor de servomotor uitkomen
SYMBOL Potmeter     = PORTA.1 ;Hierop zit de potmeter om de servo te "sturen"

;Variabele declareren
;WORD
DIM Positie         AS WORD   ;WORD(!) variabele bevat de positie van de motor-as

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11111110             ;PORTA.0 is uitgang voor de servomotor

CLEAR                         ;Wis alle RAM geheugen
DELAYMS 500                   ;LCD stabilisering


;Hoofdprogramma
CLS
PRINT "Servo:"

WHILE 1 = 1                   ;Oneindige lus
  Positie = POT Potmeter, 140 ;Meet potmeter, geef gemeten waarde aan 'Positie' 

  PRINT AT 1, 8, DEC3 (Positie * 180) / 255, %11011111, "  " ;Zet ASCII 223 erachter

  Positie = 700 + (5 * Positie) ;Reken 'Positie' om en geef waarde terug aan 'Positie' 

  SERVO Motor, Positie        ;Stuur 'Positie' pulsjes naar poort 'Motor'
  DELAYMS 10                  ;10mSec, de functie POT neemt namelijk ook 10mSec in 
WEND                          ;Terug naar WHILE

Als je een HD44780/A00 display hebt dan wordt er een ° tekentje achter de hoekwaarde gezet.
Dit wordt bereikt door de ASCII waarde %11011111 (oftewel 223) op te geven.
Je kunt natuurlijk ook gewoon decimaal 223 schrijven, voor de weergave op het display maakt dat echt geen reet uit niets uit.
Makkelijk is de binaire notatie wel, omdat in de ASCII tabel de karaktercode ook binair is weergegeven.


Twee (of meer) servomotoren
Het is mogelijk om meerdere servomotoren tegelijk te laten werken.
Denk eraan dat de voeding van de servomotoren wordt betrokken vóór de Schottky diode, omdat anders de servomotoren de PIC voeding verstoren en de PIC steeds reset.


Denk aan de Schottky diode.
 

Met onderstaande voorbeeld is servomotor 1 met de potmeter te regelen en servomotor 2 met de 2 toetsen te bedienen:

DEVICE 16F628A                ;Gebruik een 16F628A type
CONFIG INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE              ;Alle ingangen digitaal

;Logische constanten
SYMBOL LAAG         = 0       ;Laag signaal
SYMBOL OFF          = 0       ;UIT
SYMBOL ON           = 1       ;AAN

;Algemene constanten
SYMBOL MaxPos       = 2300    ;Maximale uitslag van beide servomotoren (180o)
SYMBOL MinPos       = 700     ;Minimale uitslag van beide servomotoren (0o)
SYMBOL Snelheid     = 50      ;Snelheid waarmee servomotor 2 verdraaid

;De waarde voor constante 'Midden' wordt uitgerekend door de compiler
SYMBOL Midden       = (MaxPos + MinPos) / 2 ;Bereken neutraalstand voor servomotor 2

;Poortnamen
SYMBOL Motor_1      = PORTA.0 ;Poort waar de pulsjes voor servomotor 1 uitkomen
SYMBOL Potmeter     = PORTA.1 ;Hierop zit de potmeter om de servo te "sturen"
SYMBOL Motor_2      = PORTA.3 ;Poort waar de pulsjes voor servomotor 2 uitkomen
SYMBOL LinksOm      = PORTB.0 ;Toets voor 'Motor_2' linksom
SYMBOL RechtsOm     = PORTB.1 ;Toets voor 'Motor_2' rechtsom

;Variabelen declareren
DIM Positie_1       AS WORD   ;WORD(!) variabele bevat de positie van servomotor 1
DIM Positie_2       AS WORD   ;WORD(!) variabele bevat de positie van servomotor 2

;        76543210
PORTA = %00000000             ;Maak alle poorten van PORTA laag
TRISA = %11110110             ;PORTA.0 en A.3 uitgangen voor beide servomotoren

PORTB_PULLUPS ON              ;On-chip pull-up weerstanden actief (voor de toetsen) 
CLEAR                         ;Wis alle RAM geheugen


;Hoofdprogramma
Positie_2 = Midden            ;Startwaarde, servomotor 2 in het midden zetten

WHILE 1 = 1                   ;Oneindige lus
  Positie_1 = POT Potmeter, 140 ;Geef de gemeten potmeterwaarde aan variabele 'Positie_1'
  Positie_1 = MinPos + (Positie_1 * 5) ;Omrekenen naar microseconden voor SERVO instructie 

  IF RechtsOm = LAAG THEN Positie_2 = Positie_2 + Snelheid ;Servo 2 verhogen met 'Snelheid'
  IF Positie_2 > MaxPos THEN Positie_2 = MaxPos ;Als 'Positie' boven maximum uitkomt...

  IF LinksOm  = LAAG THEN Positie_2 = Positie_2 - Snelheid ;Servo 2 verlagen met 'Snelheid' 
  IF Positie_2 < MinPos THEN Positie_2 = MinPos ;Als 'Positie' onder minimum uitkomt...

  SERVO Motor_1, Positie_1    ;Stuur 'Positie_1' pulsjes naar poort 'Motor_1'
  SERVO Motor_2, Positie_2    ;Stuur 'Positie_2' pulsjes naar poort 'Motor_2'
  DELAYMS 8                   ;Minder dan normaal, POT en de 2e motor nemen overige tijd 
WEND  

Sluit een tweede servomotor aan op PORTA.2.
Zoals eerder gezegd, zijn de waarden van alle voorbeelden gebaseerd op de goedkope servomotoren van Conrad.
Andere modellen kunnen andere waarden hebben en zul je de waarden van 'MaxPos' en 'MinPos' moeten aanpassen.

Servomotor 2 wordt eerst in de neutraalstand gezet.
Bij servomotor 1 hoeft dat niet, want die is meteen afhankelijk van de stand van de potmeter.


De instructie SERVO doet dus niets anders dan een hoog pulsje van een opgegeven tijd in microseconden naar een opgegeven poort verzenden.
Daarom kun je deze instructie natuurlijk ook voor heel andere doeleinden gebruiken, als pulsjes nodig zijn van microseconden (µs) grootte.

 


PIC Basic cursus deel 9 gaat over de interne EEPROM van de PIC.