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

idk denkproof

Browse Source
This commit is contained in:
Blboun3 2023-11-18 12:35:42 +01:00
parent 01c706619d
commit f73ac6eb88
2 changed files with 229 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
app
View File

Binary file not shown.

283
app.cpp
View File

@ -7,6 +7,9 @@
/* /*
CHANGELOG: CHANGELOG:
0.8.1 - 'MONIKA' (#86)
Fixed error correction when brick stuck (changed motor speeds to fix the angle)
0.8.0 - 'MONIKA' (#85) 0.8.0 - 'MONIKA' (#85)
Error correction on every straight drive Error correction on every straight drive
Multiple turning step Multiple turning step
@ -103,7 +106,7 @@ struct MPWRSPlus
struct note struct note
{ {
/// @brief uint16_t: Frequency of the note /// @brief uint16_t: Frequency of the note
uint16_t frequency; double frequency;
/// @brief int32_t: Duration of the note /// @brief int32_t: Duration of the note
int32_t duration; int32_t duration;
}; };
@ -340,6 +343,7 @@ version createVersion(int versionID, const char *codename, int major, int minor,
/// @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.onForDegrees(-25, 200); hinge.onForDegrees(-25, 200);
} }
@ -347,7 +351,10 @@ void open_door(ev3cxx::Motor hinge)
/// @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 close_door(ev3cxx::Motor hinge) void close_door(ev3cxx::Motor hinge)
{ {
hinge.onForDegrees(25, 200); hinge.off();
hinge.onForDegrees(25, 200, false, false);
tslp_tsk(1000);
hinge.off(true);
} }
/// @brief Function for turning /// @brief Function for turning
@ -408,35 +415,66 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5) void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
{ {
cleanAndTitle(); cleanAndTitle();
endAngle = endAngle - TFC; endAngle = endAngle + (TFC / 2);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40); // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
// left, right, rotations (faster), brake, blocking, wait_after // left, right, rotations (faster), brake, blocking, wait_after
motors.on(-25, 45); motors.on(-15, 35);
int min = endAngle - 10;
int max = endAngle + 10;
bool rotating = true;
int counter = 0; int counter = 0;
while (rotating && counter < 25000) while (counter < 4000)
{ {
int currAngle = ev3cxx::abs(gyro.angle()); int angle = ev3cxx::abs(gyro.angle());
ev3cxx::display.format(3, "Angle: % ") % currAngle; ev3cxx::display.format(3, "angle: % ") % angle;
ev3cxx::display.format(4, "Counter: % ") % counter;
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle)) if ((min < angle) && (angle < max))
{ {
counter = 40000;
int error = endAngle - currAngle; motors.off(true);
ev3cxx::display.format(5, "Error: % deg.") % error; tslp_tsk(400);
if (angle > (endAngle + THRESHOLD))
{
bool innerRun = true;
motors.on(15, 5);
while (innerRun)
{
int inner_angle = ev3cxx::abs(gyro.angle());
ev3cxx::display.format(3, "angle: % ") % inner_angle;
if (inner_angle < (endAngle + THRESHOLD))
{
innerRun = false;
motors.off(true); motors.off(true);
rotating = false;
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
ev3_speaker_play_tone(NOTE_F5, 100);
return;
} }
tslp_tsk(20); tslp_tsk(50);
}
}
else if (angle < (endAngle - THRESHOLD))
{
motors.on(-5, 15);
bool innerRun = true;
while (innerRun)
{
int inner_angle = ev3cxx::abs(gyro.angle());
ev3cxx::display.format(3, "angle: % ") % inner_angle;
if (inner_angle > (endAngle - THRESHOLD))
{
innerRun = false;
motors.off(true);
}
tslp_tsk(50);
}
}
}
ev3_speaker_play_tone(NOTE_F6, 50);
tslp_tsk(50);
counter++; counter++;
} }
motors.off(true);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
return; return;
} }
@ -568,15 +606,18 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
/// @param CYCLE_LIMIT int: how many cycle to run (default 180) /// @param CYCLE_LIMIT int: how many cycle to run (default 180)
/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20) /// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
/// @return bool: false if stopped by the middle button /// @return bool: false if stopped by the middle button
bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 180, int CORRECTION_MULTIPLIER = 20) bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 180, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
{ {
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; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 10;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
bool run = true; bool run = true;
@ -588,7 +629,7 @@ 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 + 20, motor_powers.rMotorPWR - 20); motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
tslp_tsk(50); tslp_tsk(50);
// Reset both motor's powers // Reset both motor's powers
@ -602,7 +643,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
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;
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))
@ -627,18 +668,18 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
errorStrike = 0; errorStrike = 0;
// Back up a bit // Back up a bit
motors.off(true); motors.off(true);
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20)); motors.on(-80, -40);
tslp_tsk(350); tslp_tsk(350);
continue; continue;
} }
} }
if (angle > 5) if (angle < -2)
{ {
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.lMotorPWR = 100;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 35; motor_powers.rMotorPWR = 40;
ev3_speaker_play_tone(NOTE_D4, 50); motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); tslp_tsk(100);
} }
// Emergency break using middle button (BTN_ENTER) // Emergency break using middle button (BTN_ENTER)
@ -655,14 +696,16 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
ev3_speaker_play_tone(NOTE_F4, 100); ev3_speaker_play_tone(NOTE_F4, 100);
motors.off(true); motors.off(true);
// Drive to the wall annd try to pickup all the bricks or get them to the container
int s_cycleCounter = 0; int s_cycleCounter = 0;
int s_lastError = -1; int s_lastError = -1;
int s_errorStrike = 0; int s_errorStrike = 0;
int s_maxCycles = 60; int s_maxCycles = 70;
while (s_cycleCounter <= s_maxCycles) while (s_cycleCounter <= s_maxCycles)
{ {
s_cycleCounter++; s_cycleCounter++;
motors.on(100, 100);
int lPower = motors.leftMotor().currentPower(); int lPower = motors.leftMotor().currentPower();
int rPower = motors.rightMotor().currentPower(); int rPower = motors.rightMotor().currentPower();
if (lPower == 0 || rPower == 0) if (lPower == 0 || rPower == 0)
@ -681,7 +724,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
s_lastError = s_cycleCounter; s_lastError = s_cycleCounter;
// If this is third error cycle in row // If this is third error cycle in row
if (s_errorStrike == 3) if (s_errorStrike == 5)
{ {
ev3_speaker_play_tone(NOTE_F6, 50); ev3_speaker_play_tone(NOTE_F6, 50);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
@ -690,16 +733,14 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
errorStrike = 0; errorStrike = 0;
// Back up a bit // Back up a bit
motors.off(true); motors.off(true);
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20)); motors.on(-80, -40);
tslp_tsk(350); tslp_tsk(350);
continue; continue;
} }
} }
tslp_tsk(50);
} }
motors.on(100, 100);
tslp_tsk(3000);
run = false; run = false;
motors.off(true); motors.off(true);
} }
@ -854,29 +895,163 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
return !error; return !error;
} }
/**
*
* Driver functions
*
* music_task - play erika
* sub_task - watchdog; will kill main_task after 85 seconds, beep and start music_task
* main_task - main program lives here
*
*/
void music_task(intptr_t unused) void music_task(intptr_t unused)
{ {
while (true) int PLAY_SPEED = 250;
note erika[72];
//
erika[0] = {NOTE_E4, 3};
erika[1] = {NOTE_F4, 1};
erika[2] = {NOTE_G4, 2};
erika[3] = {NOTE_G4, 2};
erika[4] = {NOTE_G4, 2};
erika[5] = {NOTE_C5, 2};
erika[6] = {NOTE_C5, 2};
erika[7] = {NOTE_E5, 2};
erika[8] = {NOTE_E5, 3};
erika[9] = {NOTE_D5, 1};
erika[10] = {NOTE_C5, 1};
//
erika[11] = {0, 1};
erika[12] = {0, 2};
erika[13] = {0, 4};
//
erika[14] = {NOTE_B4, 2};
erika[15] = {NOTE_C5, 2};
erika[16] = {NOTE_D5, 1};
//
erika[17] = {0, 1};
erika[18] = {0, 2};
erika[19] = {0, 4};
//
erika[20] = {NOTE_E5, 3};
erika[21] = {NOTE_D5, 1};
erika[22] = {NOTE_C5, 1};
//
erika[23] = {0, 1};
erika[24] = {0, 2};
erika[25] = {0, 4};
//
//
erika[26] = {NOTE_E4, 3};
erika[27] = {NOTE_F4, 1};
erika[28] = {NOTE_G4, 2};
erika[29] = {NOTE_G4, 2};
erika[30] = {NOTE_G4, 2};
erika[31] = {NOTE_C5, 2};
erika[32] = {NOTE_C5, 2};
erika[33] = {NOTE_E5, 2};
erika[34] = {NOTE_E5, 3};
erika[35] = {NOTE_D5, 1};
erika[36] = {NOTE_C5, 1};
//
erika[37] = {0, 1};
erika[38] = {0, 2};
erika[39] = {0, 4};
//
erika[40] = {NOTE_B4, 2};
erika[41] = {NOTE_C5, 2};
erika[42] = {NOTE_D5, 1};
//
erika[43] = {0, 1};
erika[44] = {0, 2};
erika[45] = {0, 4};
//
erika[46] = {NOTE_E5, 3};
erika[47] = {NOTE_D5, 1};
erika[48] = {NOTE_C5, 1};
//
erika[49] = {0, 1};
erika[50] = {0, 2};
erika[51] = {0, 4};
//
erika[52] = {NOTE_G4, 3};
erika[53] = {NOTE_C5, 1};
erika[54] = {NOTE_B4, 2};
erika[55] = {NOTE_B4, 2};
erika[56] = {NOTE_B4, 2};
erika[57] = {NOTE_B4, 2};
erika[58] = {NOTE_A4, 2};
erika[59] = {NOTE_B4, 2};
erika[60] = {NOTE_C5, 5};
//
erika[61] = {0, 3};
//
erika[62] = {NOTE_B4, 3};
erika[63] = {NOTE_C5, 1};
erika[64] = {NOTE_D5, 2};
erika[65] = {NOTE_D5, 2};
erika[66] = {NOTE_D5, 2};
erika[67] = {NOTE_D5, 2};
erika[68] = {NOTE_G5, 2};
erika[69] = {NOTE_F5, 2};
erika[70] = {NOTE_E5, 5};
//
erika[71] = {0, 3};
int i = 0;
for (i = 0; i < 1000; i++)
{ {
ev3_speaker_play_tone(NOTE_F4, 600); ev3_speaker_play_tone((250 + 2 * i), 2);
tslp_tsk(1600); tslp_tsk(2);
ev3_speaker_play_tone(NOTE_A4, 600); }
tslp_tsk(1600);
for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito-=2)
{
ev3_speaker_play_tone((250 +ito), 2);
tslp_tsk(2);
}
for (const auto &note : erika)
{
if (note.frequency != 0)
{
ev3_speaker_play_tone(note.frequency, note.duration * PLAY_SPEED);
}
tslp_tsk(note.duration * PLAY_SPEED + (PLAY_SPEED / 5));
} }
} }
void sub_task(intptr_t unused) void sub_task(intptr_t unused)
{ {
// Sleep 89 seconds // Sleep 85 seconds
tslp_tsk(89 * 1000); int time = 0;
while (time < 85)
{
time++;
ev3cxx::display.format(2, "Time: % s") % time;
tslp_tsk(1000);
}
// Make sure the volume is up and play the final beep
ev3_speaker_set_volume(100);
ev3_speaker_play_tone(NOTE_E5, 500);
// Terminate main task // Terminate main task
ter_tsk(MAIN_TASK); ter_tsk(MAIN_TASK);
// 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)
hinge.off(true);
motors.off(true);
// Activate music task
act_tsk(MUSIC_TASK);
// Stop self
ter_tsk(SUB_TASK); ter_tsk(SUB_TASK);
} }
void main_task(intptr_t unused) void main_task(intptr_t unused)
{ {
const int CYCLE_LIMIT_1 = 250; const int CYCLE_LIMIT_1 = 350;
const int CYCLE_LIMIT_2 = 350; const int CYCLE_LIMIT_2 = 350;
const int THRESHOLD = 2; const int THRESHOLD = 2;
const int TURNING_THRESHOLD = 1; const int TURNING_THRESHOLD = 1;
@ -886,14 +1061,14 @@ 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, 0); const version VERSION = createVersion(85, "MONIKA", 0, 8, 1);
// 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(); MPWRSPlus calcedPWRS = calculate_motor_pwrs(71, 50);
MPWRS idealMPWRS = calcedPWRS.motor_powers; MPWRS idealMPWRS = calcedPWRS.motor_powers;
const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER; const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
@ -959,7 +1134,7 @@ void main_task(intptr_t unused)
close_door(hinge); close_door(hinge);
// Run first long side // Run first long side
bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER); bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD);
// If something happened // If something happened
if (!side_1) if (!side_1)
{ {
@ -970,13 +1145,11 @@ void main_task(intptr_t unused)
ev3_speaker_play_tone(NOTE_FS4, 100); ev3_speaker_play_tone(NOTE_FS4, 100);
} }
motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25); gyro.resetHard();
motors.onForRotations(-(motor_powers.rMotorPWR + 25), -motor_powers.lMotorPWR, 3.25);
// turn 90 degress left // turn 90 degress left
//turn_forever(motors, gyro, 35, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
//motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 120); // motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
turn_forever(motors, gyro, 95, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
// Cross to the other side // Cross to the other side
gyro.resetHard(); gyro.resetHard();
@ -1015,11 +1188,13 @@ void main_task(intptr_t unused)
ev3_speaker_play_tone(NOTE_FS4, 100); ev3_speaker_play_tone(NOTE_FS4, 100);
} }
gyro.resetHard();
motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25);
// reset gyro; turn 150 degrees left // reset gyro; turn 150 degrees left
gyro.resetHard(); gyro.resetHard();
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION); turn_forever(motors, gyro, 150, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12); motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 6);
open_door(hinge); open_door(hinge);