/*    ///RAVE CONTROL Servo///
// |**********************************************************************;
* Project           : Einstellbare Auslasssteuerung für die Aprilia RS 125 mit Modellbauservo mit oder ohne RAVE Steuergerät und Solenoid
*                     Auch für andere Motorräder mit Powervalve anwendbar
* 
* Program name      : RAVE_Control_Micros_Solid_rpm.ino
*
* Author            : InterlinkKnight
*
* Date created      : 05/23/2019
*
* Modified by       : el bodo es loco
*
* Revision History  :
*
* Date modifiied    Vers.   Revision Date  
* 06/08/2020         2.0     12/29/2021
*
|**********************************************************************;
*/
#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal.h>
#define CONTROL_PIN 10  // Ausgang zum RAVE Steuergerät (wenn die Originale Auslasssteuerung beibehalten werden soll)
#define LED_PIN 13 // Warnleuchte "RAVE_Aktiv"
LiquidCrystal lcd (12, 11, 6, 5, 4, 2);
int PIN_ADJUST = A1;
// int set; // Ausgeklammert für Feste Öffnungsdrehzahl
int setsmooth;
int Setpoint;
Servo pvservo;

//##### Kalibrierung #####// 
//########################//  Wichtige Parameter sind mit "####" gekennzeichnet


//SERVO Einstellung//
int auf = 90; //#### Servoanschlag in grad, bei geöffnetem Powervalve (bei der RS 125: 90)
int zu = 145;//#### Servoanschlag in grad, bei geschlossenem Powervalve (bei der RS 125: 140)

//Einstellung Drehzahlabnahme//
const byte PulsesPerRevolution = 1;  // #### 1 für Drehzahlmessung am Pickup oder Zündspule; 6 für Drehzahlmessung an der Lima (ROTAX)
// bei der Mito ist der Wert an der Lichtmaschine und der Zündspule 2, es muss ggf. bei anderen Fahrzeugen die Drehzahl am Drehzahlmesser der Fahrzeugs
// und die Drehzahl auf dem LCD Display verglichen werden und der PPR Wert von 1-6 angepasst werden bis die Drehzahlen übereinstimmen


//Timeout, Öffnungsdrehzahl und Genauigkeit//
int set = 8100; //#### Auslass Schieber Öffnungsdrehzahl für eine Feste einstellunng ohne potentiometer
int valvesmooth = 100; // Schwelwert in Rpm, ab dem die ALS wieder schießen soll (verhindert ein wildes auf und zu bei konstanter Drehzahl)
const unsigned long ZeroTimeout = 100000;  
const byte numReadings = 5;  //Anzahl der Messwertblöcke zur Mittelwert bildung Je höher des to genauer aber auch trägerzwischen 3-10 ist in Ordnung
//muss nicht verändert werden 

/////////////
// Variablen:
/////////////

volatile unsigned long LastTimeWeMeasured;  
volatile unsigned long PeriodBetweenPulses = ZeroTimeout+1000;  
volatile unsigned long PeriodAverage = ZeroTimeout+1000;  
unsigned long FrequencyRaw;  
unsigned long FrequencyReal; 
unsigned long RPM; 
unsigned int PulseCounter = 1;  

unsigned long PeriodSum; 
unsigned long LastTimeCycleMeasure = LastTimeWeMeasured;  
unsigned long CurrentMicros = micros(); 
unsigned int AmountOfReadings = 1;
unsigned int ZeroDebouncingExtra;  
unsigned long readings[numReadings];  
unsigned long readIndex;  
unsigned long total;  
unsigned long average;  

void setup() 
{

  Serial.begin(9600); 
  lcd.begin(16, 2);
  Wire.begin(); 
  pvservo.attach(9);
  pvservo.write(auf);
  tone (CONTROL_PIN, 1000);
  delay(1500);
  pvservo.write(zu);
  tone (CONTROL_PIN, 500);
  delay(700);
  attachInterrupt(digitalPinToInterrupt(3), Pulse_Event, RISING);  
  pinMode (3, INPUT_PULLUP);

  delay(1000);  
}  

void loop()
{
LastTimeCycleMeasure = LastTimeWeMeasured;
CurrentMicros = micros(); 
if(CurrentMicros < LastTimeCycleMeasure)
  {
    LastTimeCycleMeasure = CurrentMicros;
  }

  FrequencyRaw = 10000000000 / PeriodAverage;
  if(PeriodBetweenPulses > ZeroTimeout - ZeroDebouncingExtra || CurrentMicros - LastTimeCycleMeasure > ZeroTimeout - ZeroDebouncingExtra)
  {  
    FrequencyRaw = 0;   
    ZeroDebouncingExtra = 2000;
  }
  else
  {
    ZeroDebouncingExtra = 0;  
  }
 FrequencyReal = FrequencyRaw / 10000;  
  RPM = FrequencyRaw / PulsesPerRevolution * 60;  
  RPM = RPM / 10000; 
  total = total - readings[readIndex];  
  readings[readIndex] = RPM;  
  total = total + readings[readIndex];  
  readIndex = readIndex + 1;  

  if (readIndex >= numReadings) 
  {
    readIndex = 0;  
  }
  average = total / numReadings; 
int r = RPM ;
// Setpoint = analogRead (PIN_ADJUST);  // Ausgeklammert für Feste Öffnungsdrehzahl
// set = map(Setpoint, 0, 1024, 6000, 10500 );  // Ausgeklammert für Feste Öffnungsdrehzahl
setsmooth = set - valvesmooth ;
      lcd.setCursor(0, 0);
lcd.clear();
lcd.print("Drehzahl: ");
lcd.print(RPM);   //  zeigt die aktuelle Drehzahl auf dem Display
lcd.setCursor(0, 1);
lcd.print("RAVE: ");
lcd.print(set);
if (RPM <= setsmooth){ // unter diesem Wert bleibt der Auslassschieber geschlossen
        digitalWrite(LED_PIN,LOW);
        tone (CONTROL_PIN, 500); 
        pvservo.write(zu);
      
      }
    if (RPM >= set){ // über diesem Wert öffnet der Auslassschieber
        digitalWrite(LED_PIN,HIGH);
        tone (CONTROL_PIN, 1000); 
         pvservo.write(auf);
        
      }

Serial.print("rpm.val=");
      Serial.println(r);
      Serial.print("set=");
      Serial.println(set);
     
}  
void Pulse_Event()  
{

  PeriodBetweenPulses = micros() - LastTimeWeMeasured;  

  LastTimeWeMeasured = micros();  

 if(PulseCounter >= AmountOfReadings)  
  {
    PeriodAverage = PeriodSum / AmountOfReadings; 
    PulseCounter = 1;  
    PeriodSum = PeriodBetweenPulses; 
    int RemapedAmountOfReadings = map(PeriodBetweenPulses, 40000, 5000, 1, 10);  
    
    RemapedAmountOfReadings = constrain(RemapedAmountOfReadings, 1, 10);
    AmountOfReadings = RemapedAmountOfReadings;  
  }
  else
  {
    PulseCounter++; 
    PeriodSum = PeriodSum + PeriodBetweenPulses; 
  }

}