sieja/Arduino
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Walidacja prędkosci

Browse Source 
zmiana bar grafu na strzałkę i prawo
This commit is contained in:
sieja
2024-11-26 21:20:25 +01:00
parent bed83a7a97
commit 1804bbd793
1 changed files with 52 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
LeonardoProMicro/AutomatedGearShifter/AutomatedGearShifter.ino
View File

@@ -6,10 +6,11 @@
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define Version "1.14.3"
#define Version "1.15.3"
////2DO:
//algorytm walidacji odczytu predkosci
//zmiana gear baaru na wskaznik
//diagnostyka i/lub przeciwdziałanie skokom predkosci
//dlaczego wskaznik odnosi sie do poprawnego biegu a w tym czasie bieg jest zly? bo czas ponizej 2s?
//zmienic system usypiania dodac bezwzglednie czas 3 min, i po czeku z przyciskiem ponowny odczyt magnesu po 0,5sec
// menu do zmiany zakresu predkosci biegów
// menu do zmiany zakresu kątów biegów, obwodu koła, ilosci magnesow
@@ -202,7 +203,9 @@ void loop() {
//################################################
//SPEED
sleepSpd = millis() - lastMillisSpd;
if (sleepSpd >= 2000) { //podaj zerową prędkość jeśli nie było odcztu od 1,5 s
if (sleepSpd >= 1500) { //podaj zerową prędkość jeśli nie było odcztu od 1,5 s
speed4Gear = 0.0;
speed4Gear_2 = 0.0;
speed = 0.0;
speed_last = 0.0;
speed_last_2 = 0.0;
@@ -216,8 +219,8 @@ void loop() {
}
}
if (speed > 99 || isinf(speed)) {
speed = 21.37;
if (speed4Gear > 99 || isinf(speed4Gear)) {
speed4Gear = 21.37;
}
calcGear();
displGear = 9 - currentGear;
@@ -230,7 +233,7 @@ void loop() {
display.setCursor(40, 0);
display.print(currentGear);
//SPEED_TREND
display.setCursor(100, 00);
display.setCursor(75, 00);
if (speedTrend <= -1 ) {
display.write(31);
} else {
@@ -251,14 +254,13 @@ void loop() {
display.fillRect(115, 0, 7, 1, SSD1306_WHITE);
display.fillRect(115, 63, 7, 1, SSD1306_WHITE);
if (speed >= currentGearRangeMiddle) {
gearBarHeight = int(((speedForBar - currentGearRangeMiddle) / (currentGearRangeUpper - currentGearRangeMiddle)) * 32);
gearBarPosition = 32 - gearBarHeight;
display.fillRect(122, gearBarPosition, 4, gearBarHeight, SSD1306_WHITE);
} else {
gearBarHeight = int(((currentGearRangeMiddle - speedForBar) / (currentGearRangeMiddle - currentGearRangeLower)) * 32);
display.fillRect(122, 32, 4, gearBarHeight, SSD1306_WHITE);
}
display.setTextSize(2);
gearBarHeight = 64 - int(((speed4Gear - currentGearRangeLower) / (currentGearRangeUpper - currentGearRangeLower)) * 64) - 5;
display.setCursor(115, gearBarHeight);
display.write(16);
display.setTextSize(3);
//VOLT_BAR
adcBattVoltValue = analogRead(VoltInptPin);
@@ -279,7 +281,7 @@ void loop() {
if (speedTrend == -1 && speed > 0.0 ) {
if (speedTrend == -1 && speed4Gear > 0.0 ) {
if (BrakingLightSwitch == 1) {
digitalWrite(BrakingLight, HIGH);
BrakingLightSwitch = 0;
@@ -307,41 +309,41 @@ void calcSpeed() {
lastMillisSpd = millissSpd;
millissSpd = millis();
sigleTimeSpd = double(millissSpd - lastLastMillisSpd) / 1000;
speed_last_3 = speed_last_2;
speed_last_2 = speed_last;
speed_last = speed;
// speed_last_3 = speed_last_2;
// speed_last_2 = speed_last;
// speed_last = speed;
speed = (((2 * Pi) / sigleTimeSpd * ((WheelCircumference) / (Pi)) * ms2kmh)) / MagnetsCnt;
// diag
if (((speed_last_3 * 1.5) > speed) && speed > 10.0 && speedTrend > 0) { //zabezpieczenie przed losowymi sygnałami magesu
speed = speed_last_3;
}
speed = (speed + speed_last)/2;
// if (((speed_last_3 * 1.5) > speed) && speed > 10.0 && speedTrend > 0) { //zabezpieczenie przed losowymi sygnałami magesu
// speed = speed_last_3;
// }
// speed = (speed + speed_last)/2;
totalDist = totalDist + (1/MagnetsCnt);
}
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;
// }
//
//
speedDiff_3 = speed4Gear_3-speed4Gear_2;
speedDiff_2 = speed4Gear_2-speed4Gear_1;
speed4Gear_1 = speed;
speed4Gear_2 = speed4Gear_1;
speed4Gear_3 = speed4Gear_2;
speed4Geat_estimated = (((speedDiff_3 * avgWeight_3 + speedDiff_2 * avgWeight_2)/(avgWeight_3 + avgWeight_2))+ speed4Gear_1) + 4;
if (speed <= 10.0 or speed <= speed4Geat_estimated) {
speed4Gear_2 = speed4Gear;
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ść
if ((speed4Gear / speed4Gear_2) >= 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) {
} else if ((speed4Gear - speed4Gear_2) <= -0.5) {
speedTrend = -1;
} else {
speedTrend = 0;
@@ -352,35 +354,35 @@ void calcGear() {
// }else {
accelerationShift = 1;
// }
if (speed * accelerationShift >= 0 && speed * accelerationShift < spdRange1and2) {
if (speed4Gear * accelerationShift >= 0 && speed4Gear * accelerationShift < spdRange1and2) {
calculatedGear = 1;
currentGearRangeLower = 2.5;
currentGearRangeUpper = spdRange1and2;
} else if (speed * accelerationShift >= spdRange1and2 && speed * accelerationShift < spdRange2and3) {
} else if (speed4Gear * accelerationShift >= spdRange1and2 && speed4Gear * accelerationShift < spdRange2and3) {
calculatedGear = 2;
currentGearRangeLower = spdRange1and2;
currentGearRangeUpper = spdRange2and3;
} else if (speed * accelerationShift >= spdRange2and3 && speed * accelerationShift < spdRange3and4) {
} else if (speed4Gear * accelerationShift >= spdRange2and3 && speed4Gear * accelerationShift < spdRange3and4) {
calculatedGear = 3;
currentGearRangeLower = spdRange2and3;
currentGearRangeUpper = spdRange3and4;
} else if (speed * accelerationShift >= spdRange3and4 && speed * accelerationShift < spdRange4and5) {
} else if (speed4Gear * accelerationShift >= spdRange3and4 && speed4Gear * accelerationShift < spdRange4and5) {
calculatedGear = 4;
currentGearRangeLower = spdRange3and4;
currentGearRangeUpper = spdRange4and5;
} else if (speed * accelerationShift >= spdRange4and5 && speed * accelerationShift < spdRange5and6) {
} else if (speed4Gear * accelerationShift >= spdRange4and5 && speed4Gear * accelerationShift < spdRange5and6) {
calculatedGear = 5;
currentGearRangeLower = spdRange4and5;
currentGearRangeUpper = spdRange5and6;
} else if (speed * accelerationShift >= spdRange5and6 && speed * accelerationShift < spdRange6and7) {
} else if (speed4Gear * accelerationShift >= spdRange5and6 && speed4Gear * accelerationShift < spdRange6and7) {
calculatedGear = 6;
currentGearRangeLower = spdRange5and6;
currentGearRangeUpper = spdRange6and7;
} else if (speed * accelerationShift >= spdRange6and7 && speed * accelerationShift < spdRange7and8) {
} else if (speed4Gear * accelerationShift >= spdRange6and7 && speed4Gear * accelerationShift < spdRange7and8) {
calculatedGear = 7;
currentGearRangeLower = spdRange6and7;
currentGearRangeUpper = spdRange7and8;
} else if (speed * accelerationShift >= spdRange7and8) {
} else if (speed4Gear * accelerationShift >= spdRange7and8) {
calculatedGear = 8;
currentGearRangeLower = spdRange7and8;
currentGearRangeUpper = 60.0;
@@ -390,7 +392,7 @@ void calcGear() {
calcTimeDiff = millis() - lastGearCalc;
if (calculatedGear == currentGear) {
speedForBar = speed * accelerationShift;
speedForBar = speed4Gear * accelerationShift;
}
if ((calculatedGear + 1) < currentGear || (calculatedGear - 1) > currentGear || calcTimeDiff >= changeDelayMs || speedTrend > 0 ) {
//zmień bieg tylko, gdy rożnica między biegiem wyliczonym a obecnym jest większa niż jeden lub gdy od zmieny biegu minely 3 sec
@@ -403,7 +405,7 @@ void calcGear() {
currentGear = calculatedGear;
previousGear = currentGear;
lastGearCalc = millis();
speedForBar = speed * accelerationShift;
speedForBar = speed4Gear * accelerationShift;
}
if (speedTrend >= 1 and calculatedGear < 8) {
@@ -416,7 +418,7 @@ void calcGear() {
currentGear = calculatedGear;
previousGear = currentGear;
lastGearCalc = millis();
speedForBar = speed * accelerationShift;
speedForBar = speed4Gear * accelerationShift;
} else if (speedTrend <= -1 and calculatedGear > 1) {
if (currentGear > calculatedGear) {
@@ -425,10 +427,10 @@ void calcGear() {
if (currentGear < calculatedGear) {
ups = ups + 1;
}
currentGear = calculatedGear;
previousGear = currentGear;
currentGear = calculatedGear;
lastGearCalc = millis();
speedForBar = speed * accelerationShift;
speedForBar = speed4Gear * accelerationShift;
};
}