diff --git a/README.md b/README.md index 501e3bf..343f820 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ Members: *Postavit a naprogramovat robota tak, aby samostatně, bez jakékoli další pomoci sebral co nejvíc barevných kostek ze soutěžní plochy (hřiště) a umístil je dovnitř žlutě vyznačeného obdélníku na hracím hřišti a současně postavil z těchto barevných kostek co nejvyšší věž. Pro snazší orientaci jsou na hřišti vyznačeny naváděcí černé čáry. O vítězství rozhoduje počet získaných bodů. Body robot získává za způsob umístění barevných kostek uvnitř žlutě vyznačeného obdélníku. Většina soutěže bude organizována jako vzájemný zápas dvou robotů na dvou samostatných hřištích* -zdroj: https://robosoutez.fel.cvut.cz/zadani-soutezni-ulohy-vez-tower +zdroj: [https://robosoutez.fel.cvut.cz/zadani-soutezni-ulohy-vez-tower](https://robosoutez.fel.cvut.cz/zadani-soutezni-ulohy-vez-tower) ## > Konstrukce Konstrukce se zásobníkem na 8 kostek, a pásem na přepravu kostek do zásobníku. Jeden motor pohání pás a levé kolo, druhý motor pohání přední kola k nabírání kostek a pravé kolo. Servo ovládá zadní dveře. diff --git a/app b/app index 759ca0d..233f02d 100644 Binary files a/app and b/app differ diff --git a/app.cpp b/app.cpp index 495139d..a138e1b 100644 --- a/app.cpp +++ b/app.cpp @@ -7,6 +7,9 @@ /* CHANGELOG: +0.9.0 - 'KATRIN' (#90) +now I have no idea what am I doing... but it does something + 0.8.1 - 'MONIKA' (#86) Fixed error correction when brick stuck (changed motor speeds to fix the angle) @@ -123,7 +126,9 @@ struct allValues int TURNING_FACTOR_CORRECTION; int CORRECTION_MULTIPLIER; int shortOneCycleLimit; - int loneOneCycleLimit; + int longOneCycleLimit; + int longTwoCycleLimit; + double RATIO; }; /// @brief Function to calculate motor powers using quadratic equation, depending on battery powerMPWRSPlus @@ -216,13 +221,15 @@ void displayAllValues(allValues values) ev3cxx::display.format(3, "TT: % ") % values.turningThreshold; ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER; ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION; - ev3cxx::display.format(6, "-------_1_-------"); - ev3cxx::display.format(7, "LOCL: % ") % values.shortOneCycleLimit; + ev3cxx::display.format(6, "-------_G_-------"); + ev3cxx::display.format(7, "RATIO: % ") % values.RATIO; break; case 2: ev3cxx::display.resetScreen(); - ev3cxx::display.format(0, "-------_2_-------"); - ev3cxx::display.format(1, "LOCL: % ") % values.shortOneCycleLimit; + ev3cxx::display.format(0, "-------_1_-------"); + ev3cxx::display.format(1, "LOCL: % ") % values.longOneCycleLimit; + ev3cxx::display.format(2, "-------_2_-------"); + ev3cxx::display.format(3, "LOCL: % ") % values.longTwoCycleLimit; break; case 3: ev3cxx::display.resetScreen(); @@ -339,12 +346,24 @@ version createVersion(int versionID, const char *codename, int major, int minor, return retVersion; } +/// @brief Function to calculate faster motor speed by using ratio and slower motor speed +/// @param slower_motor_speed int: slower motor speed +/// @param RATIO double: ration betwenn faster and slower wheel +/// @return int[2]: [slower motor speed, faster motor speed] +int* calculate_ratio(int slower_motor_speed, double RATIO) +{ + int* retVal = new int[2]; + retVal[0] = slower_motor_speed; + retVal[1] = (int)round(slower_motor_speed * RATIO); + return retVal; +} + /// @brief Function to open door /// @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); + hinge.onForDegrees(-25, 180); } /// @brief Function to close door @@ -352,9 +371,9 @@ void open_door(ev3cxx::Motor hinge) void close_door(ev3cxx::Motor hinge) { hinge.off(); - hinge.onForDegrees(25, 200, false, false); + ev3_motor_set_power(HINGE_MOTOR, 25); tslp_tsk(1000); - hinge.off(true); + ev3_motor_set_power(HINGE_MOTOR, 0); } /// @brief Function for turning @@ -378,7 +397,7 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int counter = 0; while (rotating && counter < 250) { - int currAngle = ev3cxx::abs(gyro.angle()); + int currAngle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)); ev3cxx::display.format(3, "Angle: % ") % currAngle; ev3cxx::display.format(4, "Counter: % ") % counter; if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle)) @@ -396,8 +415,8 @@ 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)) + if ((endAngle - THRESHOLD < ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT))) && + (ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)) < endAngle + THRESHOLD)) { turn(motors, gyro, endAngle, THRESHOLD, TFC); } @@ -428,7 +447,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl int counter = 0; while (counter < 4000) { - int angle = ev3cxx::abs(gyro.angle()); + int angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)); ev3cxx::display.format(3, "angle: % ") % angle; if ((min < angle) && (angle < max)) @@ -443,7 +462,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl motors.on(15, 5); while (innerRun) { - int inner_angle = ev3cxx::abs(gyro.angle()); + int inner_angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)); ev3cxx::display.format(3, "angle: % ") % inner_angle; if (inner_angle < (endAngle + THRESHOLD)) { @@ -459,7 +478,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl bool innerRun = true; while (innerRun) { - int inner_angle = ev3cxx::abs(gyro.angle()); + int inner_angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)); ev3cxx::display.format(3, "angle: % ") % inner_angle; if (inner_angle > (endAngle - THRESHOLD)) { @@ -518,7 +537,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens // Get power of both motors int lPower = motors.leftMotor().currentPower(); int rPower = motors.rightMotor().currentPower(); - int angle = gyro.angle(); + int angle = ev3_gyro_sensor_get_angle(GYRO_PORT); ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle; @@ -610,14 +629,11 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso { 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 + 10; + motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; bool run = true; @@ -629,19 +645,21 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso while (run) { ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); - motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); + // motors.on(motor_powers.lMotorPWR + 10, motor_powers.rMotorPWR); + ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR); + ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR); tslp_tsk(50); // Reset both motor's powers motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; - motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; + motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 5; // Get power of both motors - int lPower = motors.leftMotor().currentPower(); - int rPower = motors.rightMotor().currentPower(); - int angle = gyro.angle(); + int lPower = ev3_motor_get_power(LEFT_MOTOR); + int rPower = ev3_motor_get_power(RIGHT_MOTOR); + int angle = ev3_gyro_sensor_get_angle(GYRO_PORT); - ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle; + ev3cxx::display.format(4, "Cycles: % \nAngle: % ") % cycleCounter % angle; if (lPower == 0 || rPower == 0) { @@ -659,9 +677,9 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso lastError = cycleCounter; // If this is third error cycle in row - if (errorStrike == 3) + if (errorStrike == 5) { - ev3_speaker_play_tone(NOTE_F6, 50); + ev3_speaker_play_tone(NOTE_G5, 50); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); // Reset counters etc. lastError = 0; @@ -669,17 +687,29 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso // Back up a bit motors.off(true); motors.on(-80, -40); - tslp_tsk(350); + tslp_tsk(450); continue; } } - if (angle < -2) + // Turning left + if (angle < -3) { - motor_powers.lMotorPWR = 100; - motor_powers.rMotorPWR = 40; - motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); - tslp_tsk(100); + // Stop the right wheel and boost left wheel + ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); + ev3_speaker_play_tone(NOTE_F4, 100); + motor_powers.lMotorPWR += 30; + motor_powers.rMotorPWR -= 20; + } + + // Turning right + if (angle > 5) + { + // Stop the left wheel and boost right wheel + ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED); + ev3_speaker_play_tone(NOTE_F6, 100); + motor_powers.lMotorPWR -= 10; + motor_powers.rMotorPWR += 30; } // Emergency break using middle button (BTN_ENTER) @@ -693,7 +723,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso // Exit when front bumper is hit if (bumper.isPressed()) { - ev3_speaker_play_tone(NOTE_F4, 100); + ev3_speaker_play_tone(NOTE_C4, 100); motors.off(true); // Drive to the wall annd try to pickup all the bricks or get them to the container @@ -734,7 +764,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso // Back up a bit motors.off(true); motors.on(-80, -40); - tslp_tsk(350); + tslp_tsk(450); continue; } } @@ -797,7 +827,8 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen while (run) { - motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); + ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR); + ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR); tslp_tsk(50); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); @@ -806,10 +837,9 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; // Get power of both motors - int lPower = motors.leftMotor().currentPower(); - int rPower = motors.rightMotor().currentPower(); - int angle = gyro.angle(); - + int lPower = ev3_motor_get_power(LEFT_MOTOR); + int rPower = ev3_motor_get_power(RIGHT_MOTOR); + int angle = ev3_gyro_sensor_get_angle(GYRO_PORT); ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle; // Emergency break using middle button (BTN_ENTER) @@ -847,7 +877,7 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); } - if (lPower == 0 || rPower == 0 || angle < -2) + if (lPower == 0 || rPower == 0) { // If error in previous cycle if (lastError == (cycleCounter - 1)) @@ -909,7 +939,7 @@ void music_task(intptr_t unused) { int PLAY_SPEED = 250; - note erika[72]; + note erika[95]; // erika[0] = {NOTE_E4, 3}; erika[1] = {NOTE_F4, 1}; @@ -998,7 +1028,34 @@ void music_task(intptr_t unused) erika[69] = {NOTE_F5, 2}; erika[70] = {NOTE_E5, 5}; // - erika[71] = {0, 3}; + erika[71] = {NOTE_E4, 3}; + erika[72] = {NOTE_F4, 1}; + erika[73] = {NOTE_G4, 2}; + erika[74] = {NOTE_G4, 2}; + erika[75] = {NOTE_G4, 2}; + erika[76] = {NOTE_C5, 2}; + erika[77] = {NOTE_C5, 2}; + erika[78] = {NOTE_E5, 2}; + erika[79] = {NOTE_E5, 3}; + erika[80] = {NOTE_D5, 1}; + erika[81] = {NOTE_C5, 1}; + // + erika[82] = {0, 1}; + erika[83] = {0, 2}; + erika[84] = {0, 4}; + // + erika[85] = {NOTE_B4, 2}; + erika[86] = {NOTE_C5, 2}; + erika[87] = {NOTE_D5, 1}; + // + erika[88] = {0, 1}; + erika[89] = {0, 2}; + erika[90] = {0, 4}; + // + erika[91] = {NOTE_E5, 3}; + erika[92] = {NOTE_D5, 1}; + erika[93] = {NOTE_C5, 1}; + erika[94] = {0, 3}; int i = 0; for (i = 0; i < 1000; i++) @@ -1007,9 +1064,9 @@ void music_task(intptr_t unused) tslp_tsk(2); } - for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito-=2) + for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito -= 2) { - ev3_speaker_play_tone((250 +ito), 2); + ev3_speaker_play_tone((250 + ito), 2); tslp_tsk(2); } @@ -1053,6 +1110,7 @@ void main_task(intptr_t unused) { const int CYCLE_LIMIT_1 = 350; const int CYCLE_LIMIT_2 = 350; + const double RATIO = 1.42; const int THRESHOLD = 2; const int TURNING_THRESHOLD = 1; const int TURNING_FACTOR_CORRECTION = 5; @@ -1061,14 +1119,15 @@ void main_task(intptr_t unused) int volume = 100; // Create version info - const version VERSION = createVersion(85, "MONIKA", 0, 8, 1); + const version VERSION = createVersion(90, "KATRIN", 0, 9, 0); // Set-up screen ev3cxx::display.resetScreen(); ev3cxx::display.setFont(EV3_FONT_MEDIUM); // Set up motor powers - MPWRSPlus calcedPWRS = calculate_motor_pwrs(71, 50); + int* rationalized_speeds = calculate_ratio(45, RATIO); + MPWRSPlus calcedPWRS = calculate_motor_pwrs(rationalized_speeds[1], rationalized_speeds[0]); MPWRS idealMPWRS = calcedPWRS.motor_powers; const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER; @@ -1082,9 +1141,11 @@ void main_task(intptr_t unused) // 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) - 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) + + 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 @@ -1118,14 +1179,15 @@ void main_task(intptr_t unused) values.turningThreshold = TURNING_THRESHOLD; values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION; values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER; - values.shortOneCycleLimit = CYCLE_LIMIT_1; - values.loneOneCycleLimit = CYCLE_LIMIT_2; + values.shortOneCycleLimit = 0; + values.longOneCycleLimit = CYCLE_LIMIT_1; + values.longTwoCycleLimit = CYCLE_LIMIT_2; + values.RATIO = RATIO; // Start program start_program_exe(btnLeft, btnEnter, touchS, values); act_tsk(SUB_TASK); - // act_tsk(MUSIC_TASK); ev3_speaker_play_tone(NOTE_F4, 100); tslp_tsk(200); @@ -1146,8 +1208,13 @@ void main_task(intptr_t unused) } gyro.resetHard(); - motors.onForRotations(-(motor_powers.rMotorPWR + 25), -motor_powers.lMotorPWR, 3.25); + motors.onForRotations(-(motor_powers.rMotorPWR + 25), -(motor_powers.lMotorPWR - 25), 3.25); // turn 90 degress left + turn_forever(motors, gyro, 30, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); + ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR); + ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR); + tslp_tsk(250); + motors.off(true); turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); // motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000); @@ -1201,5 +1268,5 @@ void main_task(intptr_t unused) motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12); motors.off(true); - act_tsk(MUSIC_TASK); + tslp_tsk(120000); } diff --git a/app.h b/app.h index 0e3f931..1b9c571 100644 --- a/app.h +++ b/app.h @@ -75,6 +75,13 @@ extern "C" { #define PRIORITY_PRD_TSK_1 TMIN_APP_TPRI #define PRIORITY_PRD_TSK_2 TMIN_APP_TPRI +#define GYRO_PORT EV3_PORT_1 +#define BUTTON_UP EV3_PORT_4 +#define BUTTON_BUMPER EV3_PORT_3 +#define LEFT_MOTOR EV3_PORT_B +#define RIGHT_MOTOR EV3_PORT_C +#define HINGE_MOTOR EV3_PORT_A + /** * Task periods in ms */