/*  brushless RPM meter
Readout: Volt, Amp, RPM, KV
 
Setup: instellen aantal polen
       instellen interne meting via ESC of extern via draden
 
Tijdens meting knop drukken is weer naar setup
Knop draaien is gas
*/
 
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
 
LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
 
Servo ESC; // servo object voor ESC
 
// define custom charaters
byte Gas0[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x04};
byte Gas1[8] = {0x00,0x00,0x00,0x00,0x00,0x0F,0x1F};
byte Gas2[8] = {0x00,0x00,0x00,0x00,0x0F,0x0F,0x1F};
byte Gas3[8] = {0x00,0x00,0x00,0x07,0x0F,0x0F,0x1F};
byte Gas4[8] = {0x00,0x00,0x07,0x07,0x0F,0x0F,0x1F};
byte Gas5[8] = {0x00,0x03,0x07,0x07,0x0F,0x0F,0x1F};
byte Gas6[8] = {0x03,0x03,0x07,0x07,0x0F,0x0F,0x1F};
 
String Pole;
String Volt;
String Amp;
String KV;
String RPM;
String KVad;
String RPMad;
 
const int ESCpin=10;     // ESC out signaal
const int PinS1=4;       // encoder signaal
const int PinS2=3;       // encoder signaal (VAST INTERRUPT)
const int PinRPM=2;      // RPM sensor signaal (VAST INTERRUPT)
const int PinSW=5;       // drukschakelaar encoder
const int BeepPin=8;     // plus beeper
const int RelaisPin1=A0; // relaicoil 1 plus
const int RelaisPin2=A1; // relaiscoil 2 plus
const int AmpPin=A2;     // AMP sensor signaal
const int VoltPin=A3;    // Volt read pin (midden weestandbrug)
//        A4             // I2C pin SDA (VAST) 
//        A5             // I2C pin SCL (VAST)
 
int P=6;
double V;
double V_oud;
double A;
double A_oud;
int Kv;
int G;
int RpmLengte;
int KvLengte;
int ESCval=1000;
 
volatile int Teller;
volatile long Rpm;
volatile long RPMcount;
 
bool RPMinput;
bool Relais1;
bool Relais2;
 
unsigned long LaatsteDraai=0;
 
void setup() {
 
  lcd.init();                 // initialize the lcd 
  lcd.backlight();            // backlight aan
  Serial.begin(115200);
 
  pinMode(PinS2, INPUT);
  attachInterrupt(digitalPinToInterrupt(PinS2), EncoderDraai, LOW);
  pinMode(PinRPM, INPUT);
  attachInterrupt(digitalPinToInterrupt(PinRPM), InterruptFired, CHANGE);
 
  lcd.createChar(0, Gas0);    // charakters gas instellen
  lcd.createChar(1, Gas1);
  lcd.createChar(2, Gas2);
  lcd.createChar(3, Gas3);
  lcd.createChar(4, Gas4);
  lcd.createChar(5, Gas5);
  lcd.createChar(6, Gas6);
 
  pinMode(PinS1, INPUT);
  pinMode(PinSW, INPUT_PULLUP);
  pinMode(BeepPin, OUTPUT);
  pinMode(RelaisPin1, OUTPUT);
  pinMode(RelaisPin2, OUTPUT);
  pinMode(AmpPin, INPUT);
  pinMode(VoltPin, INPUT);
 
  ESC.writeMicroseconds(ESCval);
  ESC.attach(ESCpin);
 
  V_oud=analogRead(VoltPin); // init oud voltage
  Beep();                    // aan signaal
  manual_input_setup();      // manual setup draaien
}
 
