idk denkproof
This commit is contained in:
Blboun3
parent
01c706619d
commit
f73ac6eb88
283
app.cpp
283
app.cpp
@ -7,6 +7,9 @@
|
||||
/*
|
||||
CHANGELOG:
|
||||
|
||||
0.8.1 - 'MONIKA' (#86)
|
||||
Fixed error correction when brick stuck (changed motor speeds to fix the angle)
|
||||
|
||||
0.8.0 - 'MONIKA' (#85)
|
||||
Error correction on every straight drive
|
||||
Multiple turning step
|
||||
@ -103,7 +106,7 @@ struct MPWRSPlus
|
||||
struct note
|
||||
{
|
||||
/// @brief uint16_t: Frequency of the note
|
||||
uint16_t frequency;
|
||||
double frequency;
|
||||
/// @brief int32_t: Duration of the note
|
||||
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
|
||||
void open_door(ev3cxx::Motor hinge)
|
||||
{
|
||||
hinge.off();
|
||||
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
|
||||
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
|
||||
@ -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)
|
||||
{
|
||||
cleanAndTitle();
|
||||
endAngle = endAngle - TFC;
|
||||
endAngle = endAngle + (TFC / 2);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
|
||||
// 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;
|
||||
while (rotating && counter < 25000)
|
||||
while (counter < 4000)
|
||||
{
|
||||
int currAngle = ev3cxx::abs(gyro.angle());
|
||||
ev3cxx::display.format(3, "Angle: % ") % currAngle;
|
||||
ev3cxx::display.format(4, "Counter: % ") % counter;
|
||||
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
|
||||
int angle = ev3cxx::abs(gyro.angle());
|
||||
ev3cxx::display.format(3, "angle: % ") % angle;
|
||||
|
||||
if ((min < angle) && (angle < max))
|
||||
{
|
||||
|
||||
int error = endAngle - currAngle;
|
||||
ev3cxx::display.format(5, "Error: % deg.") % error;
|
||||
|
||||
counter = 40000;
|
||||
motors.off(true);
|
||||
rotating = false;
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
ev3_speaker_play_tone(NOTE_F5, 100);
|
||||
return;
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
tslp_tsk(20);
|
||||
ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
tslp_tsk(50);
|
||||
counter++;
|
||||
}
|
||||
motors.off(true);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
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 CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
|
||||
/// @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();
|
||||
|
||||
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;
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 10;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
||||
|
||||
bool run = true;
|
||||
@ -588,7 +629,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
|
||||
while (run)
|
||||
{
|
||||
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);
|
||||
|
||||
// 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;
|
||||
|
||||
if (lPower == 0 || rPower == 0 || angle < -2)
|
||||
if (lPower == 0 || rPower == 0)
|
||||
{
|
||||
// If error in previous cycle
|
||||
if (lastError == (cycleCounter - 1))
|
||||
@ -627,18 +668,18 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
|
||||
errorStrike = 0;
|
||||
// Back up a bit
|
||||
motors.off(true);
|
||||
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20));
|
||||
motors.on(-80, -40);
|
||||
tslp_tsk(350);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (angle > 5)
|
||||
if (angle < -2)
|
||||
{
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 35;
|
||||
ev3_speaker_play_tone(NOTE_D4, 50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
motor_powers.lMotorPWR = 100;
|
||||
motor_powers.rMotorPWR = 40;
|
||||
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
|
||||
tslp_tsk(100);
|
||||
}
|
||||
|
||||
// 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);
|
||||
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_lastError = -1;
|
||||
int s_errorStrike = 0;
|
||||
int s_maxCycles = 60;
|
||||
int s_maxCycles = 70;
|
||||
|
||||
while (s_cycleCounter <= s_maxCycles)
|
||||
{
|
||||
s_cycleCounter++;
|
||||
motors.on(100, 100);
|
||||
int lPower = motors.leftMotor().currentPower();
|
||||
int rPower = motors.rightMotor().currentPower();
|
||||
if (lPower == 0 || rPower == 0)
|
||||
@ -681,7 +724,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
|
||||
s_lastError = s_cycleCounter;
|
||||
|
||||
// If this is third error cycle in row
|
||||
if (s_errorStrike == 3)
|
||||
if (s_errorStrike == 5)
|
||||
{
|
||||
ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
@ -690,16 +733,14 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
|
||||
errorStrike = 0;
|
||||
// Back up a bit
|
||||
motors.off(true);
|
||||
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20));
|
||||
motors.on(-80, -40);
|
||||
tslp_tsk(350);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
tslp_tsk(50);
|
||||
}
|
||||
|
||||
motors.on(100, 100);
|
||||
|
||||
tslp_tsk(3000);
|
||||
run = false;
|
||||
motors.off(true);
|
||||
}
|
||||
@ -854,29 +895,163 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
|
||||
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)
|
||||
{
|
||||
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);
|
||||
tslp_tsk(1600);
|
||||
ev3_speaker_play_tone(NOTE_A4, 600);
|
||||
tslp_tsk(1600);
|
||||
ev3_speaker_play_tone((250 + 2 * i), 2);
|
||||
tslp_tsk(2);
|
||||
}
|
||||
|
||||
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 ¬e : 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)
|
||||
{
|
||||
// Sleep 89 seconds
|
||||
tslp_tsk(89 * 1000);
|
||||
// Sleep 85 seconds
|
||||
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
|
||||
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);
|
||||
}
|
||||
|
||||
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 THRESHOLD = 2;
|
||||
const int TURNING_THRESHOLD = 1;
|
||||
@ -886,14 +1061,14 @@ void main_task(intptr_t unused)
|
||||
int volume = 100;
|
||||
|
||||
// Create version info
|
||||
const version VERSION = createVersion(85, "MONIKA", 0, 8, 0);
|
||||
const version VERSION = createVersion(85, "MONIKA", 0, 8, 1);
|
||||
|
||||
// Set-up screen
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
|
||||
|
||||
// Set up motor powers
|
||||
MPWRSPlus calcedPWRS = calculate_motor_pwrs();
|
||||
MPWRSPlus calcedPWRS = calculate_motor_pwrs(71, 50);
|
||||
|
||||
MPWRS idealMPWRS = calcedPWRS.motor_powers;
|
||||
const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
|
||||
@ -959,7 +1134,7 @@ void main_task(intptr_t unused)
|
||||
close_door(hinge);
|
||||
|
||||
// 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 (!side_1)
|
||||
{
|
||||
@ -970,13 +1145,11 @@ void main_task(intptr_t unused)
|
||||
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_forever(motors, gyro, 35, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
//motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 120);
|
||||
turn_forever(motors, gyro, 95, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
|
||||
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
// motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
|
||||
|
||||
// Cross to the other side
|
||||
gyro.resetHard();
|
||||
@ -1015,11 +1188,13 @@ void main_task(intptr_t unused)
|
||||
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
|
||||
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);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user