- added batteryCalibration

- start with default Menus for DriveModes

lib/Battery/battery.cpp

@@ -27,10 +27,15 @@ Battery::Battery(uint8_t pin) {
 }
 
 void Battery::run() {
+    if (this->calibrationState != CalibrationState::None) {
+        this->runCalibration();
+        return;
+    }
+
     this->readAdcToBuf();
     this->loopCounter++;
 
-    if (this->loopCounter == this->calulationDelayMultiplier) {
+    if (this->loopCounter >= this->calulationDelayMultiplier) {
         this->calculateBatteryVoltage();
         this->calculateBatteryPercent();
         this->loopCounter = 0;
@@ -40,6 +45,28 @@ void Battery::run() {
     return;
 }
 
+void Battery::runCalibration() {
+    if (this->calibrationState != CalibrationState::Reading)
+        return;
+
+    this->readAdcToBuf();        
+    if (this->bufferPos == 0) {
+        uint16_t res = this->getBufAvg();
+        this->newRawAdcVoltages[this->currentCalibrationVoltage] = res;
+        std::cout << "Index: " 
+                  << (int) this->currentCalibrationVoltage 
+                  << " Value: "
+                  << (int) res
+                  << std::endl;
+        this->currentCalibrationVoltage++;
+        this->calibrationState = CalibrationState::Waiting;
+
+        if (this->currentCalibrationVoltage == 60) {
+            this->calibrationState = CalibrationState::Finished;
+        }
+    }
+}
+
 double Battery::getBatteryVoltage() const {
     double res = this->batteryVoltage;
     return (int)(res*100+0.5)/100.0;
@@ -59,6 +86,29 @@ bool Battery::isNewValue() {
     return true;
 }
 
+void Battery::nextVoltageIsReady() {
+    if (this->calibrationState == CalibrationState::Waiting) {
+        this->calibrationState = CalibrationState::Reading;
+    }
+}
+
+void Battery::startCalibration() {
+    this->calibrationState = CalibrationState::Waiting;
+    this->currentCalibrationVoltage = 0;
+    this->bufferPos = 0;
+    this->loopDelay = 50;
+    this->newRawAdcVoltages = new uint16_t[60];
+}
+
+void Battery::finishCalibration() {
+    if (this->calibrationState != CalibrationState::None)
+        return;
+
+    delete[] this->newRawAdcVoltages;
+    this->calibrationState = CalibrationState::None;
+    this->loopDelay = 100;
+}
+
 double Battery::calculateInputVoltage() {
     // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
     double reading = this->getBufAvg();