Denk proof

app.cpp

@@ -1,16 +1,19 @@
 #include "ev3cxx.h"
 #include "app.h"
+#include <time.h>
 #include <ctime>
-#include <functional>
-#include <sstream>
 #include <math.h>
-#include <cfloat>
+#include <iomanip>
-#include <limits.h>
+#include <cstring>
 
 /*
 Change notes:
 
-0.4.3 - 'HELGA' (#)
+0.5.0 - 'HELGA'
+Added diagnostics screen (opened by pressing left after boot)
+Added secret quiet mode
+
+0.4.3 - 'HELGA' (#70)
 Motor problems indicator with colors:
     left - red, right - orange,
     when motor is stuck change LED color
@@ -42,7 +45,7 @@ Re-created structs for version, MPWRS, note
 Re-created function for opening and closing door and function for generating Version ID
 */
 
-/* Structure to hole current version (major, minor, patch, codename, id, release_date)
+/* Structure to hold current version (major, minor, patch, codename, id, release_date)
     MAJOR - Major version number
     MINOR - Minor changes in code
     PATCH - Mostly just for testing
@@ -81,8 +84,131 @@ struct note
     int32_t duration;
 };
 
+MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
+{
+    int batteryLevel = ev3_battery_voltage_mV();
+
+    double squared = (double)0.00003 * (double)(batteryLevel * batteryLevel);
+    double linear = -0.42 * (double)batteryLevel;
+    double constant = 1470;
+    double modifier = squared + linear + constant;
+    int SPEED_MODIFIER = 30 - ((int)round(modifier));
+    // ev3cxx::display.format(4, "Q: % \nL: % \nC: % \nSM: % ") % squared % linear % constant % modifier;
+
+    MPWRS idealMPWRS;
+    idealMPWRS.lMotorPWR = leftMotor + SPEED_MODIFIER;
+    idealMPWRS.rMotorPWR = rightMotor + SPEED_MODIFIER;
+
+    MPWRSPlus retVal;
+    retVal.motor_powers = idealMPWRS;
+    retVal.SPEED_MODIFIER = SPEED_MODIFIER;
+    return retVal;
+}
+
+void cleanAndTitle()
+{
+    ev3cxx::display.resetScreen();
+    ev3cxx::display.format(0, "    DOBREMYSL    ");
+    ev3cxx::display.format(1, "*****************");
+}
+
+// diacnostic functin()
+void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int batteryLevel, int SPEED_MODIFIER, int turningThreshold, int CORRECTION_MULTIPLIER, int shortOneCycleLimit)
+{
+    ev3cxx::display.resetScreen();
+    ev3cxx::display.setFont(EV3_FONT_MEDIUM);
+
+    ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::RIGHT); // Right button
+    ev3cxx::BrickButton btnUp(ev3cxx::BrickButtons::UP);      // Up button
+    ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN);  // Down button
+
+    int pages = 2;
+    int page = 0;
+    while (true)
+    {
+        switch (page)
+        {
+        case 0:
+            ev3cxx::display.resetScreen();
+            ev3cxx::display.format(0, "    DOBREMYSL    ");
+            ev3cxx::display.format(1, "*****************");
+            ev3cxx::display.format(2, "Ver.: % .% .% ") % currentVersion.major % currentVersion.minor % currentVersion.patch;
+            ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id;
+            ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
+            ev3cxx::display.format(5, "Rel.: % ") % std::asctime(&currentVersion.relDate);
+            ev3cxx::display.format(7, "-----------------");
+            break;
+        case 1:
+            ev3cxx::display.resetScreen();
+            ev3cxx::display.format(0, "Bat.: % mV") % batteryLevel;
+            ev3cxx::display.format(1, "Mod.: % ") % SPEED_MODIFIER;
+            ev3cxx::display.format(2, "-------_T_-------");
+            ev3cxx::display.format(3, "TT: % ") % turningThreshold;
+            ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
+            ev3cxx::display.format(5, "-------_1_-------");
+            ev3cxx::display.format(6, "SOCL: % ") % shortOneCycleLimit;
+            ev3cxx::display.format(7, "");
+            break;
+        case 2:
+            ev3cxx::display.resetScreen();
+            ev3cxx::display.format(0, "% ");
+            ev3cxx::display.format(1, "");
+            break;
+
+        default:
+            break;
+        }
+
+        while (true)
+        {
+            if (btnLeft.isPressed())
+            {
+                ev3cxx::display.resetScreen();
+                cleanAndTitle();
+                ev3cxx::display.format(2, "Press ENTR to run");
+                return;
+            }
+
+            if (btnUp.isPressed())
+            {
+                page -= 1;
+                if (page < 0)
+                {
+                    page = 0;
+                }
+                break;
+            }
+            else if (btnDown.isPressed())
+            {
+                page += 1;
+                if (page > pages)
+                {
+                    page = pages;
+                }
+                break;
+            }
+            tslp_tsk(500);
+        }
+        tslp_tsk(200);
+    }
+}
+
+// TimeStamp parser
+std::tm parseTimestamp(const char *timestampStr)
+{
+    std::tm tmStruct = {};
+    std::istringstream iss(timestampStr);
+
+    // The format of __TIMESTAMP__ is implementation-dependent
+    // The example below assumes the format "Www Mmm dd hh:mm:ss yyyy"
+    // Adjust the format string based on your compiler's __TIMESTAMP__ format
+    iss >> std::get_time(&tmStruct, "%a %b %d %H:%M:%S %Y");
+
+    return tmStruct;
+}
+
 // Function to create new program version
-version createVersion(int versionID, const char *codename, int major, int minor, int patch, int day, int month, int year, int hour, int minute)
+version createVersion(int versionID, const char *codename, int major, int minor, int patch)
 {
     version retVersion;
     retVersion.id = versionID;
@@ -90,12 +216,13 @@ version createVersion(int versionID, const char *codename, int major, int minor,
     retVersion.minor = minor;
     retVersion.patch = patch;
     retVersion.codename = codename;
-    retVersion.relDate.tm_sec = 0;
+    /*retVersion.relDate.tm_sec = timestamp_tm.tm_sec;
-    retVersion.relDate.tm_min = minute;
+    retVersion.relDate.tm_min = timestamp_tm.tm_min;
-    retVersion.relDate.tm_hour = hour;
+    retVersion.relDate.tm_hour = timestamp_tm.tm_hour;
-    retVersion.relDate.tm_mday = day;
+    retVersion.relDate.tm_mday = timestamp_tm.tm_mday;
-    retVersion.relDate.tm_mon = month;
+    retVersion.relDate.tm_mon = timestamp_tm.tm_mon;*/
-    retVersion.relDate.tm_year = year - 1900;
+    retVersion.relDate = parseTimestamp(__TIMESTAMP__);
+    retVersion.relDate.tm_year = 2023 - 1900;
     retVersion.relDate.tm_isdst = 0;
     std::mktime(&retVersion.relDate);
 
