firmware.ino

// Testing QRE1113 connected to Teensy 3.2.
// V3.3, GND, PIN15
//
// Source: http://elimelecsarduinoprojects.blogspot.com/2013/06/measure-rpms-arduino.html

#include "Arduino.h"

// Using ROS instead of Serial 
#include "ros.h"
#include "ros/time.h"

ros::NodeHandle nh;

int c = 0;

int encoder_pin = 15;  // pulse output from the module

unsigned int rpm;     // rpm reading
volatile int pulses;  // number of pulses
unsigned int total_pulses;

unsigned long timeold;
unsigned long timeold_reset;

// number of pulses per revolution
// based on your encoder disc
unsigned int pulsesperturn = 49;

//Update count
void counter()
{
    pulses++;
}

void setup()
{
    //Triggers on Falling Edge (change from HIGH to LOW)
    pinMode(encoder_pin, INPUT_PULLUP); //INPUT_PULLUP
    attachInterrupt(encoder_pin, counter, CHANGE); // CHANGE, FALLING, RISING

    // Initialize
    pulses = 0;
    rpm = 0;
    timeold = 0;
    total_pulses = 0;

    timeold_reset = 0;
    
    nh.initNode();
    nh.getHardware()->setBaud(912600);

    while (!nh.connected())
    {
        nh.spinOnce();
    }
    nh.loginfo("CONNECTED");
    delay(1);
}

void loop()
{
    if (millis() - timeold >= 1000)
    {
        //Don't process interrupts during calculations
        noInterrupts();

        rpm = (60 * 1000 / pulsesperturn) / (millis() - timeold) * pulses;
        total_pulses = total_pulses + pulses;

        char buffer[300];
        sprintf (buffer, "pulses: %d, RPM %d, total_pulses: %d", pulses, rpm, total_pulses);
        nh.loginfo(buffer);
        
        
        timeold = millis();
        pulses = 0;
        
        c++;

        //Restart the interrupt processing
        interrupts();
    }

    if (millis() - timeold_reset >= 5000)
    {   
        timeold_reset = millis();
        total_pulses = 0;
    }

    //call all the callbacks waiting to be called
    nh.spinOnce();
}