diff --git a/app b/app index 392e409..759ca0d 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index 6435b67..495139d 100644 --- a/app.cpp +++ b/app.cpp @@ -7,6 +7,9 @@ /* CHANGELOG: +0.8.1 - 'MONIKA' (#86) +Fixed error correction when brick stuck (changed motor speeds to fix the angle) + 0.8.0 - 'MONIKA' (#85) Error correction on every straight drive Multiple turning step @@ -103,7 +106,7 @@ struct MPWRSPlus struct note { /// @brief uint16_t: Frequency of the note - uint16_t frequency; + double frequency; /// @brief int32_t: Duration of the note int32_t duration; }; @@ -340,6 +343,7 @@ version createVersion(int versionID, const char *codename, int major, int minor, /// @param hinge ev3cxx::Motor: motor to use when opening or closing the door void open_door(ev3cxx::Motor hinge) { + hinge.off(); hinge.onForDegrees(-25, 200); } @@ -347,7 +351,10 @@ void open_door(ev3cxx::Motor hinge) /// @param hinge ev3cxx::Motor: motor to use when opening or closing the door void close_door(ev3cxx::Motor hinge) { - hinge.onForDegrees(25, 200); + hinge.off(); + hinge.onForDegrees(25, 200, false, false); + tslp_tsk(1000); + hinge.off(true); } /// @brief Function for turning @@ -408,35 +415,66 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5) { cleanAndTitle(); - endAngle = endAngle - TFC; + endAngle = endAngle + (TFC / 2); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40); // left, right, rotations (faster), brake, blocking, wait_after - motors.on(-25, 45); + motors.on(-15, 35); + + int min = endAngle - 10; + int max = endAngle + 10; - bool rotating = true; int counter = 0; - while (rotating && counter < 25000) + while (counter < 4000) { - 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 angle = ev3cxx::abs(gyro.angle()); + ev3cxx::display.format(3, "angle: % ") % angle; + + if ((min < angle) && (angle < max)) { - - int error = endAngle - currAngle; - ev3cxx::display.format(5, "Error: % deg.") % error; - + counter = 40000; motors.off(true); - rotating = false; - ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED); - ev3_speaker_play_tone(NOTE_F5, 100); - return; + tslp_tsk(400); + + if (angle > (endAngle + THRESHOLD)) + { + bool innerRun = true; + motors.on(15, 5); + while (innerRun) + { + int inner_angle = ev3cxx::abs(gyro.angle()); + ev3cxx::display.format(3, "angle: % ") % inner_angle; + if (inner_angle < (endAngle + THRESHOLD)) + { + innerRun = false; + motors.off(true); + } + tslp_tsk(50); + } + } + else if (angle < (endAngle - THRESHOLD)) + { + motors.on(-5, 15); + bool innerRun = true; + while (innerRun) + { + int inner_angle = ev3cxx::abs(gyro.angle()); + ev3cxx::display.format(3, "angle: % ") % inner_angle; + if (inner_angle > (endAngle - THRESHOLD)) + { + innerRun = false; + motors.off(true); + } + tslp_tsk(50); + } + } } - tslp_tsk(20); + ev3_speaker_play_tone(NOTE_F6, 50); + tslp_tsk(50); counter++; } + motors.off(true); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); return; } @@ -568,15 +606,18 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens /// @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) +bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 180, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2) { cleanAndTitle(); + const int LEFT_THRESHOLD = -(THRESHOLD * 3); + const int RIGHT_THRESHOLD = (THRESHOLD * 3); + 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.lMotorPWR = idealMPWRS.lMotorPWR + 10; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; bool run = true; @@ -588,7 +629,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso while (run) { ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); - motors.on(motor_powers.lMotorPWR + 20, motor_powers.rMotorPWR - 20); + motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); tslp_tsk(50); // Reset both motor's powers @@ -602,7 +643,7 @@ 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 (lPower == 0 || rPower == 0 || angle < -2) + if (lPower == 0 || rPower == 0) { // If error in previous cycle if (lastError == (cycleCounter - 1)) @@ -627,18 +668,18 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso errorStrike = 0; // Back up a bit motors.off(true); - motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20)); + motors.on(-80, -40); tslp_tsk(350); continue; } } - if (angle > 5) + if (angle < -2) { - motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; - motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 35; - ev3_speaker_play_tone(NOTE_D4, 50); - ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); + motor_powers.lMotorPWR = 100; + motor_powers.rMotorPWR = 40; + motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); + tslp_tsk(100); } // Emergency break using middle button (BTN_ENTER) @@ -655,14 +696,16 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso ev3_speaker_play_tone(NOTE_F4, 100); motors.off(true); + // Drive to the wall annd try to pickup all the bricks or get them to the container int s_cycleCounter = 0; int s_lastError = -1; int s_errorStrike = 0; - int s_maxCycles = 60; + int s_maxCycles = 70; while (s_cycleCounter <= s_maxCycles) { s_cycleCounter++; + motors.on(100, 100); int lPower = motors.leftMotor().currentPower(); int rPower = motors.rightMotor().currentPower(); if (lPower == 0 || rPower == 0) @@ -681,7 +724,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso s_lastError = s_cycleCounter; // If this is third error cycle in row - if (s_errorStrike == 3) + if (s_errorStrike == 5) { ev3_speaker_play_tone(NOTE_F6, 50); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); @@ -690,16 +733,14 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso errorStrike = 0; // Back up a bit motors.off(true); - motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20)); + motors.on(-80, -40); tslp_tsk(350); continue; } } + tslp_tsk(50); } - motors.on(100, 100); - - tslp_tsk(3000); run = false; motors.off(true); } @@ -854,29 +895,163 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen return !error; } +/** + * + * Driver functions + * + * music_task - play erika + * sub_task - watchdog; will kill main_task after 85 seconds, beep and start music_task + * main_task - main program lives here + * + */ + void music_task(intptr_t unused) { - while (true) + int PLAY_SPEED = 250; + + note erika[72]; + // + erika[0] = {NOTE_E4, 3}; + erika[1] = {NOTE_F4, 1}; + erika[2] = {NOTE_G4, 2}; + erika[3] = {NOTE_G4, 2}; + erika[4] = {NOTE_G4, 2}; + erika[5] = {NOTE_C5, 2}; + erika[6] = {NOTE_C5, 2}; + erika[7] = {NOTE_E5, 2}; + erika[8] = {NOTE_E5, 3}; + erika[9] = {NOTE_D5, 1}; + erika[10] = {NOTE_C5, 1}; + // + erika[11] = {0, 1}; + erika[12] = {0, 2}; + erika[13] = {0, 4}; + // + erika[14] = {NOTE_B4, 2}; + erika[15] = {NOTE_C5, 2}; + erika[16] = {NOTE_D5, 1}; + // + erika[17] = {0, 1}; + erika[18] = {0, 2}; + erika[19] = {0, 4}; + // + erika[20] = {NOTE_E5, 3}; + erika[21] = {NOTE_D5, 1}; + erika[22] = {NOTE_C5, 1}; + // + erika[23] = {0, 1}; + erika[24] = {0, 2}; + erika[25] = {0, 4}; + // + // + erika[26] = {NOTE_E4, 3}; + erika[27] = {NOTE_F4, 1}; + erika[28] = {NOTE_G4, 2}; + erika[29] = {NOTE_G4, 2}; + erika[30] = {NOTE_G4, 2}; + erika[31] = {NOTE_C5, 2}; + erika[32] = {NOTE_C5, 2}; + erika[33] = {NOTE_E5, 2}; + erika[34] = {NOTE_E5, 3}; + erika[35] = {NOTE_D5, 1}; + erika[36] = {NOTE_C5, 1}; + // + erika[37] = {0, 1}; + erika[38] = {0, 2}; + erika[39] = {0, 4}; + // + erika[40] = {NOTE_B4, 2}; + erika[41] = {NOTE_C5, 2}; + erika[42] = {NOTE_D5, 1}; + // + erika[43] = {0, 1}; + erika[44] = {0, 2}; + erika[45] = {0, 4}; + // + erika[46] = {NOTE_E5, 3}; + erika[47] = {NOTE_D5, 1}; + erika[48] = {NOTE_C5, 1}; + // + erika[49] = {0, 1}; + erika[50] = {0, 2}; + erika[51] = {0, 4}; + // + erika[52] = {NOTE_G4, 3}; + erika[53] = {NOTE_C5, 1}; + erika[54] = {NOTE_B4, 2}; + erika[55] = {NOTE_B4, 2}; + erika[56] = {NOTE_B4, 2}; + erika[57] = {NOTE_B4, 2}; + erika[58] = {NOTE_A4, 2}; + erika[59] = {NOTE_B4, 2}; + erika[60] = {NOTE_C5, 5}; + // + erika[61] = {0, 3}; + // + erika[62] = {NOTE_B4, 3}; + erika[63] = {NOTE_C5, 1}; + erika[64] = {NOTE_D5, 2}; + erika[65] = {NOTE_D5, 2}; + erika[66] = {NOTE_D5, 2}; + erika[67] = {NOTE_D5, 2}; + erika[68] = {NOTE_G5, 2}; + erika[69] = {NOTE_F5, 2}; + erika[70] = {NOTE_E5, 5}; + // + erika[71] = {0, 3}; + + int i = 0; + for (i = 0; i < 1000; i++) { - ev3_speaker_play_tone(NOTE_F4, 600); - tslp_tsk(1600); - ev3_speaker_play_tone(NOTE_A4, 600); - tslp_tsk(1600); + ev3_speaker_play_tone((250 + 2 * i), 2); + tslp_tsk(2); + } + + for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito-=2) + { + ev3_speaker_play_tone((250 +ito), 2); + tslp_tsk(2); + } + + for (const auto ¬e : erika) + { + if (note.frequency != 0) + { + ev3_speaker_play_tone(note.frequency, note.duration * PLAY_SPEED); + } + tslp_tsk(note.duration * PLAY_SPEED + (PLAY_SPEED / 5)); } } void sub_task(intptr_t unused) { - // Sleep 89 seconds - tslp_tsk(89 * 1000); + // Sleep 85 seconds + int time = 0; + while (time < 85) + { + time++; + ev3cxx::display.format(2, "Time: % s") % time; + tslp_tsk(1000); + } + // Make sure the volume is up and play the final beep + ev3_speaker_set_volume(100); + ev3_speaker_play_tone(NOTE_E5, 500); // Terminate main task ter_tsk(MAIN_TASK); + // Set up motors + ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::MEDIUM); // Hinge motor + ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors (Left - B; Right - C) + hinge.off(true); + motors.off(true); + // Activate music task + act_tsk(MUSIC_TASK); + // Stop self ter_tsk(SUB_TASK); } void main_task(intptr_t unused) { - const int CYCLE_LIMIT_1 = 250; + const int CYCLE_LIMIT_1 = 350; const int CYCLE_LIMIT_2 = 350; const int THRESHOLD = 2; const int TURNING_THRESHOLD = 1; @@ -886,14 +1061,14 @@ void main_task(intptr_t unused) int volume = 100; // Create version info - const version VERSION = createVersion(85, "MONIKA", 0, 8, 0); + const version VERSION = createVersion(85, "MONIKA", 0, 8, 1); // Set-up screen ev3cxx::display.resetScreen(); ev3cxx::display.setFont(EV3_FONT_MEDIUM); // Set up motor powers - MPWRSPlus calcedPWRS = calculate_motor_pwrs(); + MPWRSPlus calcedPWRS = calculate_motor_pwrs(71, 50); MPWRS idealMPWRS = calcedPWRS.motor_powers; const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER; @@ -959,7 +1134,7 @@ void main_task(intptr_t unused) close_door(hinge); // Run first long side - bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER); + bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD); // If something happened if (!side_1) { @@ -970,13 +1145,11 @@ void main_task(intptr_t unused) ev3_speaker_play_tone(NOTE_FS4, 100); } - motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25); - + gyro.resetHard(); + motors.onForRotations(-(motor_powers.rMotorPWR + 25), -motor_powers.lMotorPWR, 3.25); // turn 90 degress left - //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); + turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); + // motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000); // Cross to the other side gyro.resetHard(); @@ -1015,11 +1188,13 @@ void main_task(intptr_t unused) ev3_speaker_play_tone(NOTE_FS4, 100); } + gyro.resetHard(); + motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25); // reset gyro; turn 150 degrees left gyro.resetHard(); - turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); + turn_forever(motors, gyro, 150, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); - motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12); + motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 6); open_door(hinge);