blboun3/robosoutez-2023
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

0.9.0 - 'KATRIN' (#90)

Browse Source 
0.9.0 - 'KATRIN' (#90)
now I have no idea what am I doing... but it does something
This commit is contained in:
Blboun3 2023-11-20 09:58:49 +01:00
parent f73ac6eb88
commit 5dae59930a
4 changed files with 130 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -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* *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
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. 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.

BIN
app
View File

Binary file not shown.

167
app.cpp
View File

@ -7,6 +7,9 @@
/* /*
CHANGELOG: CHANGELOG:
0.9.0 - 'KATRIN' (#90)
now I have no idea what am I doing... but it does something
0.8.1 - 'MONIKA' (#86) 0.8.1 - 'MONIKA' (#86)
Fixed error correction when brick stuck (changed motor speeds to fix the angle) Fixed error correction when brick stuck (changed motor speeds to fix the angle)
@ -123,7 +126,9 @@ struct allValues
int TURNING_FACTOR_CORRECTION; int TURNING_FACTOR_CORRECTION;
int CORRECTION_MULTIPLIER; int CORRECTION_MULTIPLIER;
int shortOneCycleLimit; int shortOneCycleLimit;
int loneOneCycleLimit; int longOneCycleLimit;
int longTwoCycleLimit;
double RATIO;
}; };
/// @brief Function to calculate motor powers using quadratic equation, depending on battery powerMPWRSPlus /// @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(3, "TT: % ") % values.turningThreshold;
ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER; ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER;
ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION; ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION;
ev3cxx::display.format(6, "-------_1_-------"); ev3cxx::display.format(6, "-------_G_-------");
ev3cxx::display.format(7, "LOCL: % ") % values.shortOneCycleLimit; ev3cxx::display.format(7, "RATIO: % ") % values.RATIO;
break; break;
case 2: case 2:
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
ev3cxx::display.format(0, "-------_2_-------"); ev3cxx::display.format(0, "-------_1_-------");
ev3cxx::display.format(1, "LOCL: % ") % values.shortOneCycleLimit; ev3cxx::display.format(1, "LOCL: % ") % values.longOneCycleLimit;
ev3cxx::display.format(2, "-------_2_-------");
ev3cxx::display.format(3, "LOCL: % ") % values.longTwoCycleLimit;
break; break;
case 3: case 3:
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
@ -339,12 +346,24 @@ version createVersion(int versionID, const char *codename, int major, int minor,
return retVersion; 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 /// @brief Function to open 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 open_door(ev3cxx::Motor hinge) void open_door(ev3cxx::Motor hinge)
{ {
hinge.off(); hinge.off();
hinge.onForDegrees(-25, 200); hinge.onForDegrees(-25, 180);
} }
/// @brief Function to close door /// @brief Function to close door
@ -352,9 +371,9 @@ void open_door(ev3cxx::Motor hinge)
void close_door(ev3cxx::Motor hinge) void close_door(ev3cxx::Motor hinge)
{ {
hinge.off(); hinge.off();
hinge.onForDegrees(25, 200, false, false); ev3_motor_set_power(HINGE_MOTOR, 25);
tslp_tsk(1000); tslp_tsk(1000);
hinge.off(true); ev3_motor_set_power(HINGE_MOTOR, 0);
} }
/// @brief Function for turning /// @brief Function for turning
@ -378,7 +397,7 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
int counter = 0; int counter = 0;
while (rotating && counter < 250) 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(3, "Angle: % ") % currAngle;
ev3cxx::display.format(4, "Counter: % ") % counter; ev3cxx::display.format(4, "Counter: % ") % counter;
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle)) if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
@ -396,8 +415,8 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
tslp_tsk(20); tslp_tsk(20);
counter++; counter++;
} }
if ((endAngle - THRESHOLD < ev3cxx::abs(gyro.angle())) && if ((endAngle - THRESHOLD < ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT))) &&
(ev3cxx::abs(gyro.angle()) < endAngle + THRESHOLD)) (ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)) < endAngle + THRESHOLD))
{ {
turn(motors, gyro, endAngle, THRESHOLD, TFC); turn(motors, gyro, endAngle, THRESHOLD, TFC);
} }
@ -428,7 +447,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
int counter = 0; int counter = 0;
while (counter < 4000) 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; ev3cxx::display.format(3, "angle: % ") % angle;
if ((min < angle) && (angle < max)) if ((min < angle) && (angle < max))
@ -443,7 +462,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
motors.on(15, 5); motors.on(15, 5);
while (innerRun) 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; ev3cxx::display.format(3, "angle: % ") % inner_angle;
if (inner_angle < (endAngle + THRESHOLD)) if (inner_angle < (endAngle + THRESHOLD))
{ {
@ -459,7 +478,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
bool innerRun = true; bool innerRun = true;
while (innerRun) 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; ev3cxx::display.format(3, "angle: % ") % inner_angle;
if (inner_angle > (endAngle - THRESHOLD)) 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 // Get power of both motors
int lPower = motors.leftMotor().currentPower(); int lPower = motors.leftMotor().currentPower();
int rPower = motors.rightMotor().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; 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(); cleanAndTitle();
const int LEFT_THRESHOLD = -(THRESHOLD * 3);
const int RIGHT_THRESHOLD = (THRESHOLD * 3);
gyro.resetHard(); gyro.resetHard();
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
MPWRS motor_powers; MPWRS motor_powers;
// Reset both motor's powers // Reset both motor's powers
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 10; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
bool run = true; bool run = true;
@ -629,19 +645,21 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
while (run) while (run)
{ {
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); 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); tslp_tsk(50);
// Reset both motor's powers // Reset both motor's powers
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 5;
// Get power of both motors // Get power of both motors
int lPower = motors.leftMotor().currentPower(); int lPower = ev3_motor_get_power(LEFT_MOTOR);
int rPower = motors.rightMotor().currentPower(); int rPower = ev3_motor_get_power(RIGHT_MOTOR);
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; ev3cxx::display.format(4, "Cycles: % \nAngle: % ") % cycleCounter % angle;
if (lPower == 0 || rPower == 0) if (lPower == 0 || rPower == 0)
{ {
@ -659,9 +677,9 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
lastError = cycleCounter; lastError = cycleCounter;
// If this is third error cycle in row // 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); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
// Reset counters etc. // Reset counters etc.
lastError = 0; lastError = 0;
@ -669,17 +687,29 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
// Back up a bit // Back up a bit
motors.off(true); motors.off(true);
motors.on(-80, -40); motors.on(-80, -40);
tslp_tsk(350); tslp_tsk(450);
continue; continue;
} }
} }
if (angle < -2) // Turning left
if (angle < -3)
{ {
motor_powers.lMotorPWR = 100; // Stop the right wheel and boost left wheel
motor_powers.rMotorPWR = 40; ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(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) // 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 // Exit when front bumper is hit
if (bumper.isPressed()) if (bumper.isPressed())
{ {
ev3_speaker_play_tone(NOTE_F4, 100); ev3_speaker_play_tone(NOTE_C4, 100);
motors.off(true); motors.off(true);
// Drive to the wall annd try to pickup all the bricks or get them to the container // 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 // Back up a bit
motors.off(true); motors.off(true);
motors.on(-80, -40); motors.on(-80, -40);
tslp_tsk(350); tslp_tsk(450);
continue; continue;
} }
} }
@ -797,7 +827,8 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
while (run) 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); tslp_tsk(50);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); 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; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
// Get power of both motors // Get power of both motors
int lPower = motors.leftMotor().currentPower(); int lPower = ev3_motor_get_power(LEFT_MOTOR);
int rPower = motors.rightMotor().currentPower(); int rPower = ev3_motor_get_power(RIGHT_MOTOR);
int angle = gyro.angle(); int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle; ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle;
// Emergency break using middle button (BTN_ENTER) // 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); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
} }
if (lPower == 0 || rPower == 0 || angle < -2) if (lPower == 0 || rPower == 0)
{ {
// If error in previous cycle // If error in previous cycle
if (lastError == (cycleCounter - 1)) if (lastError == (cycleCounter - 1))
@ -909,7 +939,7 @@ void music_task(intptr_t unused)
{ {
int PLAY_SPEED = 250; int PLAY_SPEED = 250;
note erika[72]; note erika[95];
// //
erika[0] = {NOTE_E4, 3}; erika[0] = {NOTE_E4, 3};
erika[1] = {NOTE_F4, 1}; erika[1] = {NOTE_F4, 1};
@ -998,7 +1028,34 @@ void music_task(intptr_t unused)
erika[69] = {NOTE_F5, 2}; erika[69] = {NOTE_F5, 2};
erika[70] = {NOTE_E5, 5}; 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; int i = 0;
for (i = 0; i < 1000; i++) for (i = 0; i < 1000; i++)
@ -1053,6 +1110,7 @@ void main_task(intptr_t unused)
{ {
const int CYCLE_LIMIT_1 = 350; const int CYCLE_LIMIT_1 = 350;
const int CYCLE_LIMIT_2 = 350; const int CYCLE_LIMIT_2 = 350;
const double RATIO = 1.42;
const int THRESHOLD = 2; const int THRESHOLD = 2;
const int TURNING_THRESHOLD = 1; const int TURNING_THRESHOLD = 1;
const int TURNING_FACTOR_CORRECTION = 5; const int TURNING_FACTOR_CORRECTION = 5;
@ -1061,14 +1119,15 @@ void main_task(intptr_t unused)
int volume = 100; int volume = 100;
// Create version info // Create version info
const version VERSION = createVersion(85, "MONIKA", 0, 8, 1); const version VERSION = createVersion(90, "KATRIN", 0, 9, 0);
// Set-up screen // Set-up screen
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
ev3cxx::display.setFont(EV3_FONT_MEDIUM); ev3cxx::display.setFont(EV3_FONT_MEDIUM);
// Set up motor powers // 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; MPWRS idealMPWRS = calcedPWRS.motor_powers;
const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER; const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
@ -1082,7 +1141,9 @@ void main_task(intptr_t unused)
// Set up motors // Set up motors
ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::MEDIUM); // Hinge motor 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::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors (Left - B; Right - C)
ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor
ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4); // Touch sensor ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4); // Touch sensor
ev3cxx::TouchSensor frontTouch(ev3cxx::SensorPort::S3); // Touch sensor (bumper) ev3cxx::TouchSensor frontTouch(ev3cxx::SensorPort::S3); // Touch sensor (bumper)
@ -1118,14 +1179,15 @@ void main_task(intptr_t unused)
values.turningThreshold = TURNING_THRESHOLD; values.turningThreshold = TURNING_THRESHOLD;
values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION; values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION;
values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER; values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER;
values.shortOneCycleLimit = CYCLE_LIMIT_1; values.shortOneCycleLimit = 0;
values.loneOneCycleLimit = CYCLE_LIMIT_2; values.longOneCycleLimit = CYCLE_LIMIT_1;
values.longTwoCycleLimit = CYCLE_LIMIT_2;
values.RATIO = RATIO;
// Start program // Start program
start_program_exe(btnLeft, btnEnter, touchS, values); start_program_exe(btnLeft, btnEnter, touchS, values);
act_tsk(SUB_TASK); act_tsk(SUB_TASK);
// act_tsk(MUSIC_TASK);
ev3_speaker_play_tone(NOTE_F4, 100); ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(200); tslp_tsk(200);
@ -1146,8 +1208,13 @@ void main_task(intptr_t unused)
} }
gyro.resetHard(); 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 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); turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
// motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000); // 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.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
motors.off(true); motors.off(true);
act_tsk(MUSIC_TASK); tslp_tsk(120000);
} }

7
app.h
View File

@ -75,6 +75,13 @@ extern "C" {
#define PRIORITY_PRD_TSK_1 TMIN_APP_TPRI #define PRIORITY_PRD_TSK_1 TMIN_APP_TPRI
#define PRIORITY_PRD_TSK_2 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 * Task periods in ms
*/ */