idk denkproof

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

BIN
app

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);