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

Uproszczenie kodu

Browse Source
This commit is contained in:
sieja
2025-05-18 22:31:22 +02:00
parent 29b6d62873
commit 9904aca97a
3 changed files with 82 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

201
ESP32/ESP_AutomatedGearShifter/ESP_AutomatedGearShifter.ino
View File

@@ -1,14 +1,16 @@
// #include <avr/eeprom.h>
// #include <Servo.h> // model servo: DS3218 PRO
#include <ESP32Servo.h> // model servo: DS3218 PRO
// nie włącza serovo
// nie podaje napięcia z baterii
#include <SPI.h>
#include <Wire.h>
#include <time.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define Version "1.15.8"
#define Version "2.0.1"
////2DO:
//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?
@@ -23,13 +25,15 @@
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// #define PinInterrupt 0 //on board: RX deklaracja pod stabilizacje TX
#define PinInSpeed 4 //on board: D4
#define ServoSwitch 2 //on board: 4
#define ServoPin 19 //on board: 8
#define BrakingLight 13 //on board: 9 BrakingLight
#define PinLED 25 //on board: 10 ORANGE loop signal
#define Btn1 27 //on board: A0 Button1
#define VoltInptPin 23 //on board: D23 Battery Voltage
#define PinInSpeed 34 //on board: D34
#define ServoSwitch 15 //on board: D15
#define ServoPin 19 //on board: D19
#define BrakingLight 13 //on board: D13 BrakingLight
#define PinLED 25 //on board: D25 ORANGE loop signal
#define Btn1 27 //on board: D27 Button1
#define Btn2 33 //on board: D33 Button2
// #define VoltInptPin 23 //on board: D23 Battery Voltage
#define ServoMaxAngle 130
#define MaxAngle 179
@@ -47,12 +51,20 @@ Servo myservo;
//SPEED
double speed = 0;
double speed_last = 0.0;
double speed_last_2 = 0.0;
double speed_last_3 = 0.0;
double readSignalTime_4;
double readSignalTime_3;
double readSignalTime_2;
double readSignalTime_1;
double readSignalTimeAvg;
double raw_speed;
double calcSpeedMain;
double calcSpeed3;
double calcSpeed2;
double calcSpeed1;
double calcSpeedAvg;
int speedTrend = 0;
double sigleTimeSpd = 0.0;
double readSignalTime = 0.0;
double sleepSpd = 0.0;
unsigned long millissSpd = millis();
unsigned long lastMillisSpd = millis();
@@ -147,14 +159,16 @@ void setup() {
// pinMode(PinInterrupt, OUTPUT);
// digitalWrite(PinInterrupt, HIGH);
pinMode(Btn1, INPUT);
pinMode(VoltInptPin, INPUT);
pinMode(Btn2, INPUT);
// pinMode(VoltInptPin, INPUT);
//OUTPUT
pinMode(PinLED, OUTPUT);
pinMode(BrakingLight, OUTPUT);
pinMode(ServoSwitch, OUTPUT);
//Interrupts
attachInterrupt(digitalPinToInterrupt(PinInSpeed), calcSpeed, FALLING);
attachInterrupt(digitalPinToInterrupt(PinInSpeed), readSpeed, FALLING);
display.clearDisplay();
delay(550);
setPosition(7);
@@ -176,10 +190,12 @@ void setup() {
display.setCursor(75, 45);
display.println(ServoMaxAngle);
display.display();
digitalWrite(ServoSwitch, HIGH);
for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
myservo.write(servoCurrPos);
delay(15);
}
//MEMORY
// eeprom_read_block(&totalDistReaded, totalDistMemLocation, 2);
// totalDist = float(totalDistReaded);
@@ -205,16 +221,14 @@ void loop() {
//################################################
//SPEED
sleepSpd = millis() - lastMillisSpd;
if (sleepSpd >= 1500) { //podaj zerową prędkość jeśli nie było odcztu od 1,5 s
sleepSpd = millis() - millissSpd;
if (sleepSpd >= 1000.0) { //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;
speed_last_3 = 0.0;
raw_speed = 0.0;
//przejście w tryb uśpienia za pomocą odpowiedniego ustawienia koła i magnesu
if ((digitalRead(PinInSpeed) == LOW)|| (sleepSpd >= LongTimeToSleep)) {
if ((digitalRead(Btn2) == HIGH)|| (sleepSpd >= LongTimeToSleep)) {
display.fillCircle(75, 10, 10, SSD1306_WHITE);
if (sleepSpd >= TimeToSleep) {
prepareTurnOff();
@@ -225,10 +239,12 @@ void loop() {
if (speed4Gear > 40 || isinf(speed4Gear)) {
speed4Gear = speed4Gear_3;
}
calcSpeed();
calcGear();
displGear = 9 - currentGear;
setPosition(currentGear);
//duzy font
//GEAR
display.setCursor(8, 0);
@@ -250,7 +266,7 @@ void loop() {
display.setCursor(8, 40);
display.println("S:");
display.setCursor(40, 40);
display.println(speed, 1);
display.println(calcSpeedMain, 1);
//GearBar
currentGearRangeMiddle = (currentGearRangeLower + currentGearRangeUpper) / 2;
display.fillRect(115, 32, 7, 1, SSD1306_WHITE);
@@ -266,9 +282,9 @@ void loop() {
//VOLT_BAR
adcBattVoltValue = analogRead(VoltInptPin);
measuredVoltage = (adcBattVoltValue * referenceVoltage) / maxADCValue;
inputVoltage = measuredVoltage * voltageDividerRatio;
// adcBattVoltValue = analogRead(VoltInptPin);
// measuredVoltage = (adcBattVoltValue * referenceVoltage) / maxADCValue;
// inputVoltage = measuredVoltage * voltageDividerRatio;
voltBarHeight = int(((inputVoltage-9)/3)*64); //odjemowanie 9 bo to minimalne napiecie, podział przez 3 bo zakladam max napiecie 12.0V a nie 12.6V
voltBarPosition = 64 - voltBarHeight;
@@ -277,8 +293,14 @@ void loop() {
display.display();
loopTime = millis();
if ((digitalRead(Btn2) == HIGH)) {
digitalWrite(PinLED, HIGH);
}else {
digitalWrite(PinLED, LOW);
}
//wstrzymanie pętli by odczyty były co 0,5s
for (; (millis() - loopTime) < 100 ;) {
for (; (millis() - loopTime) < 300 ;) {
delay(10);
}
@@ -307,134 +329,71 @@ void loop() {
}
//########################################### LOOP ############################################################
//########################################### LOOP ############################################################
void calcSpeed() {
void readSpeed() {
lastLastMillisSpd = lastMillisSpd;
lastMillisSpd = millissSpd;
millissSpd = millis();
sigleTimeSpd = double(millissSpd - lastLastMillisSpd) / 1000;
// 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;
totalDist = totalDist + (1/MagnetsCnt);
readSignalTime_3 = readSignalTime_2;
readSignalTime_2 = readSignalTime_1;
readSignalTime_1 = double(millissSpd - lastLastMillisSpd) / 1000;
readSignalTimeAvg = (readSignalTime_1 + readSignalTime_2 + readSignalTime_3 )/3;
raw_speed = (((2 * Pi) / readSignalTimeAvg * ((WheelCircumference) / (Pi)) * ms2kmh)) / MagnetsCnt;
}
void calcSpeed(){
calcSpeed3 = calcSpeed2;
calcSpeed2 = calcSpeed1;
calcSpeed1 = raw_speed;
calcSpeedAvg = (calcSpeed1 + calcSpeed2 + calcSpeed3)/3;
calcSpeedMain = calcSpeedAvg;
}
void calcGear() {
//speed validation
speedDiff_3 = speed4Gear_3-speed4Gear_2;
speedDiff_2 = speed4Gear_2-speed4Gear_1;
speed4Gear_3 = speed4Gear_2;
speed4Gear_2 = speed4Gear_1;
speed4Gear_1 = speed;
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 ((speed4Gear / speed4Gear_2) >= 1.5) { // przyspieszenie DO weryfikacji czy nie trzeba zamienic na czas lub zwiększyc wartość
speedTrend = 1;
} else if ((speed4Gear - speed4Gear_2) <= -1.5) {
speedTrend = -1;
} else {
speedTrend = 0;
}
// if (speedTrend > 0){ // wymusza wczerśniejszą zmianę biegów gdy wykryto przyspieszanie
// accelerationShift = 1.05;
// }else {
accelerationShift = 1;
// }
if (speed4Gear * accelerationShift >= 0 && speed4Gear * accelerationShift < spdRange1and2) {
if (calcSpeedMain >= 0 && calcSpeedMain < spdRange1and2) {
calculatedGear = 1;
currentGearRangeLower = 2.5;
currentGearRangeUpper = spdRange1and2;
} else if (speed4Gear * accelerationShift >= spdRange1and2 && speed4Gear * accelerationShift < spdRange2and3) {
} else if (calcSpeedMain >= spdRange1and2 && calcSpeedMain < spdRange2and3) {
calculatedGear = 2;
currentGearRangeLower = spdRange1and2;
currentGearRangeUpper = spdRange2and3;
} else if (speed4Gear * accelerationShift >= spdRange2and3 && speed4Gear * accelerationShift < spdRange3and4) {
} else if (calcSpeedMain >= spdRange2and3 && calcSpeedMain < spdRange3and4) {
calculatedGear = 3;
currentGearRangeLower = spdRange2and3;
currentGearRangeUpper = spdRange3and4;
} else if (speed4Gear * accelerationShift >= spdRange3and4 && speed4Gear * accelerationShift < spdRange4and5) {
} else if (calcSpeedMain >= spdRange3and4 && calcSpeedMain < spdRange4and5) {
calculatedGear = 4;
currentGearRangeLower = spdRange3and4;
currentGearRangeUpper = spdRange4and5;
} else if (speed4Gear * accelerationShift >= spdRange4and5 && speed4Gear * accelerationShift < spdRange5and6) {
} else if (calcSpeedMain >= spdRange4and5 && calcSpeedMain < spdRange5and6) {
calculatedGear = 5;
currentGearRangeLower = spdRange4and5;
currentGearRangeUpper = spdRange5and6;
} else if (speed4Gear * accelerationShift >= spdRange5and6 && speed4Gear * accelerationShift < spdRange6and7) {
} else if (calcSpeedMain >= spdRange5and6 && calcSpeedMain < spdRange6and7) {
calculatedGear = 6;
currentGearRangeLower = spdRange5and6;
currentGearRangeUpper = spdRange6and7;
} else if (speed4Gear * accelerationShift >= spdRange6and7 && speed4Gear * accelerationShift < spdRange7and8) {
} else if (calcSpeedMain >= spdRange6and7 && calcSpeedMain < spdRange7and8) {
calculatedGear = 7;
currentGearRangeLower = spdRange6and7;
currentGearRangeUpper = spdRange7and8;
} else if (speed4Gear * accelerationShift >= spdRange7and8) {
} else if (calcSpeedMain >= spdRange7and8) {
calculatedGear = 8;
currentGearRangeLower = spdRange7and8;
currentGearRangeUpper = 60.0;
} else {
calculatedGear = 8; //Default
};
calcTimeDiff = millis() - lastGearCalc;
// previousGear = currentGear;
currentGear = calculatedGear;
lastGearCalc = millis();
if (calculatedGear == currentGear) {
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
if (currentGear > calculatedGear) {
downs = downs + 1;
}
if (currentGear < calculatedGear) {
ups = ups + 1;
}
currentGear = calculatedGear;
previousGear = currentGear;
lastGearCalc = millis();
speedForBar = speed4Gear * accelerationShift;
}
if (speedTrend >= 1 and calculatedGear < 8) {
if (currentGear > calculatedGear) {
downs = downs + 1;
}
if (currentGear < calculatedGear) {
ups = ups + 1;
}
currentGear = calculatedGear;
previousGear = currentGear;
lastGearCalc = millis();
speedForBar = speed4Gear * accelerationShift;
} else if (speedTrend <= -1 and calculatedGear > 1) {
if (currentGear > calculatedGear) {
downs = downs + 1;
}
if (currentGear < calculatedGear) {
ups = ups + 1;
}
previousGear = currentGear;
currentGear = calculatedGear;
lastGearCalc = millis();
speedForBar = speed4Gear * accelerationShift;
};
}
void setPosition(int currentGear) {
@@ -446,7 +405,7 @@ void setPosition(int currentGear) {
if (pos <= 0) {
pos = MinAngle;
}
if (sleepMode == 1 && speed > 0.0) {
if (sleepMode == 1 && calcSpeedMain > 0.0) {
digitalWrite(ServoSwitch, HIGH);
for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
myservo.write(servoCurrPos);

2
ESP32/ESP_AutomatedGearShifter/nowy 2.txt → ESP32/ESP_AutomatedGearShifter/Portowanie pinow.txt
View File

@@ -1,7 +1,9 @@
czeck plytki HAT
PinLED ARDUINO ESP32
SCL 3 BRAK docelowy: D22
ServoSwitch 4 BRAK docelowy: D2
VCC OK OK
GND OK OK
SLA 2 D21

BIN
ESP32/ESP_AutomatedGearShifter/PortowaniePinow.xlsx Normal file
View File

Binary file not shown.