0.7.0 - 'URSULA'

0.7.0 - 'URSULA' (#80)
Done first short and long side
Modified turn function
Changed drive plan (long - long - middle - short - short - middle)

app.cpp

@@ -9,9 +9,18 @@
 /*
 CHANGELOG:
 
-0.6.0 - 'HELGA' ()
+0.7.2 - 'URSULA' (#)
+
+
+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 - 'HELGA' (#75)
 Added Doxygen comments
-Added volume to display_all_values (diagnostics screenS)
+Added volume to display_all_values (diagnostics screen)
 
 0.5.0 - 'HELGA' (#72)
 Added diagnostics screen (opened by pressing left after boot)
@@ -57,7 +66,7 @@ struct version
 {
     /// @brief int: version id
     int id;
-    /// @brief int: major 
+    /// @brief int: major
     int major;
     /// @brief int: minor
     int minor;
@@ -90,12 +99,11 @@ struct MPWRSPlus
     int SPEED_MODIFIER;
 };
 
-
 /// @struct note
 /// @brief Structure for holding note (frequency and duration)
 struct note
 {
-    /// @brief uint16_t: Frequency of the note 
+    /// @brief uint16_t: Frequency of the note
     uint16_t frequency;
     /// @brief int32_t: Duration of the note
     int32_t duration;
@@ -105,7 +113,7 @@ struct note
 /// @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
-MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
+MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 60)
 {
     int batteryLevel = ev3_battery_voltage_mV();
 
@@ -144,7 +152,8 @@ void cleanAndTitle()
 /// @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
-void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit)
+/// @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);
@@ -153,7 +162,7 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
     ev3cxx::BrickButton btnUp(ev3cxx::BrickButtons::UP);      // Up button
     ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN);  // Down button
 
-    int pages = 2;
+    int pages = 3;
     int page = 0;
     while (true)
     {
@@ -178,14 +187,15 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
             ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
             ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION;
             ev3cxx::display.format(6, "-------_1_-------");
-            ev3cxx::display.format(7, "SOCL: % ") % shortOneCycleLimit;
+            ev3cxx::display.format(7, "LOCL: % ") % shortOneCycleLimit;
             break;
         case 2:
             ev3cxx::display.resetScreen();
-            ev3cxx::display.format(0, "");
+            ev3cxx::display.format(0, "-------_2_-------");
-            ev3cxx::display.format(1, "");
+            ev3cxx::display.format(1, "LOCL: % ") % shortOneCycleLimit;
-            ev3cxx::display.format(2, "");
             break;
+        case 3:
+            ev3cxx::display.resetScreen();
 
         default:
             break;
@@ -241,7 +251,6 @@ std::tm parseTimestamp(const char *timestampStr)
     return tmStruct;
 }
 
-
 /// @brief Function to generate version struct of the current version
 ///        Includes getting and parsing time of compilation using parseTimestamp function
 /// @param versionID int: versionID, increment with every new change
@@ -279,13 +288,12 @@ void open_door(ev3cxx::Motor hinge)
 }
 
 /// @brief Function to close door
-/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door 
+/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door
 void close_door(ev3cxx::Motor hinge)
 {
     hinge.onForDegrees(25, 200);
 }
 
-
 /// @brief Function for turning
 ///        !! ROBOT CAN TURN ONLY LEFT !!
 /// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
@@ -295,6 +303,7 @@ void close_door(ev3cxx::Motor hinge)
 /// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
 void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
 {
+    cleanAndTitle();
     endAngle = endAngle - TFC;
     ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
 
@@ -324,6 +333,54 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
         tslp_tsk(20);
         counter++;
     }
+    if ((endAngle - THRESHOLD < ev3cxx::abs(gyro.angle())) &&
+        (ev3cxx::abs(gyro.angle()) < endAngle + THRESHOLD))
+    {
+        turn(motors, gyro, endAngle, THRESHOLD, TFC);
+    }
+    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
+    return;
+}
+
+/// @brief Function for turning (alternative way XD)
+///        !! ROBOT CAN TURN ONLY LEFT !!
+/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
+/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
+/// @param endAngle int: angle to turn to (wanted angle, will be modified internally)
+/// @param THRESHOLD int: turning accuracy in degrees (default 2)
+/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
+void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
+{
+    cleanAndTitle();
+    endAngle = endAngle - TFC;
+    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+
+    // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
+    // left, right, rotations (faster), brake, blocking, wait_after
+    motors.on(-10, 25);
+
+    bool rotating = true;
+    int counter = 0;
+    while (rotating && counter < 25000)
+    {
+        int currAngle = ev3cxx::abs(gyro.angle());
+        ev3cxx::display.format(3, "Angle: % ") % currAngle;
+        ev3cxx::display.format(4, "Counter: % ") % counter;
+        if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
+        {
+
+            int error = endAngle - currAngle;
+            ev3cxx::display.format(5, "Error: % deg.") % error;
+
+            motors.off(true);
+            rotating = false;
+            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
+            ev3_speaker_play_tone(NOTE_F5, 100);
+            return;
+        }
+        tslp_tsk(20);
+        counter++;
+    }
     ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
     return;
 }
@@ -333,12 +390,14 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
 /// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
 /// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
 /// @param gyro ev3cxx::GyroSensor: gyro sensor to use
+/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
 /// @param CYCLE_LIMIT int: how many cycle to run (default 90)
 /// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
 /// @param THRESHOLD int: when to start correcting drive (in degrees) (default 2)
-/// @return bool: false if stopped by the middle button 
+/// @return bool: false if stopped by the middle button
-bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
+bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
 {
+    cleanAndTitle();
     const int LEFT_THRESHOLD = -THRESHOLD;
     const int RIGHT_THRESHOLD = THRESHOLD;
 
@@ -377,6 +436,13 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
             motors.off(true);
         }
 
+        // Exit when bumper hit
+        if (bumper.isPressed())
+        {
+            run = false;
+            motors.off(true);
+        }
+
         // Check gyro angle and change driving speed to fix the angle
         // To the left
         if (angle < LEFT_THRESHOLD)
@@ -390,6 +456,9 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
             {
                 ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
                 ev3_speaker_play_tone(NOTE_A5, 250);
+                run = false;
+                error = true;
+                motors.off(true);
             }
 
             // To the right
@@ -405,6 +474,9 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
             {
                 ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
                 ev3_speaker_play_tone(NOTE_A4, 250);
+                run = false;
+                error = true;
+                motors.off(true);
             }
         }
 
@@ -431,10 +503,189 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
     return !error;
 }
 
+/// @brief Function to pick up all cubes on longer side.
+///        Returns true if ran till the end and false if stopped by middle button
+/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
+/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
+/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
+/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
+/// @param CYCLE_LIMIT int: how many cycle to run (default 180)
+/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
+/// @return bool: false if stopped by the middle button
+bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 180, int CORRECTION_MULTIPLIER = 20)
+{
+    cleanAndTitle();
+
+    gyro.resetHard();
+    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
+    MPWRS motor_powers;
+    // Reset both motor's powers
+    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
+    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
+
+    bool run = true;
+    bool error = false;
+    int cycleCounter = 0;
+
+    while (run)
+    {
+        motors.on(motor_powers.lMotorPWR + 20, motor_powers.rMotorPWR - 20);
+        tslp_tsk(50);
+
+        // Reset both motor's powers
+        motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
+        motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
+
+        // Get power of both motors
+        int lPower = motors.leftMotor().currentPower();
+        int rPower = motors.rightMotor().currentPower();
+        int angle = gyro.angle();
+
+        ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
+
+        if (angle < -2)
+        {
+            motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 100;
+            motor_powers.rMotorPWR = idealMPWRS.rMotorPWR - 20;
+        }
+
+        if (angle > 5)
+        {
+            motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
+            motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 40;
+        }
+
+        // Emergency break using middle button (BTN_ENTER)
+        if (btnEnter.isPressed())
+        {
+            run = false;
+            error = true;
+            motors.off(true);
+        }
+
+        // Exit when front bumper is hit
+        if (bumper.isPressed())
+        {
+            run = false;
+            motors.off(true);
+        }
+
+        cycleCounter++;
+
+        if (cycleCounter == CYCLE_LIMIT)
+        {
+            run = false;
+        }
+    }
+    motors.off(true);
+    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);
+    ev3_speaker_play_tone(NOTE_C4, 250);
+    tslp_tsk(250);
+    ev3_speaker_play_tone(NOTE_B4, 750);
+    tslp_tsk(750);
+    ev3_speaker_play_tone(NOTE_G4, 750);
+    tslp_tsk(750);
+    return !error;
+}
+
+/// @brief Function to drive forward till bumper button is pressed
+/// @param motors ev3cxx::MotorTank motors to use for driving
+/// @param idealMPWRS MPWRS ideal motor powers for the motors
+/// @param gyro ev3cxx::GyroSensor gyro sensor to use for error correction
+/// @param bumper ev3cxx::TouchSensor touch sensor to use a bumper for stoping when wall is hit
+/// @param CORRECTION_MULTIPLIER int for correcting errors in direction
+/// @return bool: false if stopped by the middle button
+bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CORRECTION_MULTIPLIER = 2, int THRESHOLD = 2)
+{
+    cleanAndTitle();
+    const int LEFT_THRESHOLD = -THRESHOLD;
+    const int RIGHT_THRESHOLD = THRESHOLD;
+
+    gyro.resetHard();
+    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
+    MPWRS motor_powers;
+    // Reset both motor's powers
+    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
+    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
+
+    bool run = true;
+    bool error = false;
+
+    while (run)
+    {
+        motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
+        tslp_tsk(50);
+
+        // Reset both motor's powers
+        motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
+        motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
+
+        // Get power of both motors
+        int lPower = motors.leftMotor().currentPower();
+        int rPower = motors.rightMotor().currentPower();
+        int angle = gyro.angle();
+
+        ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle;
+
+        // Emergency break using middle button (BTN_ENTER)
+        if (btnEnter.isPressed())
+        {
+            run = false;
+            error = true;
+            motors.off(true);
+        }
+
+        // Exit when bumper hit
+        if (bumper.isPressed())
+        {
+            run = false;
+            motors.off(true);
+        }
+
+        // Check gyro angle and change driving speed to fix the angle
+        // To the left
+        if (angle < LEFT_THRESHOLD)
+        {
+            // ev3_speaker_play_tone(NOTE_A5, 250);
+            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);
+        }
+        // To the right
+        else if (angle > RIGHT_THRESHOLD)
+        {
+            // ev3_speaker_play_tone(NOTE_A4, 250);
+            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);
+        }
+    }
+
+    motors.off(true);
+    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);
+    ev3_speaker_play_tone(NOTE_C4, 250);
+    tslp_tsk(250);
+    ev3_speaker_play_tone(NOTE_B4, 750);
+    tslp_tsk(750);
+    ev3_speaker_play_tone(NOTE_G4, 750);
+    tslp_tsk(750);
+    return !error;
+}
+
 void main_task(intptr_t unused)
 {
-
+    const int CYCLE_LIMIT_1 = 80;
-    const int CYCLE_LIMIT_1 = 90;
+    const int CYCLE_LIMIT_2 = 80;
     const int THRESHOLD = 2;
     const int TURNING_THRESHOLD = 1;
     const int TURNING_FACTOR_CORRECTION = 5;
@@ -443,8 +694,7 @@ void main_task(intptr_t unused)
     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(85, "URSULA", 0, 7, 2);
-    const version VERSION = createVersion(73, "URSULA", 0, 6, 0);
 
     // Set-up screen
     ev3cxx::display.resetScreen();
@@ -463,10 +713,11 @@ void main_task(intptr_t unused)
     ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
 
     // Set up motors
-    ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE);  // Hinge motor
+    ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::MEDIUM); // Hinge motor
-    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
+    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors (Left - B; Right - C)
     ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1);                      // gyro sensor
     ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4);                   // Touch sensor
+    ev3cxx::TouchSensor frontTouch(ev3cxx::SensorPort::S3);               // Touch sensor (bumper)
 
     // Set up buttons
     ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
@@ -514,7 +765,7 @@ void main_task(intptr_t unused)
         // tslp_tsk(20);
         if (btnLeft.isPressed())
         {
-            displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1);
+            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())
         {
@@ -533,12 +784,64 @@ void main_task(intptr_t unused)
     ev3_speaker_play_tone(NOTE_F4, 100);
     tslp_tsk(200);
 
-    bool side_1 = run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD);
+    // Close the door before begining the drive
+    close_door(hinge);
+
+    // Run first long side
+    bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER);
+    // If something happened
     if (!side_1)
     {
-        // Something went wrong
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
-        return;
+        ev3_speaker_play_tone(NOTE_F4, 100);
+        tslp_tsk(200);
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+        ev3_speaker_play_tone(NOTE_FS4, 100);
     }
 
+    // turn 90 degress left
+    turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+
+    // 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);
+        ev3_speaker_play_tone(NOTE_F4, 100);
+        tslp_tsk(200);
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+        ev3_speaker_play_tone(NOTE_FS4, 100);
+    }
+
+    // reset gyro
+    gyro.resetHard();
+    // Run second long side
+    bool side_2 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER);
+    // If something happened
+    if (!side_2)
+    {
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
+        ev3_speaker_play_tone(NOTE_F4, 100);
+        tslp_tsk(200);
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+        ev3_speaker_play_tone(NOTE_FS4, 100);
+    }
+
+    // go to the middle
+    motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 4.5);
+    // open door
+    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);
+    if (!side_2)
+    {
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
+        tslp_tsk(200);
+        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
+    }*/
 }