diff --git a/app b/app index 7739529..ac9fa11 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index 6c6bc15..3248b33 100644 --- a/app.cpp +++ b/app.cpp @@ -4,10 +4,17 @@ #include #include #include +#include +#include /* Change notes: +0.4.3 - 'HELGA' (#) +Motor problems indicator with colors: + left - red, right - orange, + when motor is stuck change LED color + 0.4.2 - 'HELGA' (#67) Updated run_short() to run_short_side() Added gyro @@ -60,6 +67,13 @@ struct MPWRS int rMotorPWR; }; +// Structure to hold motor powers and speed modifier +struct MPWRSPlus +{ + MPWRS motor_powers; + int SPEED_MODIFIER; +}; + // Structure to hold notes struct note { @@ -100,6 +114,21 @@ void close_door(ev3cxx::Motor hinge) hinge.onForDegrees(25, 200); } +void turn(ev3cxx::MotorTank naVodu, ev3cxx::GyroSensor naklonoMer) +{ + int startAngle = naklonoMer.angle(); + int endAngle = startAngle - 90; + naVodu.onForRotations(-25, 50, 1.25); + if(ev3cxx::abs(naklonoMer.angle()) > ev3cxx::abs(endAngle) + 2 || ev3cxx::abs(naklonoMer.angle()) < ev3cxx::abs(endAngle) - 2) { // !!!! NEFUNGUJEž + 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); + } +} + /* Function to pick up all cubes on shorter side return true if ran till the end and false if stopped by middle button @@ -108,7 +137,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens { const int LEFT_THRESHOLD = -2; const int RIGHT_THRESHOLD = 2; - const int CORRECTION_MULTIPLIER = 10; + const int CORRECTION_MULTIPLIER = 20; const int CYCLE_LIMIT = 90; gyro.resetHard(); @@ -184,6 +213,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens run = false; } } + motors.off(true); ev3_speaker_play_tone(NOTE_C4, 250); tslp_tsk(250); ev3_speaker_play_tone(NOTE_C4, 125); @@ -199,19 +229,50 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens return !error; } +MPWRSPlus calculate_motor_pwrs() +{ + int batteryLevel = ev3_battery_voltage_mV(); + + double squared = (double)0.00003 * (double)(batteryLevel * batteryLevel); + double linear = -0.42 * (double)batteryLevel; + double constant = 1470; + double modifier = squared + linear + constant; + int SPEED_MODIFIER = 30 - ((int)round(modifier)); + // ev3cxx::display.format(4, "Q: % \nL: % \nC: % \nSM: % ") % squared % linear % constant % modifier; + + MPWRS idealMPWRS; + idealMPWRS.lMotorPWR = 85 + SPEED_MODIFIER; + idealMPWRS.rMotorPWR = 70 + SPEED_MODIFIER; + + MPWRSPlus retVal; + retVal.motor_powers = idealMPWRS; + retVal.SPEED_MODIFIER = SPEED_MODIFIER; + return retVal; +} + 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(67, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40); + const version VERSION = createVersion(70, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40); // Set-up screen ev3cxx::display.resetScreen(); ev3cxx::display.setFont(EV3_FONT_MEDIUM); + // Set up motor powers + MPWRSPlus calcedPWRS = calculate_motor_pwrs(); + + MPWRS idealMPWRS = calcedPWRS.motor_powers; + const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER; + + MPWRS motor_powers; + motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; + motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; + // Print version information on screen - ev3cxx::display.format(" DOBREMYSL\nVERSION: % .% .% #% \nNAME: % \nBattery: % mV") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV(); + ev3cxx::display.format(" DOBREMYSL\n% .% .% #% \nNAME: % \nBattery: % mV % \n % ") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER; ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); @@ -234,16 +295,6 @@ void main_task(intptr_t unused) ev3_speaker_play_tone(NOTE_F4, 75); tslp_tsk(200); - // Set up motor powers - const int SPEED_MODIFIER = 10; - MPWRS idealMPWRS; - idealMPWRS.lMotorPWR = 75 - SPEED_MODIFIER; - idealMPWRS.rMotorPWR = 60 - SPEED_MODIFIER; - - MPWRS motor_powers; - motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; - motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; - // 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 @@ -255,8 +306,35 @@ void main_task(intptr_t unused) // Start program btnEnter.waitForClick(); - // turn_left(motors); - bool side_1 = run_short_side(motors, motor_powers, gyro); + for (int i = 0; i < 10; i++) + { + turn(motors, gyro); + ev3_speaker_play_tone(NOTE_F4, 100); + tslp_tsk(200); + ev3_speaker_play_tone(NOTE_A4, 100); + tslp_tsk(200); + ev3_speaker_play_tone(NOTE_C4, 100); + tslp_tsk(200); + ev3_speaker_play_tone(NOTE_F4, 50); + tslp_tsk(100); + ev3_speaker_play_tone(NOTE_A4, 50); + tslp_tsk(100); + ev3_speaker_play_tone(NOTE_C4, 50); + tslp_tsk(100); + ev3_speaker_play_tone(NOTE_F4, 25); + tslp_tsk(50); + ev3_speaker_play_tone(NOTE_A4, 25); + tslp_tsk(50); + ev3_speaker_play_tone(NOTE_C4, 25); + tslp_tsk(50); + ev3_speaker_play_tone(NOTE_F4, 12); + tslp_tsk(25); + ev3_speaker_play_tone(NOTE_A4, 12); + tslp_tsk(25); + ev3_speaker_play_tone(NOTE_C4, 12); + tslp_tsk(25); + } + bool side_1 = false;//run_short_side(motors, motor_powers, gyro); if (!side_1) {