@@ -114,31 +241,44 @@ void close_door(ev3cxx::Motor hinge)
     hinge.onForDegrees(25, 200);
 }
 
-void turn(ev3cxx::MotorTank naVodu, ev3cxx::GyroSensor naklonoMer)
+/*
+Function for turning
+Robot can turn only left
+*/
+void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2)
 {
-    int startAngle = naklonoMer.angle();
+    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
-    int endAngle = startAngle - 90;
+
-    naVodu.onForRotations(-25, 50, 1.25);
+    // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
-    if(ev3cxx::abs(naklonoMer.angle()) > ev3cxx::abs(endAngle) + 2 || ev3cxx::abs(naklonoMer.angle()) < ev3cxx::abs(endAngle) - 2) { // !!!! NEFUNGUJEž
+    motors.onForRotations(left_speed = -15,
-        ev3_speaker_play_tone(NOTE_C4, 250);
+                          right_speed = 30,
-        tslp_tsk(250);
+                          rotations = 2,
-        ev3_speaker_play_tone(NOTE_C4, 125);
+                          brake = true,
-        tslp_tsk(125);
+                          blocking = false,
-        ev3_speaker_play_tone(NOTE_D4, 250);
+                          wait_after_ms = 60);
-        tslp_tsk(250);
+
+    bool rotating = true;
+    while (rotating)
+    {
+        int currAngle = ev3cxx::abs(gyro.angle());
+        if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle) &&
+            (currAngle < av3cxx::abs(endAngle + THRESHOLD)))
+        {
+            motors.off(brake = true);
+            rotating = false;
+        }
     }
+    return;
 }
 
 /*
 Function to pick up all cubes on shorter side
 return true if ran till the end and false if stopped by middle button
 */
-bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro)
+bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
 {
-    const int LEFT_THRESHOLD = -2;
+    const int LEFT_THRESHOLD = -THRESHOLD;
-    const int RIGHT_THRESHOLD = 2;
+    const int RIGHT_THRESHOLD = THRESHOLD;
-    const int CORRECTION_MULTIPLIER = 20;
-    const int CYCLE_LIMIT = 90;
 
     gyro.resetHard();
     ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
@@ -229,33 +369,19 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
     return !error;
 }
 
