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_GFX.h>
#include <Adafruit_SSD1306.h> #include <Adafruit_SSD1306.h>
#define Version "1.14.3" #define Version "1.15.3"
////2DO: ////2DO:
//algorytm walidacji odczytu predkosci //diagnostyka i/lub przeciwdziałanie skokom predkosci
//zmiana gear baaru na wskaznik //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 predkosci biegów
// menu do zmiany zakresu kątów biegów, obwodu koła, ilosci magnesow // menu do zmiany zakresu kątów biegów, obwodu koła, ilosci magnesow
@@ -202,7 +203,9 @@ void loop() {
//################################################ //################################################
//SPEED //SPEED
sleepSpd = millis() - lastMillisSpd; 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 = 0.0;
speed_last = 0.0; speed_last = 0.0;
speed_last_2 = 0.0; speed_last_2 = 0.0;
@@ -216,8 +219,8 @@ void loop() {
} }
} }
if (speed > 99 || isinf(speed)) { if (speed4Gear > 99 || isinf(speed4Gear)) {
speed = 21.37; speed4Gear = 21.37;
} }
calcGear(); calcGear();
displGear = 9 - currentGear; displGear = 9 - currentGear;
@@ -230,7 +233,7 @@ void loop() {
display.setCursor(40, 0); display.setCursor(40, 0);
display.print(currentGear); display.print(currentGear);
//SPEED_TREND //SPEED_TREND
display.setCursor(100, 00); display.setCursor(75, 00);
if (speedTrend <= -1 ) { if (speedTrend <= -1 ) {
display.write(31); display.write(31);
} else { } else {
@@ -251,14 +254,13 @@ void loop() {
display.fillRect(115, 0, 7, 1, SSD1306_WHITE); display.fillRect(115, 0, 7, 1, SSD1306_WHITE);
display.fillRect(115, 63, 7, 1, SSD1306_WHITE); display.fillRect(115, 63, 7, 1, SSD1306_WHITE);
if (speed >= currentGearRangeMiddle) { display.setTextSize(2);
gearBarHeight = int(((speedForBar - currentGearRangeMiddle) / (currentGearRangeUpper - currentGearRangeMiddle)) * 32);
gearBarPosition = 32 - gearBarHeight; gearBarHeight = 64 - int(((speed4Gear - currentGearRangeLower) / (currentGearRangeUpper - currentGearRangeLower)) * 64) - 5;
display.fillRect(122, gearBarPosition, 4, gearBarHeight, SSD1306_WHITE); display.setCursor(115, gearBarHeight);
} else { display.write(16);
gearBarHeight = int(((currentGearRangeMiddle - speedForBar) / (currentGearRangeMiddle - currentGearRangeLower)) * 32); display.setTextSize(3);
display.fillRect(122, 32, 4, gearBarHeight, SSD1306_WHITE);
}
//VOLT_BAR //VOLT_BAR
adcBattVoltValue = analogRead(VoltInptPin); adcBattVoltValue = analogRead(VoltInptPin);
@@ -279,7 +281,7 @@ void loop() {
if (speedTrend == -1 && speed > 0.0 ) { if (speedTrend == -1 && speed4Gear > 0.0 ) {
if (BrakingLightSwitch == 1) { if (BrakingLightSwitch == 1) {
digitalWrite(BrakingLight, HIGH); digitalWrite(BrakingLight, HIGH);
BrakingLightSwitch = 0; BrakingLightSwitch = 0;
@@ -307,41 +309,41 @@ void calcSpeed() {
lastMillisSpd = millissSpd; lastMillisSpd = millissSpd;
millissSpd = millis(); millissSpd = millis();
sigleTimeSpd = double(millissSpd - lastLastMillisSpd) / 1000; sigleTimeSpd = double(millissSpd - lastLastMillisSpd) / 1000;
speed_last_3 = speed_last_2; // speed_last_3 = speed_last_2;
speed_last_2 = speed_last; // speed_last_2 = speed_last;
speed_last = speed; // speed_last = speed;
speed = (((2 * Pi) / sigleTimeSpd * ((WheelCircumference) / (Pi)) * ms2kmh)) / MagnetsCnt; speed = (((2 * Pi) / sigleTimeSpd * ((WheelCircumference) / (Pi)) * ms2kmh)) / MagnetsCnt;
// diag // diag
if (((speed_last_3 * 1.5) > speed) && speed > 10.0 && speedTrend > 0) { //zabezpieczenie przed losowymi sygnałami magesu // if (((speed_last_3 * 1.5) > speed) && speed > 10.0 && speedTrend > 0) { //zabezpieczenie przed losowymi sygnałami magesu
speed = speed_last_3; // speed = speed_last_3;
} // }
speed = (speed + speed_last)/2; // speed = (speed + speed_last)/2;
totalDist = totalDist + (1/MagnetsCnt); totalDist = totalDist + (1/MagnetsCnt);
} }
void calcGear() { void calcGear() {
//speed validation //speed validation
// speedDiff_3 = speed4Gear_3-speed4Gear_2;
// speedDiff_2 = speed4Gear_2-speed4Gear_1; speedDiff_2 = speed4Gear_2-speed4Gear_1;
// speedDiff_3 = speed4Gear_3-speed4Gear_2; speed4Gear_1 = speed;
// speed4Gear_1 = speed; speed4Gear_2 = speed4Gear_1;
// speed4Gear_2 = speed4Gear_1; speed4Gear_3 = speed4Gear_2;
// speed4Gear_3 = speed4Gear_2; speed4Geat_estimated = (((speedDiff_3 * avgWeight_3 + speedDiff_2 * avgWeight_2)/(avgWeight_3 + avgWeight_2))+ speed4Gear_1) + 4;
// 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) {
// if (speed <= 10.0 or speed < speed4Geat_estimated) { speed4Gear_2 = speed4Gear;
// speed4Gear = speed4Gear_1; speed4Gear = speed4Gear_1;
// } }
//
//
//poniżej zamienić speed na speed4Gear //poniżej zamienić speed na speed4Gear
//speed_last_3 na speed4Gear_2 //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; speedTrend = 1;
} else if ((speed - speed_last_3) <= -1.0) { } else if ((speed4Gear - speed4Gear_2) <= -0.5) {
speedTrend = -1; speedTrend = -1;
} else { } else {
speedTrend = 0; speedTrend = 0;
@@ -352,35 +354,35 @@ void calcGear() {
// }else { // }else {
accelerationShift = 1; accelerationShift = 1;
// } // }
if (speed * accelerationShift >= 0 && speed * accelerationShift < spdRange1and2) { if (speed4Gear * accelerationShift >= 0 && speed4Gear * accelerationShift < spdRange1and2) {
calculatedGear = 1; calculatedGear = 1;
currentGearRangeLower = 2.5; currentGearRangeLower = 2.5;
currentGearRangeUpper = spdRange1and2; currentGearRangeUpper = spdRange1and2;
} else if (speed * accelerationShift >= spdRange1and2 && speed * accelerationShift < spdRange2and3) { } else if (speed4Gear * accelerationShift >= spdRange1and2 && speed4Gear * accelerationShift < spdRange2and3) {
calculatedGear = 2; calculatedGear = 2;
currentGearRangeLower = spdRange1and2; currentGearRangeLower = spdRange1and2;
currentGearRangeUpper = spdRange2and3; currentGearRangeUpper = spdRange2and3;
} else if (speed * accelerationShift >= spdRange2and3 && speed * accelerationShift < spdRange3and4) { } else if (speed4Gear * accelerationShift >= spdRange2and3 && speed4Gear * accelerationShift < spdRange3and4) {
calculatedGear = 3; calculatedGear = 3;
currentGearRangeLower = spdRange2and3; currentGearRangeLower = spdRange2and3;
currentGearRangeUpper = spdRange3and4; currentGearRangeUpper = spdRange3and4;
} else if (speed * accelerationShift >= spdRange3and4 && speed * accelerationShift < spdRange4and5) { } else if (speed4Gear * accelerationShift >= spdRange3and4 && speed4Gear * accelerationShift < spdRange4and5) {
calculatedGear = 4; calculatedGear = 4;
currentGearRangeLower = spdRange3and4; currentGearRangeLower = spdRange3and4;
currentGearRangeUpper = spdRange4and5; currentGearRangeUpper = spdRange4and5;
} else if (speed * accelerationShift >= spdRange4and5 && speed * accelerationShift < spdRange5and6) { } else if (speed4Gear * accelerationShift >= spdRange4and5 && speed4Gear * accelerationShift < spdRange5and6) {
calculatedGear = 5; calculatedGear = 5;
currentGearRangeLower = spdRange4and5; currentGearRangeLower = spdRange4and5;
currentGearRangeUpper = spdRange5and6; currentGearRangeUpper = spdRange5and6;
} else if (speed * accelerationShift >= spdRange5and6 && speed * accelerationShift < spdRange6and7) { } else if (speed4Gear * accelerationShift >= spdRange5and6 && speed4Gear * accelerationShift < spdRange6and7) {
calculatedGear = 6; calculatedGear = 6;
currentGearRangeLower = spdRange5and6; currentGearRangeLower = spdRange5and6;
currentGearRangeUpper = spdRange6and7; currentGearRangeUpper = spdRange6and7;
} else if (speed * accelerationShift >= spdRange6and7 && speed * accelerationShift < spdRange7and8) { } else if (speed4Gear * accelerationShift >= spdRange6and7 && speed4Gear * accelerationShift < spdRange7and8) {
calculatedGear = 7; calculatedGear = 7;
currentGearRangeLower = spdRange6and7; currentGearRangeLower = spdRange6and7;
currentGearRangeUpper = spdRange7and8; currentGearRangeUpper = spdRange7and8;
} else if (speed * accelerationShift >= spdRange7and8) { } else if (speed4Gear * accelerationShift >= spdRange7and8) {
calculatedGear = 8; calculatedGear = 8;
currentGearRangeLower = spdRange7and8; currentGearRangeLower = spdRange7and8;
currentGearRangeUpper = 60.0; currentGearRangeUpper = 60.0;
@@ -390,7 +392,7 @@ void calcGear() {
calcTimeDiff = millis() - lastGearCalc; calcTimeDiff = millis() - lastGearCalc;
if (calculatedGear == currentGear) { if (calculatedGear == currentGear) {
speedForBar = speed * accelerationShift; speedForBar = speed4Gear * accelerationShift;
} }
if ((calculatedGear + 1) < currentGear || (calculatedGear - 1) > currentGear || calcTimeDiff >= changeDelayMs || speedTrend > 0 ) { 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 //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; currentGear = calculatedGear;
previousGear = currentGear; previousGear = currentGear;
lastGearCalc = millis(); lastGearCalc = millis();
speedForBar = speed * accelerationShift; speedForBar = speed4Gear * accelerationShift;
} }
if (speedTrend >= 1 and calculatedGear < 8) { if (speedTrend >= 1 and calculatedGear < 8) {
@@ -416,7 +418,7 @@ void calcGear() {
currentGear = calculatedGear; currentGear = calculatedGear;
previousGear = currentGear; previousGear = currentGear;
lastGearCalc = millis(); lastGearCalc = millis();
speedForBar = speed * accelerationShift; speedForBar = speed4Gear * accelerationShift;
} else if (speedTrend <= -1 and calculatedGear > 1) { } else if (speedTrend <= -1 and calculatedGear > 1) {
if (currentGear > calculatedGear) { if (currentGear > calculatedGear) {
@@ -425,10 +427,10 @@ void calcGear() {
if (currentGear < calculatedGear) { if (currentGear < calculatedGear) {
ups = ups + 1; ups = ups + 1;
} }
currentGear = calculatedGear;
previousGear = currentGear; previousGear = currentGear;
currentGear = calculatedGear;
lastGearCalc = millis(); lastGearCalc = millis();
speedForBar = speed * accelerationShift; speedForBar = speed4Gear * accelerationShift;
}; };
} }