vrijdag 27 april 2012

RPM meter


Ik zocht een praktische – niet te moeilijke – toepassing voor interrupts. Zo kwam ik uit bij een toerenteller, met een motorregelaar erbij zodat het toerental altijd constant blijft.
De meting geschiedt met een IR lichtsluis, wat een vrij storingsvrije weergave van het toerental geeft. De lichtsluis is opgebouwd uit twee nabijheidssensoren, waarbij één permanent IR uitzendt en de andere deze opvangt. Bij onderbreking verandert dan de  I/0 waarde op poort B4. Om de motor te kunnen aansturen wordt er (met een interrupt) om de halve seconde gekeken hoeveel keer in die tijdspanne de lichtsluis onderbroken is. Vervolgens wordt dit getal vermenigvuldigd met 120 en gedeeld door het aantal bladen van de prop – bij de mini PC ventilator in dit geval zeven – wat daarmee een vrij nauwkeurige meting oplevert.
Op het LCD scherm worden ook enkele waardes gegeven: linksboven het actuele toerental, linksonder het aantal keren dat de lichtsluis per seconde onderbroken wordt, rechtsboven het opgegeven toerental en rechtsonder de hoeveelheid "spanning" die de motor krijgt - de waarde voor de pulswijdtemodulatie. Deze heeft een maximum van 1023.
Het aanpassen van het toerental is iets ingewikkelder: eerst wordt er gekeken of de motor te snel of te traag draait. Vervolgens wat het effectieve toerental is bij een te hoge snelheid of het opgeven toerental bij een te lage snelheid – de motor moet ook bij een sterk variërend toerental zo min mogelijk uitvallen!
Indien de motor desondanks toch uitvalt (of bij het opstarten),  dan wordt er direct een korte puls gegeven om de motor meteen te laten starten.
Het laten draaien op 120 toeren gaat, hoewel dit echt het minimum is voor deze motor. De ventilator start dan wel zeer snel op, maar valt nog sneller stil. Indien je dit programma dus toepast op je eigen motor, zal je hoogstwaarschijnlijk enkele getallen moeten veranderen voor een goede werking.
Het wijzigen van het toerental is mogelijk met de drukknoppen SW_W en SW_E, om respectievelijk de snelheid te verlagen en te verhogen. Van 120 tot 1020 toeren wordt er per 60 geteld, daarboven per 120.
Ook wordt het toerental op de acht ledjes weergeven, maar indien je motor meer dan 1940 toeren zal draaien, moet je dit aanpassen om een getrouwe weergave te behouden.
Enkele foto's:
De PC ventilator in werking.


De lichtsluis, met de twee nabijheidssensoren.



Op 102rpm.

Op een wat hoger toerental (720rpm).

































































Het progamma:

#include <dwengoConfig.h>
#include <dwengoBoard.h>
#include <dwengoDelay.h>
#include <dwengoLCD.h>
#include <dwengoMotor.h>

#define TIME 5536         // 5536 = 40ms (tijd voor de interrupt).

#define B4      PORTBbits.RB4   // Poort voor blokgolf.
#define B5      TRISBbits.RB5   // Poort uitgang LED.
                                          // De led is zelf aan te sluiten.

#define factor 7                     // Aantal bladen v.d. propellor.

int M = 0;           // Variabele voor delen interrupt.
int FPS = 0;         // Variabele voor doorbreken lichtsluis per seconde.
int RPM = 0;         // Variabele voor toerental per minuut.
int speedM1 = 0;     // Variabele voor pulswijdtemodulatie motor1.
int speed = 0;       // Variabele voor opgegeven toerental.