void loop() {
 
// -------- drukken switch -----------
  if (!digitalRead(PinSW)) {      // als gedrukt
    Beep();                       // signaal
    ESC.writeMicroseconds(1000);  // ESC uit
    while (!digitalRead(PinSW)) { // wachten op loslaten
    delay(10);
    }  
  manual_input_setup();           // manual setup draaien
  }
 
// Volt
  V=analogRead(VoltPin); // volt lezen 
  V=(V*0.3)+(V_oud*0.7); // comp filter
  V_oud=V;               // reset oud
  V=map(V, 0, 666, 0, 11200); // map naar voltage
  V/=1000;               // werkelijke voltage
  if(V<0) V=0.0;         // min uitsluiten
  Volt=String(V,1);      // Volt waarde naar string
  if(Volt.length()<4) {  // als string te kort
    Volt=" "+Volt;       // spatie toevoegen
    lcd.setCursor(4,0);  // cursor plaatsen
    lcd.print(" ");      // oud char met spatie overschrijven
  }
  lcd.setCursor(4,0);    // cursor plaatsen
  lcd.print(Volt);       // string printen
 
// Amp
  A=((analogRead(AmpPin)-526.0)/100.0); // lees ampere
  if(A<0) A=0,0;         // min uitsluiten
  if(G==0) {             // bij gas nul, versneld naar nul
    A=0;                 // amp nul
    A_oud=0;             // amp oud nul
  }
  A=(A*0.1)+(A_oud*0.9); // comp filter
  A_oud=A;               // reset oud
  A*=2.84;               // omzetten meetwaarde naar amp                      
  Amp=String(A,2);       // Amp waarde naar string
  lcd.setCursor(11,0);   // cursor plaatsen
  lcd.print(Amp);        // string printen
 
// RPM
  ReadRpm();             // lees rpm
  RPM=String(Rpm);       // naar string
  RpmLengte=RPM.length();// lengte string
  RPMad="";              // opvul bepalen
  if(RpmLengte==1) {RPMad="    ";// opvul 4 spaties
  } else 
  if(RpmLengte==2) {RPMad="   "; // opvul 3 spaties
  } else
  if(RpmLengte==3) {RPMad="  ";  // opvul 2 spaties
  } else
  if(RpmLengte==4) {RPMad=" ";   // opvul 1 spatie
  }
  RPM=RPMad+RPM;         // string samenstellen
  lcd.setCursor(10,1);   // cursor plaatsen
  lcd.print("     ");    // wissen
  lcd.setCursor(10,1);   // cursor plaatsen
  lcd.print(RPM);        // string printen
 
// KV
  Kv=Rpm/V;              // kv berekenen
  KV=String(Kv);         // naar string
  KvLengte=KV.length();  // lengte string
  if(KvLengte==1) {KVad="   ";// opvul 3 spaties
  } else 
  if(KvLengte==2) {KVad="  "; // opvul 2 spaties
  } else 
  if(KvLengte==3) {KVad=" ";  // opvul 1 spaties
  } else KVad="";             // opvul 0 spaties
  KV=KVad+KV;            // string samenstellen
  lcd.setCursor(3,1);    // cursor plaatsen
  lcd.print("    ");
  lcd.setCursor(3,1);    // cursor plaatsen
  lcd.print(KV);         // string printen
 
// Gas 
  G=G+Teller;            // gas bepalen
  Teller=0;              // teller reset
  G=constrain(G, 0, 7);  // gas begrenzen
  lcd.setCursor(0,1);    // cursor plaatsen
  if(G==0) lcd.write(0); // bepalen teken graph bar
  if(G==1) lcd.print("_");
  if(G==2) lcd.write(1);
  if(G==3) lcd.write(2);
  if(G==4) lcd.write(3);
  if(G==5) lcd.write(4);
  if(G==6) lcd.write(5);
  if(G==7) lcd.write(6);
  ESCval=1000+(G*145);   // puls esc bepalen
  ESC.writeMicroseconds(ESCval); // puls schrijven
 
delay(100);              // delay voor rustigere uitlees
 
}  // >>>>>>>>>>>>>>> END LOOP <<<<<<<<<<<<<<<<<<
 
  // >>>>>>>>>>>>>> RUBROUTINES <<<<<<<<<<<<<<<<
 
