Przygotowanie pod dodanie weryfikacji prędkośći

LeonardoProMicro/AutomatedGearShifter/AutomatedGearShifter.ino

@@ -8,6 +8,9 @@
 
 #define Version "1.14.3"
 ////2DO:
+//algorytm walidacji odczytu predkosci
+//zmiana gear baaru na wskaznik
+
 // menu do zmiany zakresu predkosci biegów
 // menu do zmiany zakresu kątów biegów, obwodu koła, ilosci magnesow
 
@@ -41,9 +44,9 @@ Servo myservo;
 
 //SPEED
 double speed = 0;
-double speed_last = 0;
-double speed_last_2 = 0;
-double speed_last_3 = 0;
+double speed_last = 0.0;
+double speed_last_2 = 0.0;
+double speed_last_3 = 0.0;
 int speedTrend = 0;
 double sigleTimeSpd = 0.0;
 double sleepSpd = 0.0;
@@ -51,6 +54,17 @@ unsigned long millissSpd = millis();
 unsigned long lastMillisSpd = millis();
 unsigned long lastLastMillisSpd = millis();
 unsigned long loopTime = millis();
+//speed validation
+double speed4Gear = 0.0;
+double speed4Gear_1 = 0.0;
+double speed4Gear_2 = 0.0;
+double speed4Gear_3 = 0.0;
+double speed4Geat_estimated = 0.0;
+double speedDiff_1 = 0.0;
+double speedDiff_2 = 0.0;
+double speedDiff_3 = 0.0;
+int avgWeight_2 = 2;
+int avgWeight_3 = 1;
 //GEAR
 int currentGear = 1;
 int previousGear = 1;
@@ -306,6 +320,25 @@ void calcSpeed() {
 }
 
 void calcGear() {
+  //speed validation
+//
+//  speedDiff_2 = speed4Gear_2-speed4Gear_1;
+//  speedDiff_3 = speed4Gear_3-speed4Gear_2;
+//  speed4Gear_1 = speed;
+//  speed4Gear_2 = speed4Gear_1;
+//  speed4Gear_3 = speed4Gear_2;
+//  speed4Geat_estimated = (((speedDiff_3 * avgWeight_3 + avgWeight_2 * avgWeight_2)/(avgWeight_3 + avgWeight_2))+ speed4Gear_1)*1,2;
+//  if (speed <= 10.0 or speed < speed4Geat_estimated) {
+//    speed4Gear = speed4Gear_1;
+//  }
+//
+//
+//poniżej zamienić speed  na speed4Gear
+//speed_last_3 na speed4Gear_2
+
+
+
+  
   if ((speed / speed_last_3) >= 1.1) { // przyspieszenie DO weryfikacji czy nie trzeba zamienic na czas lub zwiększyc wartość
     speedTrend = 1;
   } else if ((speed - speed_last_3) <= -1.0) {