diff --git a/app b/app index fcf833e..524a7b5 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index c80b86b..e63e807 100644 --- a/app.cpp +++ b/app.cpp @@ -7,16 +7,22 @@ #include /* -Change notes: +CHANGELOG: -0.5.0 - 'HELGA' +0.6.0 - 'HELGA' () +Added Doxygen comments +Added volume to display_all_values (diagnostics screenS) + +0.5.0 - 'HELGA' (#72) Added diagnostics screen (opened by pressing left after boot) Added secret quiet mode +Added turning_corection_factor for correcing gyro sensor measurements due to its bad placement 0.4.3 - 'HELGA' (#70) Motor problems indicator with colors: left - red, right - orange, when motor is stuck change LED color +Added calculating motor powers based on battery level 0.4.2 - 'HELGA' (#67) Updated run_short() to run_short_side() @@ -45,45 +51,60 @@ Re-created structs for version, MPWRS, note Re-created function for opening and closing door and function for generating Version ID */ -/* 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 - ID - Increment every time with new version - CODENAME - (String) Version name - RELEASE_DATE - when was current version released -*/ +/// @struct version +/// @brief Struct for holding all informations about current version struct version { + /// @brief int: version id int id; + /// @brief int: major int major; + /// @brief int: minor int minor; + /// @brief int: patch int patch; + /// @brief std::tm: release date (date & time of compilation) std::tm relDate; + /// @brief const char*: string codename of the current version const char *codename; }; -// Structure to hold motor powers (left and right) +/// @struct MPWRS +/// @brief Structure to hold motor powers (left and right) struct MPWRS { + /// @brief int: left motor power int lMotorPWR; + /// @brief int: right motor power int rMotorPWR; }; -// Structure to hold motor powers and speed modifier +/// @struct MPWRSPlus +/// @brief Structure to hold motor powers and speed modifier (extends MPWRS) struct MPWRSPlus { + + /// @brief MPWRS: motor powers MPWRS motor_powers; + /// @brief int: speed modifier int SPEED_MODIFIER; }; -// Structure to hold notes + +/// @struct note +/// @brief Structure for holding note (frequency and duration) struct note { + /// @brief uint16_t: Frequency of the note uint16_t frequency; + /// @brief int32_t: Duration of the note int32_t duration; }; +/// @brief Function to calculate motor powers using quadratic equation, depending on battery power +/// @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) { int batteryLevel = ev3_battery_voltage_mV(); @@ -105,6 +126,7 @@ MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70) return retVal; } +/// @brief Function to clean display and write first two line void cleanAndTitle() { ev3cxx::display.resetScreen(); @@ -112,8 +134,17 @@ void cleanAndTitle() 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) +/// @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 volume: int: current set volume +/// @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 +void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit) { ev3cxx::display.resetScreen(); ev3cxx::display.setFont(EV3_FONT_MEDIUM); @@ -135,24 +166,25 @@ void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int 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(¤tVersion.relDate); - ev3cxx::display.format(7, "-----------------"); + ev3cxx::display.format(5, "Volume: % %%") % volume; + ev3cxx::display.format(6, "Rel.: % ") % std::asctime(¤tVersion.relDate); break; case 1: ev3cxx::display.resetScreen(); - ev3cxx::display.format(0, "Bat.: % mV") % batteryLevel; + ev3cxx::display.format(0, "Bat.: % mV") % ev3_battery_voltage_mV(); 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, ""); + ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION; + ev3cxx::display.format(6, "-------_1_-------"); + ev3cxx::display.format(7, "SOCL: % ") % shortOneCycleLimit; break; case 2: ev3cxx::display.resetScreen(); - ev3cxx::display.format(0, "% "); + ev3cxx::display.format(0, ""); ev3cxx::display.format(1, ""); + ev3cxx::display.format(2, ""); break; default: @@ -193,7 +225,9 @@ void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int } } -// TimeStamp parser +/// @brief Function to parse timestamp to std::tm +/// @param timestampStr const char*: string form of the timestamp +/// @return std::tm: parsed timestamp std::tm parseTimestamp(const char *timestampStr) { std::tm tmStruct = {}; @@ -207,7 +241,15 @@ std::tm parseTimestamp(const char *timestampStr) return tmStruct; } -// Function to create new program version + +/// @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 +/// @param codename const char*: string codename of the current version +/// @param major int: major +/// @param minor int: minor +/// @param patch int: patch +/// @return version: current version of the code, with date and time of compilation included version createVersion(int versionID, const char *codename, int major, int minor, int patch) { version retVersion; @@ -229,52 +271,72 @@ version createVersion(int versionID, const char *codename, int major, int minor, return retVersion; } -// Function to open door +/// @brief Function to open door +/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door void open_door(ev3cxx::Motor hinge) { hinge.onForDegrees(-25, 200); } -// Function to close door +/// @brief Function to close door +/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door void close_door(ev3cxx::Motor hinge) { hinge.onForDegrees(25, 200); } -/* -Function for turning -Robot can turn only left -*/ -void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2) + +/// @brief Function for turning +/// !! 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(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5) { + endAngle = endAngle - TFC; ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40); - motors.onForRotations(left_speed = -15, - right_speed = 30, - rotations = 2, - brake = true, - blocking = false, - wait_after_ms = 60); + // left, right, rotations (faster), brake, blocking, wait_after + motors.onForRotations(-10, 25, 1.5, true, false, 60); bool rotating = true; - while (rotating) + int counter = 0; + while (rotating && counter < 250) { int currAngle = ev3cxx::abs(gyro.angle()); - if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle) && - (currAngle < av3cxx::abs(endAngle + THRESHOLD))) + ev3cxx::display.format(3, "Angle: % ") % currAngle; + ev3cxx::display.format(4, "Counter: % ") % counter; + if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle)) { - motors.off(brake = true); + + 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; } -/* -Function to pick up all cubes on shorter side -return true if ran till the end and false if stopped by middle button -*/ +/// @brief Function to pick up all cubes on shorter 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 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 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 = -THRESHOLD; @@ -375,13 +437,14 @@ void main_task(intptr_t unused) const int CYCLE_LIMIT_1 = 90; const int THRESHOLD = 2; const int TURNING_THRESHOLD = 1; + const int TURNING_FACTOR_CORRECTION = 5; 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(72, "URSULA", 0, 5, 0); + const version VERSION = createVersion(73, "URSULA", 0, 6, 0); // Set-up screen ev3cxx::display.resetScreen(); @@ -408,10 +471,17 @@ void main_task(intptr_t unused) // 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) + ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button (quiet mode) - if(btnDown.isPressed()){ - volume = 10; + // quiet mode activation + if (btnDown.isPressed()) + { + volume = 0; + ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); + tslp_tsk(200); + ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED); + tslp_tsk(200); + ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); } // Print version information on screen @@ -438,68 +508,37 @@ void main_task(intptr_t unused) tslp_tsk(200); // Start program - while (1 == 1) + while (true) { // 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); + displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1); } 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); + // 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++) - { - 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); - 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, CYCLE_LIMIT_1, CORRECTION_MLUTIPLIER, THRESHOLD); - + bool side_1 = run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD); if (!side_1) { + // Something went wrong return; } - else - { - return; - } + + turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); }