diff --git a/app b/app index 524a7b5..47657ad 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index e63e807..312d9b8 100644 --- a/app.cpp +++ b/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(1, ""); - ev3cxx::display.format(2, ""); + ev3cxx::display.format(0, "-------_2_-------"); + ev3cxx::display.format(1, "LOCL: % ") % shortOneCycleLimit; 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 -bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2) +/// @return bool: false if stopped by the middle button +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 = 90; + const int CYCLE_LIMIT_1 = 80; + 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(73, "URSULA", 0, 6, 0); + const version VERSION = createVersion(85, "URSULA", 0, 7, 2); // 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::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank 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) 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 - return; + 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); } + // 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); + }*/ }