-MPWRSPlus calculate_motor_pwrs()
-{
-    int batteryLevel = ev3_battery_voltage_mV();
-
-    double squared = (double)0.00003 * (double)(batteryLevel * batteryLevel);
-    double linear = -0.42 * (double)batteryLevel;
-    double constant = 1470;
-    double modifier = squared + linear + constant;
-    int SPEED_MODIFIER = 30 - ((int)round(modifier));
-    // ev3cxx::display.format(4, "Q: % \nL: % \nC: % \nSM: % ") % squared % linear % constant % modifier;
-
-    MPWRS idealMPWRS;
-    idealMPWRS.lMotorPWR = 85 + SPEED_MODIFIER;
-    idealMPWRS.rMotorPWR = 70 + SPEED_MODIFIER;
-
-    MPWRSPlus retVal;
-    retVal.motor_powers = idealMPWRS;
-    retVal.SPEED_MODIFIER = SPEED_MODIFIER;
-    return retVal;
-}
-
 void main_task(intptr_t unused)
 {
 
+    const int CYCLE_LIMIT_1 = 90;
+    const int THRESHOLD = 2;
+    const int TURNING_THRESHOLD = 1;
+    const int CORRECTION_MULTIPLIER = 20;
+
+    int volume = 100;
+
     // Create version info
     // version createVersion(int versionID, const char *codename, int major, int minor, int patch, int day, int month, int year, int hour, int minute)
-    const version VERSION = createVersion(70, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
+    const version VERSION = createVersion(72, "URSULA", 0, 5, 0);
 
     // Set-up screen
     ev3cxx::display.resetScreen();
@@ -271,13 +397,29 @@ void main_task(intptr_t unused)
     motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
     motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
 
-    // Print version information on screen
-    ev3cxx::display.format("    DOBREMYSL\n% .% .%  #% \nNAME: % \nBattery: % mV % \n % ") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER;
-
     ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
 
+    // Set up motors
+    ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE);  // Hinge motor
+    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
+    ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1);                      // gyro sensor
+    ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4);                   // Touch sensor
+
+    // Set up buttons
+    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
+    ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::LEFT);   // Left button (for entering diagnostics)
+    ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button (quiet mode)
+
+    if(btnDown.isPressed()){
+        volume = 10;
+    }
+
+    // Print version information on screen
+    cleanAndTitle();
+    ev3cxx::display.format(2, "% .% .%  #% \nNAME: % \nBattery: % mV % \nPress ENTR to run") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER;
+
     // Play starting melody
-    ev3_speaker_set_volume(100);
+    ev3_speaker_set_volume(volume);
     ev3_speaker_play_tone(NOTE_C5, 400);
     tslp_tsk(500);
     ev3_speaker_play_tone(NOTE_F5, 400);
@@ -295,20 +437,36 @@ void main_task(intptr_t unused)
     ev3_speaker_play_tone(NOTE_F4, 75);
     tslp_tsk(200);
 
-    // Set up motors
-    ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE);  // Hinge motor
-    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
-    ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1);                      // gyro sensor
-
-    // Set up buttons
-    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
-
     // Start program
-    btnEnter.waitForClick();
+    while (1 == 1)
+    {
+        // ev3_speaker_play_tone(NOTE_FS6, 20);
+        // tslp_tsk(20);
+        if (btnLeft.isPressed())
+        {
+            displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, ev3_battery_voltage_mV(), SPEED_MODIFIER, TURNING_THRESHOLD, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1);
+        }
+        if (btnEnter.isPressed() || touchS.isPressed())
+        {
+            cleanAndTitle();
+            break;
+        }
+        ev3_speaker_play_tone(NOTE_F6, 50);
+        tslp_tsk(50);
+    }
+
+    ev3_speaker_play_tone(NOTE_F4, 100);
+    tslp_tsk(200);
 
     for (int i = 0; i < 10; i++)
     {
-        turn(motors, gyro);
+        gyro.resetHard();
+        if (gyro.angle() == 0)
+        {
+            ev3_speaker_play_tone(NOTE_F6, 200);
+            tslp_tsk(200);
+        }
+        turn(motors, gyro, 90, TURNING_THRESHOLD);
         ev3_speaker_play_tone(NOTE_F4, 100);
         tslp_tsk(200);
         ev3_speaker_play_tone(NOTE_A4, 100);
@@ -334,7 +492,7 @@ void main_task(intptr_t unused)
         ev3_speaker_play_tone(NOTE_C4, 12);
         tslp_tsk(25);
     }
-    bool side_1 = false;//run_short_side(motors, motor_powers, gyro);
+    bool side_1 = false; // run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MLUTIPLIER, THRESHOLD);
 
     if (!side_1)
     {