diff --git a/app b/app index 500a454..7739529 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index 1a59ab2..5118245 100644 --- a/app.cpp +++ b/app.cpp @@ -8,8 +8,11 @@ /* Change notes: -0.4.0 - 'HELGA' (#50) -Fixed +0.4.2 - 'HELGA' (#64) +Updated run_short() to run_short_side() +Added gyro +Done calculations with gyro to EC driving straight +TODO: problems with battery 0.3.0 - 'INGRID' (#42) Some weird magic in calculations of motor speeds @@ -97,7 +100,7 @@ void close_door(ev3cxx::Motor hinge) hinge.onForDegrees(25, 200); } -/* +/* Function to pick up all cubes on shorter side return true if ran till the end and false if stopped by middle button */ @@ -116,6 +119,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; bool run = true; + bool error = false; int cycleCounter = 0; while (run) @@ -123,7 +127,6 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens 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; @@ -139,58 +142,76 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens if (btnEnter.isPressed()) { run = false; - return false; + error = true; + 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); + 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); + motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction); + ev3_speaker_play_tone(correction * 1000, 30); // Check if the motor is stuck - if(lPower == 0){ + if (lPower == 0) + { ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED); - ev3_speaker_play_tone(NOTE_A5,250); + ev3_speaker_play_tone(NOTE_A5, 250); } - - // To the right - } else if (angle > RIGHT_THRESHOLD) { - //ev3_speaker_play_tone(NOTE_A4, 250); + + // 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); + motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER; + ev3_speaker_play_tone(correction * 1000, 30); // Check if the motor is stuck - if(rPower == 0){ + if (rPower == 0) + { ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); - ev3_speaker_play_tone(NOTE_A4,250); + ev3_speaker_play_tone(NOTE_A4, 250); } } cycleCounter++; - if(cycleCounter == CYCLE_LIMIT) { + if (cycleCounter == CYCLE_LIMIT) + { run = false; } } - return 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) { // 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(62, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40); + const version VERSION = createVersion(67, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40); // Set-up screen ev3cxx::display.resetScreen(); ev3cxx::display.setFont(EV3_FONT_MEDIUM); // Print version information on screen - ev3cxx::display.format(" DOBREMYSL\nVERSION: % .% .% #% \nNAME: % ") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename; + ev3cxx::display.format(" DOBREMYSL\nVERSION: % .% .% #% \nNAME: % \nBattery: % mV") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV(); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); @@ -214,10 +235,10 @@ void main_task(intptr_t unused) tslp_tsk(200); // Set up motor powers - const int SPEED_MODIFIER = 30; + const int SPEED_MODIFIER = 10; MPWRS idealMPWRS; - idealMPWRS.lMotorPWR = 70 - SPEED_MODIFIER; - idealMPWRS.rMotorPWR = 50 - SPEED_MODIFIER; + idealMPWRS.lMotorPWR = 75 - SPEED_MODIFIER; + idealMPWRS.rMotorPWR = 60 - SPEED_MODIFIER; MPWRS motor_powers; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; @@ -226,7 +247,7 @@ void main_task(intptr_t unused) // 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::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor + ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor // Set up buttons ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button @@ -234,19 +255,15 @@ void main_task(intptr_t unused) // Start program btnEnter.waitForClick(); - //turn_left(motors); + // turn_left(motors); bool side_1 = run_short_side(motors, motor_powers, gyro); - ev3_speaker_play_tone(NOTE_C4, 250); - ev3_speaker_play_tone(NOTE_C4, 125); - ev3_speaker_play_tone(NOTE_D4, 250); - ev3_speaker_play_tone(NOTE_C4, 250); - ev3_speaker_play_tone(NOTE_B4, 750); - - if(!side_1) { + if (!side_1) + { return; - } else { - ev3_speaker_play_tone(NOTE_G5, 500); + } + else + { return; } }