// Functie voor weergeven toerental.
void displayRPM() {
  clearLCD();
  RPM = FPS*60/factor;                    // RPM berekenen.
  printStringToLCD("RPM: ",0,0);     // Schrijf: "RPM: ".
  appendIntToLCD(RPM);               // Toeren/minuut weergeven.
  printStringToLCD("FPS: ",1,0);     // Schrijf: "FPS: ".
  appendIntToLCD(FPS);               // Toeren/seconde weergeven.
  FPS = 0;                                // Toerental resetten.
}
// Functie voor uitlezen schakelaars.
void readSwitches() {
  if ((SW_W == PRESSED) && (speed >= 120))          // Met SW_W de snelheid verlagen.
     if (speed == 120) speed = 0;
     else if (speed <= 1000) speed -= 60;
     else speed -= 120;
  else if ((SW_E == PRESSED) && (speed < 3000)) // En met SW_E verhogen.
     if (speed == 0) speed = 120;
     else if (speed <= 1000) speed += 60;
     else speed += 120;

  printStringToLCD("C: ",0,9);       // Schrijf: "C: "
  appendIntToLCD(speed);                  // Opgegeven snelheid weergeven
}
// Functie voor toerental aan te passen.
     // Diverse getallen zijn motortype afhankelijk!
     // Deze zijn zelf naar wens in te stellen.
void adjustRPM() {
  // Indien torental te hoog.
  if ((RPM - speed) > 10) {
     // Bij lage toerentallen.
     if (RPM <= 360) {
       if ((RPM - speed) > 20) speedM1 -= 10;
     }
     // Bij hogere toerentallen.
     else {
       if ((RPM - speed) > 200) speedM1 -= 120;
       if ((RPM - speed) > 60) speedM1 -= 20;
       if ((RPM - speed) > 10) speedM1 -= 5;
    }
  }
  // Indien toerental te laag.
  if ((speed - RPM) > 10) {
     // Bij lage opgegeven snelheid.
     if (speed <= 240) {
       if ((speed - RPM) > 20) speedM1 += 40;
     }
     // Bij hogere opgegeven snelheid.
     else {
       if ((speed - RPM) > 200) speedM1 += 120;
       if ((speed - RPM) > 60) speedM1 += 20;
       if ((speed - RPM) > 10) speedM1 += 5;
    }
  }

  if ((RPM == 0) && (speed > 0)) {   // Heropstart bij stilstaande motor.
     setSpeedMotor1(1023);
     if (speed <= 240)                    // Bij gevraagd toerental <= 240RPM.
       speedM1 = 400;
     else                                 // Anders (gevraagd toerental > 240RPM).
       speedM1 += 400;                    // De snelheid flink verhogen voor opstarten.
     delay_ms(150);                       // Even wachten tot de motor op toeren is.
  }

  if (speedM1 >= 1023)    // Beletten dat speedM1 meer dan 1023 wordt.
     speedM1 = 1023;
  if ((speedM1 <= 0) || (speed == 0))
     speedM1 = 0;         // Beletten dat uitgang M1 negatief voltage geeft.
                               // En dat er bij speed == 0 nog stroom loopt.

  printStringToLCD("V: ",1,9); // Schrijf: "V: " (deel door 200 voor spanning in V).
  appendIntToLCD(speedM1);           // Nieuwe spanning weergeven.
  setSpeedMotor1(speedM1);           // Zet motor op aangepaste snelheid.
}
// Functie om leds te laten branden - max. 1940 RPM.
// Aan te passen i.v.m. max. snelheid motor.
void setLEDS() {
  LEDS = 0;
  if (RPM > 0) LED7 = 1;
  if (RPM >= 240) LED6 = 1;
  if (RPM >= 480) LED5 = 1;
  if (RPM >= 720) LED4 = 1;
  if (RPM >= 960) LED3 = 1;
  if (RPM >= 1200) LED2 = 1;
  if (RPM >= 1440) LED1 = 1;
  if (RPM >= 1700) LED0 = 1;
}
// Interrupt.
#pragma interrupt ISR
void ISR() {
     // Uit te voeren code bij interrupt.
     if(M == 12) {                        // Indien M = 12.
       displayRPM();                      // Start weergaveprogramma.
       readSwitches();                    // Lees de schakelaars uit.
       adjustRPM();                       // Wijzig toerental.
       setLEDS();                         // Geef toerental weer met leds.
       M = 0;                             // Reset M.
       // Zet TIMER1 op 40ms.
      TMR1L = TIME & 0x00FF;         // Lengte timer.
      TMR1H = (TIME & 0xFF00) >> 8;
     }
     else {
       M++;                                    // Indien M < 12, dan M + 1.
       // Zet TIMER1 op 40ms.
      TMR1L = TIME & 0x00FF;         //Lengte timer.
      TMR1H = (TIME & 0xFF00) >> 8;
     }
     // Einde code interrupt.
    PIR1bits.TMR1IF = 0;        // Reenable TIMER1 interrupt.
}