void EncoderDraai(){            // bij bewegen encoder
  if (millis()-LaatsteDraai>5){ // als minimaal 5ms verstreken
    Beep();                     // beep
    if (digitalRead(PinS1)){    // als draairichting CW
      Teller--;                 // teller bijwerken           
      } else                    // als draairichting CCW
      {
      Teller++;                 // teller bijwerken
      }
    LaatsteDraai=millis();      // tijd reset
  }
}
 
void manual_input_setup() {
  // initialiseren
  lcd.clear();        
  lcd.setCursor(8,0);
  lcd.print("P");
  lcd.setCursor(2,1);
  lcd.print("Pole nr setup");
  // instellen aantal polen
  while(digitalRead(PinSW)) { // terwijl knop niet gedrukt instellen waarde
    P=P+Teller*2;             // pool waarde veranderen
    Teller=0;                 // teller reset
    P=constrain(P, 2, 30);    // polen begrenzen
    Pole=String(P);           // Pole waarde naar string
    if(Pole.length()<2) {     // als string te kort
      Pole=" "+Pole;          // spatie toevoegen
      lcd.setCursor(5,0);     // cursor plaatsen
      lcd.print(" ");         // oud char met spatie overschrijven
    }
  lcd.setCursor(5,0);         // cursor plaatsen
  lcd.print(Pole);            // string printen
  }
  Beep();
  while(digitalRead(PinSW)==LOW) {} // wacht knop loslaten
  // initialiseren
  lcd.clear();
  lcd.setCursor(0,0);
  // ingestelde waarde weergeven
  if(RPMinput){                // extern geschakeld
      lcd.print(" EXTERN CONNECT "); 
    } else {                   // bij intern geschakeld
      lcd.print("   INTERN ESC   ");
      }
  lcd.setCursor(3,1);
  lcd.print("Input setup");
  while(digitalRead(PinSW)) { // terwijl kop niet gedrukt
    if(Teller!=0) {           // bij draaien
    Teller=0;                 // reset teller
    RPMinput=!RPMinput;       // wissel input
    lcd.setCursor(0,0);
    if(RPMinput){             // betreffende input weergeven
      lcd.print(" EXTERN CONNECT ");
      digitalWrite(RelaisPin1, HIGH);
      delay(5);
      digitalWrite(RelaisPin1, LOW);
    } else {
      lcd.print("   INTERN ESC   ");
      digitalWrite(RelaisPin2, HIGH);
      delay(5);
      digitalWrite(RelaisPin2, LOW);
      }
    }
  }
  Beep();
  while(digitalRead(PinSW)==LOW) {} // wacht knop loslaten 
  // initialiseer vast display
  lcd.setCursor(0,0);
  lcd.print("  P     V      A");
  lcd.setCursor(0,1);
  lcd.write(0);
  lcd.print("G     KV      R");
  lcd.setCursor(0,0);  // cursor plaatsen
  Pole=String(P);
  lcd.print(Pole);     // string printen
  G=0; // reset gas op nul
  // terug naar loop
}
 
void Beep() {          // beep laten klinken
  digitalWrite(BeepPin, HIGH);
  delay(5);
  digitalWrite(BeepPin, LOW);
}
 
void InterruptFired()  // interrupt voor rpm read, teller verhogen
{ RPMcount++; }
 
void ReadRpm() {       // rpm read, bereken
  noInterrupts();      // interrupts uit 
  RPMcount=0;          // set variable in critical section
  interrupts();        // interrupts aan
  delay(200);          // wachttijd waarbij stabiel RPM gemeten kan worden
  noInterrupts();      // interrupts weer uit, meetwaarde constant
  Rpm=RPMcount*60/P*5; // bereken RPM     (Hz x 60 x 2) / number of poles = no-load RPM
  interrupts();        // interrupt aan     
 }