diff --git a/app b/app index 47657ad..392e409 100644 Binary files a/app and b/app differ diff --git a/app.cfg b/app.cfg index a471044..9789e57 100644 --- a/app.cfg +++ b/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 }); diff --git a/app.cpp b/app.cpp index 312d9b8..6435b67 100644 --- a/app.cpp +++ b/app.cpp @@ -1,24 +1,23 @@ #include "ev3cxx.h" #include "app.h" -#include #include #include #include -#include /* 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 - 'HELGA' (#75) +0.6.0 - 'URSULA' (#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 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 -/// @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) +/// @param allValues values: all values in allValues structure +void displayAllValues(allValues values) { 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(3, "Nr.: #% ") % currentVersion.id; - ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename; - ev3cxx::display.format(5, "Volume: % %%") % volume; - ev3cxx::display.format(6, "Rel.: % ") % std::asctime(¤tVersion.relDate); + ev3cxx::display.format(2, "Ver.: % .% .% ") % values.currentVersion.major % values.currentVersion.minor % values.currentVersion.patch; + ev3cxx::display.format(3, "Nr.: #% ") % values.currentVersion.id; + ev3cxx::display.format(4, "Codename: % ") % values.currentVersion.codename; + ev3cxx::display.format(5, "Volume: % %%") % values.volume; + 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(4, "CM: % ") % CORRECTION_MULTIPLIER; - ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION; + ev3cxx::display.format(3, "TT: % ") % values.turningThreshold; + ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER; + 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; - motor_powers.rMotorPWR = idealMPWRS.rMotorPWR - 20; + // 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 - 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_2 = 80; + const int CYCLE_LIMIT_1 = 250; + 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 - ev3_speaker_set_volume(volume); - 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); + allValues values; + values.currentVersion = VERSION; + values.volume = volume; + values.lMotorPWR = idealMPWRS.lMotorPWR; + values.rMotorPWR = idealMPWRS.rMotorPWR; + values.SPEED_MODIFIER = SPEED_MODIFIER; + values.turningThreshold = TURNING_THRESHOLD; + values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION; + values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER; + values.shortOneCycleLimit = CYCLE_LIMIT_1; + values.loneOneCycleLimit = CYCLE_LIMIT_2; // Start program - while (true) - { - // ev3_speaker_play_tone(NOTE_FS6, 20); - // tslp_tsk(20); - 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); - } + start_program_exe(btnLeft, btnEnter, touchS, values); + + act_tsk(SUB_TASK); + // act_tsk(MUSIC_TASK); 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 - motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 4.5); - // open door - open_door(hinge); - motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 1); + // reset gyro; turn 150 degrees left + gyro.resetHard(); + turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); - /*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); - }*/ + motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12); + + open_door(hinge); + + motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12); + motors.off(true); + + act_tsk(MUSIC_TASK); } diff --git a/app.h b/app.h index a226263..0e3f931 100644 --- a/app.h +++ b/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);