0.8.0 - 'MONIKA' (#85)

0.8.0 - 'MONIKA' (#85)
Error correction on every straight drive
Multiple turning step
Complete code for building one tower

app.cfg

@@ -1,9 +1,13 @@
-INCLUDE("app_common.cfg");
+    INCLUDE("app_common.cfg");
 
 #include "app.h"
 
 DOMAIN(TDOM_APP) {
-CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
+CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI, STACK_SIZE, NULL });
+
+CRE_TSK(SUB_TASK, { TA_NULL, 0, sub_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
+
+CRE_TSK(MUSIC_TASK, { TA_NULL, 0, music_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
 
 // periodic task PRD_TSK_1 that will start automatically
 //CRE_TSK(PRD_TSK_1, { TA_NULL, 0, periodic_task_1, PRIORITY_PRD_TSK_1, STACK_SIZE, NULL });

app.cpp

@@ -1,24 +1,23 @@
 #include "ev3cxx.h"
 #include "app.h"
-#include <time.h>
 #include <ctime>
 #include <math.h>
 #include <iomanip>
-#include <cstring>
 
 /*
 CHANGELOG:
 
-0.7.2 - 'URSULA' (#)
+0.8.0 - 'MONIKA' (#85)
-
+Error correction on every straight drive
+Multiple turning step
+Complete code for building one tower
 
 0.7.0 - 'URSULA' (#80)
 Done first short and long side
 Modified turn function
 Changed drive plan (long - long - middle - short - short - middle)
 
-
+0.6.0 - 'URSULA' (#75)
-0.6.0 - 'HELGA' (#75)
 Added Doxygen comments
 Added volume to display_all_values (diagnostics screen)
 
@@ -109,7 +108,22 @@ struct note
     int32_t duration;
 };
 
-/// @brief Function to calculate motor powers using quadratic equation, depending on battery power
+/// @struct all Values for diagnostic function
+struct allValues
+{
+    version currentVersion;
+    int volume;
+    int lMotorPWR;
+    int rMotorPWR;
+    int SPEED_MODIFIER;
+    int turningThreshold;
+    int TURNING_FACTOR_CORRECTION;
+    int CORRECTION_MULTIPLIER;
+    int shortOneCycleLimit;
+    int loneOneCycleLimit;
+};
+
+/// @brief Function to calculate motor powers using quadratic equation, depending on battery powerMPWRSPlus
 /// @param leftMotor int: wanted power of left motor (default 85)
 /// @param rightMotor int: wante power or right motor (default 70)
 /// @return MPWRSPlus: motor powers and SPEED_MODIFIER
@@ -134,6 +148,28 @@ MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 60)
     return retVal;
 }
 
+/// @brief Function to play starting melody
+void play_starting_melody()
+{
+    // Play starting melody
+    ev3_speaker_play_tone(NOTE_C5, 400);
+    tslp_tsk(500);
+    ev3_speaker_play_tone(NOTE_F5, 400);
+    tslp_tsk(500);
+    ev3_speaker_play_tone(NOTE_G5, 400);
+    tslp_tsk(500);
+    ev3_speaker_play_tone(NOTE_A5, 100);
+    tslp_tsk(200);
+    ev3_speaker_play_tone(NOTE_F5, 650);
+    tslp_tsk(950);
+    ev3_speaker_play_tone(NOTE_F4, 75);
+    tslp_tsk(110);
+    ev3_speaker_play_tone(NOTE_F4, 75);
+    tslp_tsk(110);
+    ev3_speaker_play_tone(NOTE_F4, 75);
+    tslp_tsk(200);
+}
+
 /// @brief Function to clean display and write first two line
 void cleanAndTitle()
 {
@@ -143,17 +179,8 @@ void cleanAndTitle()
 }
 
 /// @brief Diagnostic function to display all value on display with ability to show multiple pages
-/// @param currentVersion version: struct verion of currently defined version
+/// @param allValues values: all values in allValues structure
-/// @param volume: int: current set volume
+void displayAllValues(allValues values)
-/// @param lMotorPWR int: base set power for the left motor
-/// @param rMotorPWR int: base set power for the right motor
-/// @param SPEED_MODIFIER int: caluclated SPEED_MODIFIER
-/// @param turningThreshold int: turning accuracy in degrees
-/// @param TURNING_FACTOR_CORRECTION int: factor for changing final turning values due to gyro sensor placement
-/// @param CORRECTION_MULTIPLIER int: for driving straight
-/// @param shortOneCycleLimit int: how many program cycles to do when running first short side
-/// @param loneOneCycleLimit int: how many program cycles to do when running first long side
-void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit, int loneOneCycleLimit)
 {
     ev3cxx::display.resetScreen();
     ev3cxx::display.setFont(EV3_FONT_MEDIUM);
@@ -172,27 +199,27 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
             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(2, "Ver.: % .% .% ") % values.currentVersion.major % values.currentVersion.minor % values.currentVersion.patch;
-            ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id;
+            ev3cxx::display.format(3, "Nr.: #% ") % values.currentVersion.id;
-            ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
+            ev3cxx::display.format(4, "Codename: % ") % values.currentVersion.codename;
-            ev3cxx::display.format(5, "Volume: % %%") % volume;
+            ev3cxx::display.format(5, "Volume: % %%") % values.volume;
-            ev3cxx::display.format(6, "Rel.: % ") % std::asctime(&currentVersion.relDate);
+            ev3cxx::display.format(6, "Rel.: % ") % std::asctime(&values.currentVersion.relDate);
             break;
         case 1:
             ev3cxx::display.resetScreen();
             ev3cxx::display.format(0, "Bat.: % mV") % ev3_battery_voltage_mV();
-            ev3cxx::display.format(1, "Mod.: % ") % SPEED_MODIFIER;
+            ev3cxx::display.format(1, "Mod.: % ") % values.SPEED_MODIFIER;
             ev3cxx::display.format(2, "-------_T_-------");
-            ev3cxx::display.format(3, "TT: % ") % turningThreshold;
+            ev3cxx::display.format(3, "TT: % ") % values.turningThreshold;
-            ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
+            ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER;
-            ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION;
+            ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION;
             ev3cxx::display.format(6, "-------_1_-------");
-            ev3cxx::display.format(7, "LOCL: % ") % shortOneCycleLimit;
+            ev3cxx::display.format(7, "LOCL: % ") % values.shortOneCycleLimit;
             break;
         case 2:
             ev3cxx::display.resetScreen();
             ev3cxx::display.format(0, "-------_2_-------");
-            ev3cxx::display.format(1, "LOCL: % ") % shortOneCycleLimit;
+            ev3cxx::display.format(1, "LOCL: % ") % values.shortOneCycleLimit;
             break;
         case 3:
             ev3cxx::display.resetScreen();
@@ -235,6 +262,35 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
     }
 }
 
+/// @brief Function to run starting phase of the program (selection of diagnostics screen)
+/// @param btnLeft ev3cxx::BrickButton: left brick button
+/// @param btnRight ev3cxx::BrickButton: right brick button
+/// @param touchS ev3cxx::TouchSensor: touch sensor
+void start_program_exe(ev3cxx::BrickButton btnLeft, ev3cxx::BrickButton btnEnter, ev3cxx::TouchSensor touchS, allValues values)
+{
+    while (true)
+    {
+        // ev3_speaker_play_tone(NOTE_FS6, 20);
+        // tslp_tsk(20);
+        if (btnLeft.isPressed())
+        {
+            displayAllValues(values);
+        }
+        if (btnEnter.isPressed() || touchS.isPressed())
+        {
+            cleanAndTitle();
+            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+            ev3_speaker_play_tone(NOTE_F4, 100);
+            tslp_tsk(750);
+            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+            ev3_speaker_play_tone(NOTE_F4, 100);
+            break;
+        }
+        // ev3_speaker_play_tone(NOTE_F6, 50);
+        tslp_tsk(50);
+    }
+}
+
 /// @brief Function to parse timestamp to std::tm
 /// @param timestampStr const char*: string form of the timestamp
 /// @return std::tm: parsed timestamp
@@ -357,7 +413,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
 
     // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
     // left, right, rotations (faster), brake, blocking, wait_after
-    motors.on(-10, 25);
+    motors.on(-25, 45);
 
     bool rotating = true;
     int counter = 0;
@@ -526,9 +582,12 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
     bool run = true;
     bool error = false;
     int cycleCounter = 0;
+    int lastError = -1;
+    int errorStrike = 0;
 
     while (run)
     {
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
         motors.on(motor_powers.lMotorPWR + 20, motor_powers.rMotorPWR - 20);
         tslp_tsk(50);
 
@@ -543,16 +602,43 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
 
         ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
 
-        if (angle < -2)
+        if (lPower == 0 || rPower == 0 || angle < -2)
         {
-            motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 100;
+            // If error in previous cycle
-            motor_powers.rMotorPWR = idealMPWRS.rMotorPWR - 20;
+            if (lastError == (cycleCounter - 1))
+            {
+                errorStrike++;
+            }
+            // Otherwise clear the error strike
+            else
+            {
+                errorStrike = 0;
+            }
+            // Set lastError to this cycle
+            lastError = cycleCounter;
+
+            // If this is third error cycle in row
+            if (errorStrike == 3)
+            {
+                ev3_speaker_play_tone(NOTE_F6, 50);
+                ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+                // Reset counters etc.
+                lastError = 0;
+                errorStrike = 0;
+                // Back up a bit
+                motors.off(true);
+                motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20));
+                tslp_tsk(350);
+                continue;
+            }
         }
 
         if (angle > 5)
         {
             motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
-            motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 40;
+            motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 35;
+            ev3_speaker_play_tone(NOTE_D4, 50);
+            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
         }
 
         // Emergency break using middle button (BTN_ENTER)
@@ -566,6 +652,54 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
         // Exit when front bumper is hit
         if (bumper.isPressed())
         {
+            ev3_speaker_play_tone(NOTE_F4, 100);
+            motors.off(true);
+
+            int s_cycleCounter = 0;
+            int s_lastError = -1;
+            int s_errorStrike = 0;
+            int s_maxCycles = 60;
+
+            while (s_cycleCounter <= s_maxCycles)
+            {
+                s_cycleCounter++;
+                int lPower = motors.leftMotor().currentPower();
+                int rPower = motors.rightMotor().currentPower();
+                if (lPower == 0 || rPower == 0)
+                {
+                    // If error in previous cycle
+                    if (s_lastError == (s_cycleCounter - 1))
+                    {
+                        s_errorStrike++;
+                    }
+                    // Otherwise clear the error strike
+                    else
+                    {
+                        s_errorStrike = 0;
+                    }
+                    // Set lastError to this cycle
+                    s_lastError = s_cycleCounter;
+
+                    // If this is third error cycle in row
+                    if (s_errorStrike == 3)
+                    {
+                        ev3_speaker_play_tone(NOTE_F6, 50);
+                        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+                        // Reset counters etc.
+                        lastError = 0;
+                        errorStrike = 0;
+                        // Back up a bit
+                        motors.off(true);
+                        motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20));
+                        tslp_tsk(350);
+                        continue;
+                    }
+                }
+            }
+
+            motors.on(100, 100);
+
+            tslp_tsk(3000);
             run = false;
             motors.off(true);
         }
@@ -616,10 +750,15 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
     bool run = true;
     bool error = false;
 
+    int cycleCounter = 0;
+    int lastError = -1;
+    int errorStrike = 0;
+
     while (run)
     {
         motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
         tslp_tsk(50);
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
 
         // Reset both motor's powers
         motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
@@ -655,6 +794,7 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
             int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
             motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
             ev3_speaker_play_tone(correction * 1000, 30);
+            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
         }
         // To the right
         else if (angle > RIGHT_THRESHOLD)
@@ -663,6 +803,38 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
             int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD);
             motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
             ev3_speaker_play_tone(correction * 1000, 30);
+            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+        }
+
+        if (lPower == 0 || rPower == 0 || angle < -2)
+        {
+            // If error in previous cycle
+            if (lastError == (cycleCounter - 1))
+            {
+                errorStrike++;
+            }
+            // Otherwise clear the error strike
+            else
+            {
+                errorStrike = 0;
+            }
+            // Set lastError to this cycle
+            lastError = cycleCounter;
+
+            // If this is third error cycle in row
+            if (errorStrike == 3)
+            {
+                ev3_speaker_play_tone(NOTE_F6, 50);
+                ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+                // Reset counters etc.
+                lastError = 0;
+                errorStrike = 0;
+                // Back up a bit
+                motors.off(true);
+                motors.on(-ev3cxx::abs(motor_powers.rMotorPWR), -ev3cxx::abs(motor_powers.lMotorPWR));
+                tslp_tsk(350);
+                continue;
+            }
         }
     }
 
@@ -682,10 +854,30 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
     return !error;
 }
 
+void music_task(intptr_t unused)
+{
+    while (true)
+    {
+        ev3_speaker_play_tone(NOTE_F4, 600);
+        tslp_tsk(1600);
+        ev3_speaker_play_tone(NOTE_A4, 600);
+        tslp_tsk(1600);
+    }
+}
+
+void sub_task(intptr_t unused)
+{
+    // Sleep 89 seconds
+    tslp_tsk(89 * 1000);
+    // Terminate main task
+    ter_tsk(MAIN_TASK);
+    ter_tsk(SUB_TASK);
+}
+
 void main_task(intptr_t unused)
 {
-    const int CYCLE_LIMIT_1 = 80;
+    const int CYCLE_LIMIT_1 = 250;
-    const int CYCLE_LIMIT_2 = 80;
+    const int CYCLE_LIMIT_2 = 350;
     const int THRESHOLD = 2;
     const int TURNING_THRESHOLD = 1;
     const int TURNING_FACTOR_CORRECTION = 5;
@@ -694,7 +886,7 @@ void main_task(intptr_t unused)
     int volume = 100;
 
     // Create version info
-    const version VERSION = createVersion(85, "URSULA", 0, 7, 2);
+    const version VERSION = createVersion(85, "MONIKA", 0, 8, 0);
 
     // Set-up screen
     ev3cxx::display.resetScreen();
@@ -735,51 +927,30 @@ void main_task(intptr_t unused)
         ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
     }
 
+    ev3_speaker_set_volume(volume);
+    play_starting_melody();
+
     // 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
+    allValues values;
-    ev3_speaker_set_volume(volume);
+    values.currentVersion = VERSION;
-    ev3_speaker_play_tone(NOTE_C5, 400);
+    values.volume = volume;
-    tslp_tsk(500);
+    values.lMotorPWR = idealMPWRS.lMotorPWR;
-    ev3_speaker_play_tone(NOTE_F5, 400);
+    values.rMotorPWR = idealMPWRS.rMotorPWR;
-    tslp_tsk(500);
+    values.SPEED_MODIFIER = SPEED_MODIFIER;
-    ev3_speaker_play_tone(NOTE_G5, 400);
+    values.turningThreshold = TURNING_THRESHOLD;
-    tslp_tsk(500);
+    values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION;
-    ev3_speaker_play_tone(NOTE_A5, 100);
+    values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER;
-    tslp_tsk(200);
+    values.shortOneCycleLimit = CYCLE_LIMIT_1;
-    ev3_speaker_play_tone(NOTE_F5, 650);
+    values.loneOneCycleLimit = CYCLE_LIMIT_2;
-    tslp_tsk(950);
-    ev3_speaker_play_tone(NOTE_F4, 75);
-    tslp_tsk(110);
-    ev3_speaker_play_tone(NOTE_F4, 75);
-    tslp_tsk(110);
-    ev3_speaker_play_tone(NOTE_F4, 75);
-    tslp_tsk(200);
 
     // Start program
-    while (true)
+    start_program_exe(btnLeft, btnEnter, touchS, values);
-    {
+
-        // ev3_speaker_play_tone(NOTE_FS6, 20);
+    act_tsk(SUB_TASK);
-        // tslp_tsk(20);
+    // act_tsk(MUSIC_TASK);
-        if (btnLeft.isPressed())
-        {
-            displayAllValues(VERSION, volume, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1, CYCLE_LIMIT_2);
-        }
-        if (btnEnter.isPressed() || touchS.isPressed())
-        {
-            cleanAndTitle();
-            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
-            ev3_speaker_play_tone(NOTE_F4, 100);
-            tslp_tsk(750);
-            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
-            ev3_speaker_play_tone(NOTE_F4, 100);
-            break;
-        }
-        // ev3_speaker_play_tone(NOTE_F6, 50);
-        tslp_tsk(50);
-    }
 
     ev3_speaker_play_tone(NOTE_F4, 100);
     tslp_tsk(200);
@@ -799,15 +970,18 @@ void main_task(intptr_t unused)
         ev3_speaker_play_tone(NOTE_FS4, 100);
     }
 
+    motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25);
+
     // turn 90 degress left
-    turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+    //turn_forever(motors, gyro, 35, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+    //motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 120);
+    turn_forever(motors, gyro, 95, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+    motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
 
     // Cross to the other side
     gyro.resetHard();
     bool crossing = unlimited_drive(motors, motor_powers, gyro, frontTouch, CORRECTION_MULTIPLIER, THRESHOLD);
 
-    // turn 90 degress left
-    turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
     if (!crossing)
     {
         ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
@@ -817,6 +991,16 @@ void main_task(intptr_t unused)
         ev3_speaker_play_tone(NOTE_FS4, 100);
     }
 
+    // turn 90 degress left
+    gyro.resetHard();
+    turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+
+    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+    tslp_tsk(200);
+    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
+    tslp_tsk(200);
+    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+
     // reset gyro
     gyro.resetHard();
     // Run second long side
@@ -831,17 +1015,16 @@ void main_task(intptr_t unused)
         ev3_speaker_play_tone(NOTE_FS4, 100);
     }
 
-    // go to the middle
+    // reset gyro; turn 150 degrees left
-    motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 4.5);
+    gyro.resetHard();
-    // open door
+    turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
-    open_door(hinge);
-    motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 1);
 
-    /*bool side_2 = run_short_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER, THRESHOLD);
+    motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
-    if (!side_2)
+
-    {
+    open_door(hinge);
-        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
+
-        tslp_tsk(200);
+    motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
-        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+    motors.off(true);
-    }*/
+
+    act_tsk(MUSIC_TASK);
 }

app.h

@@ -94,6 +94,8 @@ extern "C" {
 #ifndef TOPPERS_MACRO_ONLY
 
 extern void	main_task(intptr_t);
+extern void sub_task(intptr_t);
+extern void music_task(intptr_t);
 // extern void periodic_task_1(intptr_t);
 // extern void periodic_task_2(intptr_t);