diff --git a/app b/app index c4f3e08..efe3816 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index 5cbd715..89e238b 100644 --- a/app.cpp +++ b/app.cpp @@ -103,10 +103,10 @@ return true if ran till the end and false if stopped by middle button */ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro) { - const int LEFT_THRESHOLD = -3; - const int RIGHT_THRESHOLD = 3; + const int LEFT_THRESHOLD = -2; + const int RIGHT_THRESHOLD = 2; const int CORRECTION_MULTIPLIER = 2; - const int CYCLE_LIMIT = 0;//75; + const int CYCLE_LIMIT = 95; gyro.resetHard(); ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button @@ -115,16 +115,11 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; - int lCounter = 0; - int rCounter = 0; - bool run = true; int cycleCounter = 0; - int lastRProblem = 0; - int lastLProblem = 0; + while (run) { - motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); tslp_tsk(50); @@ -149,66 +144,25 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens // Check gyro angle and change driving speed to fix the angle // To the left - if (angle > LEFT_THRESHOLD) { + if (angle < LEFT_THRESHOLD) { //ev3_speaker_play_tone(NOTE_A5, 250); int correction = angle - LEFT_THRESHOLD; - motor_powers.lMotorPWR += (int)pow(CORRECTION_MULTIPLIER, correction); + motor_powers.lMotorPWR += correction * CORRECTION_MULTIPLIER;//(int)pow(CORRECTION_MULTIPLIER, correction); ev3_speaker_play_tone(correction*1000,50); // Check if the motor is stuck if(lPower == 0){ ev3_speaker_play_tone(NOTE_A5,250); - - // Set lastProblem na cycleCounter because there is a problem - lastLProblem = cycleCounter; - - // Check if lastProblem was in previous cycle - if(cycleCounter - 1 == lastLProblem){ - lCounter += 1; - // If 5 problems occured than try to fix the problem - if(lCounter == 6){ - ev3_speaker_play_tone(NOTE_C4, 250); - ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED); - motors.on(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR); - tslp_tsk(250); - motors.off(); - lCounter = 0; - } - // Reset counter - } else { - lCounter = 0; - } - } // To the right - } else if (angle < RIGHT_THRESHOLD) { + } else if (angle > RIGHT_THRESHOLD) { //ev3_speaker_play_tone(NOTE_A4, 250); int correction = angle - RIGHT_THRESHOLD; - motor_powers.rMotorPWR += (int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER; + motor_powers.rMotorPWR += correction * CORRECTION_MULTIPLIER;//(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER; ev3_speaker_play_tone(correction*1000,50); // Check if the motor is stuck if(rPower == 0){ ev3_speaker_play_tone(NOTE_A4,250); - - // Set lastProblem na cycleCounter because there is a problem - lastRProblem = cycleCounter; - - // Check if lastProblem was in previous cycle - if(cycleCounter - 1 == lastRProblem){ - rCounter += 1; - // If 5 problems occured than try to fix the problem - if(rCounter == 6){ - ev3_speaker_play_tone(NOTE_C5, 250); - ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); - motors.on(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR); - tslp_tsk(250); - motors.off(); - rCounter = 0; - } - // Reset counter - } else { - rCounter = 0; - } } } @@ -227,7 +181,7 @@ 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(51, "HELGA", 0, 4, 0, 8, 11, 2023, 15, 40); + const version VERSION = createVersion(54, "HELGA", 0, 4, 1, 10, 11, 2023, 12, 10); // Set-up screen ev3cxx::display.resetScreen(); @@ -256,9 +210,10 @@ void main_task(intptr_t unused) tslp_tsk(200); // Set up motor powers + const int SPEED_MODIFIER = 30; MPWRS idealMPWRS; - idealMPWRS.lMotorPWR = 85; - idealMPWRS.rMotorPWR = 50; + idealMPWRS.lMotorPWR = 70 - SPEED_MODIFIER; + idealMPWRS.rMotorPWR = 50 - SPEED_MODIFIER; MPWRS motor_powers; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;