From 8086dd287d3ce7a59733491859033f862a435bc2 Mon Sep 17 00:00:00 2001 From: Kamil Siejka Date: Wed, 2 Oct 2024 16:37:22 +0200 Subject: [PATCH] Version 1.11.3.1 --- AutomatedGearShifter/AutomatedGearShifter.ino | 441 ++++++++++++++++++ 1 file changed, 441 insertions(+) create mode 100644 AutomatedGearShifter/AutomatedGearShifter.ino diff --git a/AutomatedGearShifter/AutomatedGearShifter.ino b/AutomatedGearShifter/AutomatedGearShifter.ino new file mode 100644 index 0000000..1e8ea76 --- /dev/null +++ b/AutomatedGearShifter/AutomatedGearShifter.ino @@ -0,0 +1,441 @@ + //#include +#include // model servo: DS3218 PRO +#include +#include +#include +#include +#include + +#define Version "1.11.3.1" +//ostatnia zmiana: zmiana na PCB, dostrajanie +////2DO: + +#define SCREEN_WIDTH 128 +#define SCREEN_HEIGHT 64 +#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin) +#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 PinInSpeed 0 //on board: RXI +#define ServoSwitch 4 //on board: 4 +#define ServoPin 8 //on board: 8 +#define BrakingLight 9 //on board: 9 BrakingLight +#define PinLED 10 //on board: 10 ORANGE loop signal +#define Btn1 18 //on board: A0 Button0 +#define Btn2 19 //on board: A1 Button1 + +#define ServoMaxAngle 130 +#define MaxAngle 179 +#define MinAngle 1 +#define MaxGear 8 +#define MinGear 1 +#define WheelCircumference 2.130 +#define MagnetsCnt 4 +#define ms2kmh 3.6 +#define Pi 3.1416 +#define TimeToSleep 180000 //3 min + +Servo myservo; + + +//SPEED +double speed = 0; +double speed_last = 0; +double speed_last_2 = 0; +double speed_last_3 = 0; +int speedTrend = 0; +double sigleTimeSpd = 0.0; +double sleepSpd = 0.0; +unsigned long millissSpd = millis(); +unsigned long lastMillisSpd = millis(); +unsigned long lastLastMillisSpd = millis(); +unsigned long loopTime = millis(); +//GEAR +int currentGear = 1; +int previousGear = 1; +int calculatedGear = 1; +//Przedziały dia biegów +float spdRange1and2 = 7.5; +float spdRange2and3 = 11.0; +float spdRange3and4 = 15.5; +float spdRange4and5 = 18.0; +float spdRange5and6 = 23.5; +float spdRange6and7 = 29.3; +float spdRange7and8 = 36.5; +double calcTimeDiff = 0.0; +double lastGearCalc = millis(); +double changeDelayMs = 2000.0; +double accelerationShift = 1.0; +int displGear = 9 - currentGear; +float currentGearRangeLower = 0; +float currentGearRangeMiddle = 3.0; +float currentGearRangeUpper = 7.5; +//SERVO +int pos = 0; +int sleepMode = 0; +int servoCurrPos = ServoMaxAngle; +//GearBar +int gearBarHeight = 0; +int gearBarPosition = 0; +float speedForBar = 0; +//oth +int BrakingLightSwitch; +int ups = 0; +int downs = 0; +int run_hrs = 0; +int run_mins = 0; +//DST +int totalDist = 0; +int totalDistReaded = 0; + + +void setup() { + //SERVO + digitalWrite(ServoSwitch, HIGH); + myservo.attach(ServoPin); // attaches the servo on pin 4 to the servo object + setPosition(8); + + Serial.begin(9600); + + //DIPLAY settings + if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { + Serial.println(F("SSD1306 allocation failed")); + for (;;); // Don't proceed, loop forever + } + display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); + display.clearDisplay(); + display.setTextColor(WHITE); + display.setRotation(0); + display.setTextSize(3); + display.setCursor(0, 0); + display.println("Version:"); + display.setCursor(0, 25); + display.println(Version); + display.display(); + delay(500); + //INPUT + pinMode(PinInSpeed, INPUT); + pinMode(Btn1, INPUT); + //OUTPUT + pinMode(PinLED, OUTPUT); + pinMode(BrakingLight, OUTPUT); + pinMode(ServoSwitch, OUTPUT); + + //Interrupts + attachInterrupt(digitalPinToInterrupt(PinInSpeed), calcSpeed, FALLING); + + // powolne ustawianie pozycji servo po włączeniu + // delay(750); + // setPosition(8); + + display.clearDisplay(); + delay(550); + setPosition(7); + display.setTextSize(1); + display.setCursor(0, 0); + display.println("wheelSize:"); + display.setCursor(75, 0); + display.println(WheelCircumference); + display.setCursor(0, 16); + display.println("MagnetsCnt:"); + display.setCursor(75, 16); + display.println(MagnetsCnt); + display.setCursor(0, 30); + display.println("TimeToSleep:"); + display.setCursor(75, 30); + display.println(TimeToSleep); + display.setCursor(0, 45); + display.println("ServoMaxAgl:"); + display.setCursor(75, 45); + display.println(ServoMaxAngle); + display.display(); + for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) { + myservo.write(servoCurrPos); + delay(30); + } + //MEMORY + eeprom_read_block(&totalDistReaded, 0, 2); + totalDist = totalDistReaded; + +} + + +void loop() { + + loopTime = millis(); + display.clearDisplay(); + display.setTextSize(3); + + //################################################ + //SPEED + sleepSpd = millis() - lastMillisSpd; + if (sleepSpd >= 2000) { //podaj zerową prędkość jeśli nie było odcztu od 1,5 s + speed = 0.0; + speed_last = 0.0; + speed_last_2 = 0.0; + speed_last_3 = 0.0; + if (sleepSpd >= TimeToSleep) { //ustaw bieg 8. jeśli nie było odcztu od 120 s + prepareTurnOff(); + } + } +//DIAG +// if (speed > 99 || isinf(speed)) { +// speed = 0; +// } + calcGear(); + displGear = 9 - currentGear; + setPosition(currentGear); + + //duzy font + //GEAR + display.setCursor(0, 0); + display.println("G:"); + display.setCursor(35, 0); + display.print(currentGear); + //SPEED_TREND + display.setCursor(100, 00); + if (speedTrend <= -1 ) { + display.write(31); + } else { + if (speedTrend >= 1) { + display.write(30); + } else { + display.println("-"); + } + } + //SPEED + display.setCursor(0, 40); + display.println("S"); + display.setCursor(35, 40); + display.println(speed, 1); + //GearBar + if (sleepSpd <= 30000) { + currentGearRangeMiddle = (currentGearRangeLower + currentGearRangeUpper) / 2; + display.fillRect(115, 32, 7, 1, SSD1306_WHITE); + 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); + } + } else { + display.setTextSize(2); + display.setCursor(90, 25); + display.println(int(((TimeToSleep - sleepSpd) / 1000))); + } + display.display(); + loopTime = millis(); + + //wstrzymanie pętli by odczyty były co 0,5s + for (; (millis() - loopTime) < 100 ;) { + delay(10); + } + + ////przejście w tryb uśpienia za pomocą przycisku + if (digitalRead(Btn1) == HIGH) { + prepareTurnOff(); + } + + if (speedTrend == -1 && speed > 0.0 ) { + if (BrakingLightSwitch == 1) { + digitalWrite(BrakingLight, HIGH); + BrakingLightSwitch = 0; + } else { + digitalWrite(BrakingLight, LOW); + BrakingLightSwitch = 1; + } + } else { + digitalWrite(BrakingLight, LOW); + } + // //TTL DST + // eeprom_write_block(&totalDist,0,2); + +} + +void calcSpeed() { + 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; + 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; +} + +void calcGear() { + 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) { + speedTrend = -1; + } else { + speedTrend = 0; + } + + // if (speedTrend > 0){ // wymusza wczerśniejszą zmianę biegów gdy wykryto przyspieszanie + // accelerationShift = 1.05; + // }else { + accelerationShift = 1; + // } + if (speed * accelerationShift >= 0 && speed * accelerationShift < spdRange1and2) { + calculatedGear = 1; + currentGearRangeLower = 2.5; + currentGearRangeUpper = spdRange1and2; + } else if (speed * accelerationShift >= spdRange1and2 && speed * accelerationShift < spdRange2and3) { + calculatedGear = 2; + currentGearRangeLower = spdRange1and2; + currentGearRangeUpper = spdRange2and3; + } else if (speed * accelerationShift >= spdRange2and3 && speed * accelerationShift < spdRange3and4) { + calculatedGear = 3; + currentGearRangeLower = spdRange2and3; + currentGearRangeUpper = spdRange3and4; + } else if (speed * accelerationShift >= spdRange3and4 && speed * accelerationShift < spdRange4and5) { + calculatedGear = 4; + currentGearRangeLower = spdRange3and4; + currentGearRangeUpper = spdRange4and5; + } else if (speed * accelerationShift >= spdRange4and5 && speed * accelerationShift < spdRange5and6) { + calculatedGear = 5; + currentGearRangeLower = spdRange4and5; + currentGearRangeUpper = spdRange5and6; + } else if (speed * accelerationShift >= spdRange5and6 && speed * accelerationShift < spdRange6and7) { + calculatedGear = 6; + currentGearRangeLower = spdRange5and6; + currentGearRangeUpper = spdRange6and7; + } else if (speed * accelerationShift >= spdRange6and7 && speed * accelerationShift < spdRange7and8) { + calculatedGear = 7; + currentGearRangeLower = spdRange6and7; + currentGearRangeUpper = spdRange7and8; + } else if (speed * accelerationShift >= spdRange7and8) { + calculatedGear = 8; + currentGearRangeLower = spdRange7and8; + currentGearRangeUpper = 60.0; + } else { + calculatedGear = 8; //Default + }; + calcTimeDiff = millis() - lastGearCalc; + + if (calculatedGear == currentGear) { + speedForBar = speed * 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 = speed * 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 = speed * accelerationShift; + + } else if (speedTrend <= -1 and calculatedGear > 1) { + if (currentGear > calculatedGear) { + downs = downs + 1; + } + if (currentGear < calculatedGear) { + ups = ups + 1; + } + currentGear = calculatedGear; + previousGear = currentGear; + lastGearCalc = millis(); + speedForBar = speed * accelerationShift; + }; +} + +void setPosition(int currentGear) { + pos = 180 - round((currentGear - 1) * (ServoMaxAngle / (MaxGear - 1) )); + if (pos >= 180) { + pos = MaxAngle; + } + + if (pos <= 0) { + pos = MinAngle; + } + if (sleepMode == 1 && speed > 0.0) { + digitalWrite(ServoSwitch, HIGH); + for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) { + myservo.write(servoCurrPos); + delay(4); + } + sleepMode = 0; + } + myservo.write(pos); +} + +void prepareTurnOff() { + sleepMode = 1; + display.clearDisplay(); + display.setTextSize(1); + display.setCursor(0, 0); + display.println("Przygotwywanie..."); + display.display(); + for (servoCurrPos = myservo.read(); servoCurrPos >= 60; servoCurrPos--) { + myservo.write(servoCurrPos); + delay(30); + } + for (; 1000 < (millis() - lastMillisSpd);) { + digitalWrite(ServoSwitch, LOW); + digitalWrite(ServoPin, LOW); + // myservo.detach(ServoPin); + display.clearDisplay(); + display.setTextSize(1); + display.setCursor(0, 30); + display.println("Mozna teraz"); + display.setCursor(0, 38); + display.println("bezpiecznie wylaczyc"); + display.setCursor(0, 46); + display.println("komputer."); + display.setCursor(0, 0); + display.write(31); + display.setCursor(5, 0); + display.println("+"); + display.setCursor(10, 0); + display.write(30); + display.setCursor(20, 0); + display.println(downs + ups); + + display.println("Spins: "); + display.setCursor(60, 0); + display.println(totalDist);; + display.setCursor(70, 0); + + run_mins = floor((millis() / 1000) / 60); + run_hrs = floor(run_mins / 60); + run_mins = run_mins - (run_hrs * 60); + display.setCursor(64, 54); + display.println("T:"); + display.setCursor(75, 54); + display.println(run_hrs); + display.setCursor(80, 54); + display.println(":"); + display.setCursor(85, 54); + display.println(run_mins); + display.display(); + delay(500); + } +}