460 lines
13 KiB
C++
460 lines
13 KiB
C++
#include <avr/eeprom.h>
|
|
#include <Servo.h> // model servo: DS3218 PRO
|
|
#include <SPI.h>
|
|
#include <Wire.h>
|
|
#include <time.h>
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_SSD1306.h>
|
|
|
|
#define Version "1.12.12"
|
|
////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 PinInterrupt 0 //on board: RX deklaracja pod stabilizacje TX
|
|
#define PinInSpeed 1 //on board: TX
|
|
#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 totalDistMemLocation = 60;
|
|
int loop_cnt = 0;
|
|
unsigned int totalDist = 0;
|
|
unsigned int totalDistReaded = 0;
|
|
unsigned int totalDistWrited = 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(PinInterrupt, OUTPUT);
|
|
digitalWrite(PinInterrupt, HIGH);
|
|
pinMode(Btn1, INPUT);
|
|
//OUTPUT
|
|
pinMode(PinLED, OUTPUT);
|
|
pinMode(BrakingLight, OUTPUT);
|
|
pinMode(ServoSwitch, OUTPUT);
|
|
|
|
//Interrupts
|
|
attachInterrupt(digitalPinToInterrupt(PinInSpeed), calcSpeed, 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("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, totalDistMemLocation, 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 = 21.37;
|
|
}
|
|
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
|
|
// totalDist = 0;
|
|
loop_cnt = loop_cnt +1;
|
|
if (loop_cnt >= 10 && totalDistWrited != totalDist ){
|
|
eeprom_write_block(&totalDist,totalDistMemLocation,2);
|
|
totalDistWrited = totalDist;
|
|
loop_cnt = 0;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
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;
|
|
// 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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
eeprom_write_block(&totalDist,totalDistMemLocation,2);
|
|
totalDistWrited = totalDist;
|
|
loop_cnt = 0;
|
|
|
|
for (; 1000 < (millis() - lastMillisSpd);) {
|
|
digitalWrite(ServoSwitch, LOW);
|
|
digitalWrite(ServoPin, LOW);
|
|
//INFO
|
|
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);
|
|
//Version
|
|
display.setCursor(0, 13);
|
|
display.println("V: ");
|
|
display.setCursor(10, 13);
|
|
display.println(Version);
|
|
//DST
|
|
display.setCursor(45, 0);
|
|
display.println("Spins: ");
|
|
display.setCursor(80, 0);
|
|
display.println(totalDistWrited);
|
|
display.setCursor(53, 13);
|
|
display.println("KM: ");
|
|
display.setCursor(90, 13);
|
|
display.println((totalDistWrited*WheelCircumference)/1000);
|
|
//RUN TIME
|
|
run_mins = floor((millis() / 1000) / 60);
|
|
run_hrs = floor(run_mins / 60);
|
|
run_mins = run_mins - (run_hrs * 60);
|
|
display.setCursor(60, 54);
|
|
display.println("T:");
|
|
display.setCursor(75, 54);
|
|
display.println(run_hrs);
|
|
display.setCursor(83, 54);
|
|
display.println(":");
|
|
display.setCursor(88, 54);
|
|
display.println(run_mins);
|
|
display.display();
|
|
delay(500);
|
|
}
|
|
}
|