#pragma code high_vector=0x08
void high_vector() {
  _asm
    goto ISR
  _endasm
}
#pragma code

// Programma.
void main(void) {

 initBoard();
 initLCD();
 initMotor();
 backlightOn();

// Programmatie interrupt.
  // Initialise prescaler to 8.
  T1CONbits.T1CKPS0 = 1;
  T1CONbits.T1CKPS1 = 1;

  // Zet TIMER1 op 40ms.
  TMR1L = TIME & 0x00FF;
  TMR1H = (TIME & 0xFF00) >> 8;

  T1CONbits.TMR1ON = 1;

  // TIMER1 inputs toestaan.
  INTCONbits.GIE = 1;
  INTCONbits.PEIE = 1;
  PIE1bits.TMR1IE = 1;

  PIR1bits.TMR1IF = 0;

  // Hoofdprogramma.
  while (1) {
       while(B4 == 0) {   // Zolang lichtsluis open.
          delay_us(100);
          B5 = 0;              // LED uit.
       }
       while(B4 == 1) {   // Zolang lichtsluis dicht.
          delay_us(100);
          B5 = 1;              // LED aan.
       }
       FPS+=2;            // Per blokgolf FPS + 2.
  }
}

Dit is een van mijn langste programma's tot nu toe!

donderdag 29 maart 2012

RC besturing

Het leek mij een leuk idee om mijn robot met mijn RC besturing te kunnen besturen. Het programma is vrij eenvoudig gehouden: eerst wordt er drie seconden gewacht, zodat de ontvanger kan opstarten en de mogelijkheid heeft om verbinding te maken met de zender. Vervolgens worden er twee 'while()' lussen doorlopen: de eerste voor het geval dat er nog een puls "bezig" is (poort B0 op 0), de tweede om te zorgen dat de derde lus, die de meting moet doen, precies op de flank van de pus kan beginnen tellen.
Ook die laatste werkt zeer eenvoudig: zolang dat B0 door de puls naar nul getrokken wordt, wordt per 10us (slechts een honderdduizendste van een seconde!) RC[1] met ééntje verhoogd.
Als vervolgens de puls eindigt en B0 opnieuw '1' wordt, wordt er uit de lus gesprongen, om met een bijna identiek systeem de andere puls op B1 ook te meten.

Vervolgens worden de waardes (lengte van de pulsen) naar het LCD geschreven, en krijgen ook de motoren de nodige instructies: momenteel heb ik het zo geprogrammeerd dat de motoren met twee snelheden kunnen draaien, zodat het ook mogelijk is om een flauwe bocht te maken.
Om de lengte van de pulsen te kennen in us, hoef je enkel de waardes op het LCD te vermenigvuldigen met tien.

De schakeling op het breadboard is vrij eenvoudig: de twee draden van de ontvanger sturen twee NPN transistors aan.  Deze simuleren een drukschakelaar, zodat met de PULL-UP (3,9KOhm) weerstand de Vout aan te sturen is met een digitaal 1/0 signaal. De ontvanger is met de - en + ingeplugd in de uitbreidingsconnector, vervolgens zijn kanaal 7 en 8 via de de lange zwarte en rode draad met het breadboard verbonden.

!!!
Het kan zijn dat je zender niet ingesteld staat om de uiterste waardes te geven. Pas dit aan op je zender of in het progamma. Anders zal de puls minder variëren, bv. i.p.v. van 820us tot 1900us slechts van 1000us tot 1720us.

Nog met dank aan Francis van het Dwengo team voor de hulp bij het schrijven in C en het gratis vervangen van de motorcontroller.



Het programma:

#include <dwengoBoard.h>
#include <dwengoConfig.h>
#include <dwengoDelay.h>
#include <dwengoLCD.h>
#include <dwengoMotor.h>

#define B0          PORTBbits.RB0 //Ingangen doorverwijzen.
#define B1          PORTBbits.RB1

