445 lines
13 KiB
C++
445 lines
13 KiB
C++
#include <ESP32Servo.h> // model servo: DS3218 PRO
|
|
#include <SPI.h>
|
|
#include <Wire.h>
|
|
#include <time.h>
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_SSD1306.h>
|
|
|
|
#define Version "2.2.11"
|
|
////2DO:
|
|
// menu do zmiany zakresu predkosci biegów
|
|
// menu do zmiany zakresu kątów biegów, obwodu koła, ilosci magnesow
|
|
|
|
#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 PinInterrupt 0 //on board: RX deklaracja pod stabilizacje TX
|
|
#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 VoltInptPin 27 //on board: D27
|
|
#define Btn2 33 //on board: D33 Button2
|
|
#define ServoShift 4 //ręczna kalibracja biegu
|
|
// SDA D21
|
|
// SCL D22
|
|
// #define VoltInptPin 23 //on board: D23 Battery Voltage prawdopodobnie uszkodzone ADC
|
|
|
|
#define ServoMaxAngle 130
|
|
#define MaxAngle 179
|
|
#define MinAngle 1
|
|
#define MaxGear 8
|
|
#define MinGear 1
|
|
#define WheelCircumference 2.130
|
|
#define MagnetsCnt 8
|
|
#define ms2kmh 3.6
|
|
#define Pi 3.1416
|
|
#define TimeToSleepMs 5000 //5 sec
|
|
#define LongTimeToSleepMs 150000 //150 sec
|
|
#define GearDelayMs 1500
|
|
Servo myservo;
|
|
|
|
//SPEED
|
|
double readSignalTime_5;
|
|
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 readSignalTime = 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 calculatedGear = 1;
|
|
//Przedziały dia biegów
|
|
float spdRange1and2 = 12.0;
|
|
float spdRange2and3 = 16.0;
|
|
float spdRange3and4 = 18.5;
|
|
float spdRange4and5 = 21.0;
|
|
float spdRange5and6 = 26.0;
|
|
float spdRange6and7 = 33.0;
|
|
float spdRange7and8 = 37.0;
|
|
double calcTimeDiff = 0.0;
|
|
double lastGearCalc = millis();
|
|
double speedDiffKmh = 0.80;
|
|
double accelerationShift = 1.0;
|
|
int displGear = 9 - currentGear;
|
|
float currentGearRangeLower = 0;
|
|
float currentGearRangeUpper = 7.5;
|
|
//SERVO
|
|
int pos = 0;
|
|
int sleepMode = 0;
|
|
int servoCurrPos = ServoMaxAngle;
|
|
//GearRangePointer
|
|
int pointerPosition = 0;
|
|
unsigned int pointerVisibility = 1;
|
|
//BATTERY
|
|
float referenceVoltage = 3.3;
|
|
int maxADCValue = 4095;
|
|
float voltageDividerRatio = 5.95;
|
|
int adcBattVoltValue = 0;
|
|
float inputVoltage = 0.0;
|
|
float measuredVoltage = 0.0;
|
|
int voltBarHeight = 0;
|
|
int voltBarPosition = 0;
|
|
//oth
|
|
int BrakingLightSwitch;
|
|
int run_hrs = 0;
|
|
int run_mins = 0;
|
|
void setup() {
|
|
//SERVO
|
|
digitalWrite(ServoSwitch, HIGH);
|
|
myservo.attach(ServoPin); // attaches the servo on pin 4 to the servo object
|
|
setPosition(8);
|
|
|
|
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(PinInterrupt, OUTPUT);
|
|
// digitalWrite(PinInterrupt, HIGH);
|
|
pinMode(VoltInptPin, INPUT);
|
|
pinMode(Btn2, INPUT);
|
|
|
|
|
|
//OUTPUT
|
|
pinMode(PinLED, OUTPUT);
|
|
pinMode(BrakingLight, OUTPUT);
|
|
pinMode(ServoSwitch, OUTPUT);
|
|
|
|
//Interrupts
|
|
attachInterrupt(digitalPinToInterrupt(PinInSpeed), readSpeed, FALLING);
|
|
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("TimeToSleepMs:");
|
|
display.setCursor(75, 30);
|
|
display.println(TimeToSleepMs);
|
|
display.setCursor(0, 45);
|
|
display.println("ServoMaxAgl:");
|
|
display.setCursor(75, 45);
|
|
display.println(ServoMaxAngle);
|
|
display.display();
|
|
digitalWrite(ServoSwitch, HIGH);
|
|
for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
|
|
myservo.write(servoCurrPos);
|
|
delay(15);
|
|
}
|
|
}
|
|
|
|
|
|
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 && calcSpeedMain > 0.0) {
|
|
digitalWrite(ServoSwitch, HIGH);
|
|
for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
|
|
myservo.write(servoCurrPos);
|
|
delay(4);
|
|
}
|
|
sleepMode = 0;
|
|
}
|
|
pos = pos + ServoShift; //reczna kalibracja
|
|
myservo.write(pos);
|
|
}
|
|
|
|
void readSpeed() {
|
|
lastLastMillisSpd = lastMillisSpd;
|
|
lastMillisSpd = millissSpd;
|
|
millissSpd = millis();
|
|
readSignalTime_5 = readSignalTime_4;
|
|
readSignalTime_4 = readSignalTime_3;
|
|
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 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(15);
|
|
}
|
|
|
|
for (; 1500 < (millis() - lastMillisSpd);) { // zmiana z 1000 na 1500 w 1.13.19
|
|
digitalWrite(ServoSwitch, LOW);
|
|
digitalWrite(ServoPin, LOW);
|
|
//INFO
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setCursor(5, 30);
|
|
display.println("Mozna teraz");
|
|
display.setCursor(5, 38);
|
|
display.println("bezpiecznie wylaczyc");
|
|
display.setCursor(5, 46);
|
|
display.println("komputer.");
|
|
display.setCursor(5, 0);
|
|
display.write(31);
|
|
display.setCursor(10, 0);
|
|
display.println("+");
|
|
display.setCursor(15, 0);
|
|
display.write(30);
|
|
//Version
|
|
display.setCursor(5, 13);
|
|
display.println("V: ");
|
|
display.setCursor(15, 13);
|
|
display.println(Version);
|
|
//RUN TIME
|
|
run_mins = floor((millis() / 1000) / 60);
|
|
run_hrs = floor(run_mins / 60);
|
|
run_mins = run_mins - (run_hrs * 60);
|
|
display.setCursor(65, 54);
|
|
display.println("T:");
|
|
display.setCursor(80, 54);
|
|
display.println(run_hrs);
|
|
display.setCursor(88, 54);
|
|
display.println(":");
|
|
display.setCursor(93, 54);
|
|
display.println(run_mins);
|
|
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;
|
|
display.fillRect(0, voltBarPosition, 2, voltBarHeight, SSD1306_WHITE);
|
|
display.display();
|
|
delay(500);
|
|
}
|
|
}
|
|
|
|
void calcSpeed(){
|
|
calcSpeed3 = calcSpeed2;
|
|
calcSpeed2 = calcSpeed1;
|
|
calcSpeed1 = raw_speed;
|
|
|
|
calcSpeedAvg = (calcSpeed1 + calcSpeed2 + calcSpeed3)/3;
|
|
calcSpeedMain = calcSpeedAvg;
|
|
|
|
if(abs(raw_speed - calcSpeed2) >= speedDiffKmh) {
|
|
if ((raw_speed - calcSpeed2) < 0) {
|
|
speedTrend = -1;
|
|
} else {
|
|
speedTrend = 1;
|
|
}
|
|
}else {
|
|
speedTrend = 0;
|
|
}
|
|
}
|
|
void calcGear() {
|
|
accelerationShift = 1;
|
|
|
|
if (calcSpeedMain >= 0 && calcSpeedMain < spdRange1and2) {
|
|
calculatedGear = 1;
|
|
currentGearRangeLower = 2.5;
|
|
currentGearRangeUpper = spdRange1and2;
|
|
} else if (calcSpeedMain >= spdRange1and2 && calcSpeedMain < spdRange2and3) {
|
|
calculatedGear = 2;
|
|
currentGearRangeLower = spdRange1and2;
|
|
currentGearRangeUpper = spdRange2and3;
|
|
} else if (calcSpeedMain >= spdRange2and3 && calcSpeedMain < spdRange3and4) {
|
|
calculatedGear = 3;
|
|
currentGearRangeLower = spdRange2and3;
|
|
currentGearRangeUpper = spdRange3and4;
|
|
} else if (calcSpeedMain >= spdRange3and4 && calcSpeedMain < spdRange4and5) {
|
|
calculatedGear = 4;
|
|
currentGearRangeLower = spdRange3and4;
|
|
currentGearRangeUpper = spdRange4and5;
|
|
} else if (calcSpeedMain >= spdRange4and5 && calcSpeedMain < spdRange5and6) {
|
|
calculatedGear = 5;
|
|
currentGearRangeLower = spdRange4and5;
|
|
currentGearRangeUpper = spdRange5and6;
|
|
} else if (calcSpeedMain >= spdRange5and6 && calcSpeedMain < spdRange6and7) {
|
|
calculatedGear = 6;
|
|
currentGearRangeLower = spdRange5and6;
|
|
currentGearRangeUpper = spdRange6and7;
|
|
} else if (calcSpeedMain >= spdRange6and7 && calcSpeedMain < spdRange7and8) {
|
|
calculatedGear = 7;
|
|
currentGearRangeLower = spdRange6and7;
|
|
currentGearRangeUpper = spdRange7and8;
|
|
} else if (calcSpeedMain >= spdRange7and8) {
|
|
calculatedGear = 8;
|
|
currentGearRangeLower = spdRange7and8;
|
|
currentGearRangeUpper = 60.0;
|
|
} else {
|
|
calculatedGear = 8; //Default
|
|
};
|
|
|
|
calcTimeDiff = millis() - lastGearCalc;
|
|
if (calcTimeDiff < GearDelayMs && (currentGear - calculatedGear) == 1){
|
|
currentGear = currentGear;
|
|
pointerVisibility = 0;
|
|
} else {
|
|
if (currentGear != calculatedGear) {
|
|
lastGearCalc = millis();
|
|
}
|
|
currentGear = calculatedGear;
|
|
pointerVisibility = 1;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//########################################### LOOP ############################################################
|
|
//########################################### LOOP ############################################################
|
|
|
|
|
|
|
|
void loop() {
|
|
|
|
loopTime = millis();
|
|
display.clearDisplay();
|
|
// //DIAG Btn1
|
|
// display.setTextSize(1);
|
|
// display.setCursor(0, 30);
|
|
// display.println("Btn1:");
|
|
// display.setCursor(30, 20);
|
|
// display.println(digitalRead(Btn1));
|
|
// display.setCursor(30, 30);
|
|
// display.println(analogRead(Btn1));
|
|
// display.setCursor(55, 30);
|
|
|
|
display.setTextSize(3);
|
|
|
|
//################################################
|
|
//SPEED
|
|
sleepSpd = millis() - millissSpd;
|
|
if (sleepSpd >= 1000.0) { //podaj zerową prędkość jeśli nie było odcztu od 1,1 s
|
|
raw_speed = 0.0;
|
|
calcSpeed3 = 0.0;
|
|
calcSpeed2 = 0.0;
|
|
calcSpeed1 = 0.0;
|
|
}
|
|
//przejście w tryb uśpienia za przuycisku lub czasu
|
|
if ((digitalRead(Btn2) == HIGH)|| (sleepSpd >= LongTimeToSleepMs)) {
|
|
prepareTurnOff();
|
|
}
|
|
|
|
calcSpeed();
|
|
calcGear();
|
|
displGear = 9 - currentGear;
|
|
setPosition(currentGear);
|
|
|
|
|
|
//duzy font
|
|
//GEAR
|
|
display.setCursor(8, 0);
|
|
display.println("G:");
|
|
display.setCursor(40, 0);
|
|
display.print(currentGear);
|
|
//SPEED_TREND
|
|
display.setCursor(75, 00);
|
|
if (speedTrend <= -1 ) {
|
|
display.write(31);
|
|
} else {
|
|
if (speedTrend >= 1) {
|
|
display.write(30);
|
|
} else {
|
|
display.println("-");
|
|
}
|
|
}
|
|
//SPEED
|
|
display.setCursor(8, 40);
|
|
display.println("S:");
|
|
display.setCursor(40, 40);
|
|
display.println(calcSpeedMain, 1);
|
|
//GearRangePointer
|
|
if (pointerVisibility == 1){
|
|
display.fillRect(115, 32, 7, 1, SSD1306_WHITE);
|
|
display.fillRect(115, 0, 7, 1, SSD1306_WHITE);
|
|
display.fillRect(115, 63, 7, 1, SSD1306_WHITE);
|
|
display.setTextSize(2);
|
|
pointerPosition = 64 - int(((calcSpeedMain - currentGearRangeLower) / (currentGearRangeUpper - currentGearRangeLower)) * 64) - 5;
|
|
display.setCursor(115, pointerPosition);
|
|
display.write(16);
|
|
display.setTextSize(3);
|
|
}
|
|
|
|
//VOLT_BAR
|
|
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;
|
|
display.fillRect(0, voltBarPosition, 2, voltBarHeight, SSD1306_WHITE);
|
|
//########################################## ZAPIS DO WYŚWIETLACZA ######################################################
|
|
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) < 300 ;) {
|
|
delay(10);
|
|
}
|
|
|
|
|
|
|
|
if (speedTrend == -1 ) {
|
|
if (BrakingLightSwitch == 1) {
|
|
digitalWrite(BrakingLight, HIGH);
|
|
BrakingLightSwitch = 0;
|
|
} else {
|
|
digitalWrite(BrakingLight, LOW);
|
|
BrakingLightSwitch = 1;
|
|
}
|
|
} else {
|
|
digitalWrite(BrakingLight, LOW);
|
|
}
|
|
|
|
}
|
|
//########################################### LOOP ############################################################
|
|
//########################################### LOOP ############################################################
|