Denk proof backup #too_many

app.cpp

@@ -4,10 +4,17 @@
 #include <functional>
 #include <sstream>
 #include <math.h>
+#include <cfloat>
+#include <limits.h>
 
 /*
 Change notes:
 
+0.4.3 - 'HELGA' (#)
+Motor problems indicator with colors:
+    left - red, right - orange,
+    when motor is stuck change LED color
+
 0.4.2 - 'HELGA' (#67)
 Updated run_short() to run_short_side()
 Added gyro
@@ -60,6 +67,13 @@ struct MPWRS
     int rMotorPWR;
 };
 
+// Structure to hold motor powers and speed modifier
+struct MPWRSPlus
+{
+    MPWRS motor_powers;
+    int SPEED_MODIFIER;
+};
+
 // Structure to hold notes
 struct note
 {
@@ -100,6 +114,21 @@ void close_door(ev3cxx::Motor hinge)
     hinge.onForDegrees(25, 200);
 }
 
+void turn(ev3cxx::MotorTank naVodu, ev3cxx::GyroSensor naklonoMer)
+{
+    int startAngle = naklonoMer.angle();
+    int endAngle = startAngle - 90;
+    naVodu.onForRotations(-25, 50, 1.25);
+    if(ev3cxx::abs(naklonoMer.angle()) > ev3cxx::abs(endAngle) + 2 || ev3cxx::abs(naklonoMer.angle()) < ev3cxx::abs(endAngle) - 2) { // !!!! NEFUNGUJEž
+        ev3_speaker_play_tone(NOTE_C4, 250);
+        tslp_tsk(250);
+        ev3_speaker_play_tone(NOTE_C4, 125);
+        tslp_tsk(125);
+        ev3_speaker_play_tone(NOTE_D4, 250);
+        tslp_tsk(250);
+    }
+}
+
 /*
 Function to pick up all cubes on shorter side
 return true if ran till the end and false if stopped by middle button
@@ -108,7 +137,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
 {
     const int LEFT_THRESHOLD = -2;
     const int RIGHT_THRESHOLD = 2;
-    const int CORRECTION_MULTIPLIER = 10;
+    const int CORRECTION_MULTIPLIER = 20;
     const int CYCLE_LIMIT = 90;
 
     gyro.resetHard();
@@ -184,6 +213,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
             run = false;
         }
     }
+    motors.off(true);
     ev3_speaker_play_tone(NOTE_C4, 250);
     tslp_tsk(250);
     ev3_speaker_play_tone(NOTE_C4, 125);
@@ -199,19 +229,50 @@ 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)
 {
 
     // 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(67, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
+    const version VERSION = createVersion(70, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
 
     // Set-up screen
     ev3cxx::display.resetScreen();
     ev3cxx::display.setFont(EV3_FONT_MEDIUM);
 
+    // Set up motor powers
+    MPWRSPlus calcedPWRS = calculate_motor_pwrs();
+
+    MPWRS idealMPWRS = calcedPWRS.motor_powers;
+    const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
+
+    MPWRS motor_powers;
+    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
+    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
+
     // Print version information on screen
-    ev3cxx::display.format("    DOBREMYSL\nVERSION: % .% .%  #% \nNAME: % \nBattery: % mV") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV();
+    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);
 
@@ -234,16 +295,6 @@ void main_task(intptr_t unused)
     ev3_speaker_play_tone(NOTE_F4, 75);
     tslp_tsk(200);
 
-    // Set up motor powers
-    const int SPEED_MODIFIER = 10;
-    MPWRS idealMPWRS;
-    idealMPWRS.lMotorPWR = 75 - SPEED_MODIFIER;
-    idealMPWRS.rMotorPWR = 60 - SPEED_MODIFIER;
-
-    MPWRS motor_powers;
-    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
-    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
-
     // 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
@@ -255,8 +306,35 @@ void main_task(intptr_t unused)
     // Start program
     btnEnter.waitForClick();
 
-    // turn_left(motors);
+    for (int i = 0; i < 10; i++)
-    bool side_1 = run_short_side(motors, motor_powers, gyro);
+    {
+        turn(motors, gyro);
+        ev3_speaker_play_tone(NOTE_F4, 100);
+        tslp_tsk(200);
+        ev3_speaker_play_tone(NOTE_A4, 100);
+        tslp_tsk(200);
+        ev3_speaker_play_tone(NOTE_C4, 100);
+        tslp_tsk(200);
+        ev3_speaker_play_tone(NOTE_F4, 50);
+        tslp_tsk(100);
+        ev3_speaker_play_tone(NOTE_A4, 50);
+        tslp_tsk(100);
+        ev3_speaker_play_tone(NOTE_C4, 50);
+        tslp_tsk(100);
+        ev3_speaker_play_tone(NOTE_F4, 25);
+        tslp_tsk(50);
+        ev3_speaker_play_tone(NOTE_A4, 25);
+        tslp_tsk(50);
+        ev3_speaker_play_tone(NOTE_C4, 25);
+        tslp_tsk(50);
+        ev3_speaker_play_tone(NOTE_F4, 12);
+        tslp_tsk(25);
+        ev3_speaker_play_tone(NOTE_A4, 12);
+        tslp_tsk(25);
+        ev3_speaker_play_tone(NOTE_C4, 12);
+        tslp_tsk(25);
+    }
+    bool side_1 = false;//run_short_side(motors, motor_powers, gyro);
 
     if (!side_1)
     {