No the code above was a sample attempt at programming, however, we have finally achieved success! (With getting RPM) Using the extra Interrupt on the Arduino Mega, it was much easier.
So the way it works is by attaching the encoder's output (either A or B or the Index) to pin 18, then attaching the interrupt to interrupt 5
which is only triggered when pin 18 goes from low to high.
It then calls the incrementit function that does just that, increments the counter.
Then after 10, or 100 ms the count is tallied.
(Thus far tested to 17Khz or 1900+ RPM)
#include <MsTimer2.h>
#include <LiquidCrystal.h>
LiquidCrystal lcd(52, 50, 48, 46, 44, 42);
volatile int count = 0;
double out = 0.0;
double motorout = 0.0;
double kp = 3.0;
double setpoint = 1450.0;
void setup()
{
lcd.begin(16, 2);
pinMode(12, OUTPUT);
Serial.begin(115200); // start serial communication
attachInterrupt(5, incrementit, RISING); //attaches interrupt to the mega's pin 18 (interrupt 5)
MsTimer2::set(100, flash); // 100ms period
MsTimer2::start();
}
void loop()
{
// out = (1450 - out);
// lcd.print(out);
// delay(500);
// lcd.clear();
}
void incrementit()
{
count++;
}
void flash()
{
lcd.clear();
out = count;
out = (setpoint - out);
lcd.print(out);
motorout = (setpoint+kp*out)*(5735.0/10000.0) - 661;
motorout = constrain(motorout, 0, 255);
Serial.println(motorout,DEC);
analogWrite(12, motorout);
//Serial.println(count,DEC);
count = 0;
}