void main(void) {

  int RC[2];        //Variabelen voor tijdsduur pulsen.

  initBoard();
  initLCD();
  initMotor();

  backlightOn();

  printStringToLCD("Initializing...", 0, 0);
  delay_s(3);       //De ontvanger de tijd geven om op te starten.
 

  while (TRUE) {

       //Programma om lengte puls op B0 te meten.
       while (B0 == 0) {          //Indien aan.
       }

       while (B0 == 1) {          //Indien uit.
       }

       while (B0 == 0) {          //Indien aan.
         RC[1]+=1;
         delay_us(10);
       }

       //Programma om lengte puls op B1 te meten.
       while (B1 == 0) {          //Indien aan.
       }

       while (B1 == 1) {          //Indien uit.
       }

       while (B1 == 0) {          //Indien aan.
         RC[2]+=1;
         delay_us(10);
       }

       //Waardes naar LCD schrijven.
       clearLCD();
       printIntToLCD(RC[1], 0, 0);
       printIntToLCD(RC[2], 0, 9);

       //Besturing motor 1.
       if (RC[1] > 166) {
         if (RC[1] > 181) {
             setSpeedMotor1(1023);
         }
         else {
             setSpeedMotor1(600);
         }
       }
       else if ((RC[1] < 167) && (RC[1] > 106)) {
         setSpeedMotor1(0);
       }
       else if (RC[1] < 107) {
         if (RC[1] < 92) {
             setSpeedMotor1(-1023);
         }
         else {
             setSpeedMotor1(-600);
         }
       }

       //Besturing motor 2.
       if (RC[2] > 166) {
         if (RC[2] > 181) {
             setSpeedMotor2(-1023);
         }
         else {
             setSpeedMotor2(-600);
         }
       }
       else if ((RC[2] < 167) && (RC[2] > 106)) {
         setSpeedMotor2(0);
       }
       else if (RC[2] < 107) {
         if (RC[2] < 92) {
             setSpeedMotor2(1023);
         }
         else {
             setSpeedMotor2(600);
         }
       }


       //'RC' resetten op nul.
       RC[1] = 0;  
       RC[2] = 0;

       //Tijdsduur lus (verlaag voor een snellere reactietijd).
       delay_ms(250);

  }   
}

maandag 19 maart 2012

Extra sensors

De antibotsbot werkt ondertuseen zeer goed, maar op smalle plekken rijdt hij soms nog tijdens het achteruitrijden tegen de muur. Om dit te verhelpen, zal de robot twee extra sensors krijgen, namelijk achteraan: zo kan hij precies zien wat de afstand is met de muur.

Bij het robot startpakket van Dwengo had ik nog extra sensoren bijbesteld, deze waren echter nog niet aangesloten. Zodus heb ik ze op een printje gesoldeerd, en na enig schrijen in C ze werkend gekregen: dit was niet zo makkelijk, want de werking van de sensormodule mocht niet gestoord worden.


De Vout - pin van de fototransistor is aangeslotenop A3, de sturing voor de IR LED op E0. Merk op dat de waarde van de sensor door vier gedeeld wordt: de sensors op de module gaan maar tot en met 255, de extra sensor op A3 tot en met 1023. Ook wordt er ter vergelijking nog de sensor OS6 van het sensorbord op regel twee van het LCD weergegeven.

De sensor

Het (eenvoudige) programmaatje:

#include <dwengoBoard.h>
#include <dwengoConfig.h>
#include <dwengoADC.h>
#include <dwengoLCD.h>
#include <dwengoDelay.h>
#include <dwengoSensorModule.h>

#define IRLED PORTE     //Doorverwijzingen.
#define AF 0b00
#define AAN 0b11

void main(void) {

     int AMBIENT;
     int ACTIVE;

    TRISE = 0;     //Definieer poorten E0 en E1 als uitgang.

    initBoard();
     initLCD();
    initADC();
     initSensorModule();
    backlightOn();

    while(TRUE) {

      AMBIENT = readADC(3);

       IRLED = AAN;
      ACTIVE = readADC(3);
       IRLED = AF;

       clearLCD();
       printIntToLCD(((AMBIENT - ACTIVE)/4), 0, 0);
       printIntToLCD(readSensor(OS6, DIFF_MODE), 1, 0);
       //Geef ook waarde extra sensor ter vergelijking.

       delay_ms(250);
    }
}