0.8.0 - 'MONIKA' (#85)
0.8.0 - 'MONIKA' (#85) Error correction on every straight drive Multiple turning step Complete code for building one tower
This commit is contained in:
Blboun3
parent
482705794d
commit
01c706619d
8
app.cfg
8
app.cfg
@ -1,9 +1,13 @@
|
||||
INCLUDE("app_common.cfg");
|
||||
INCLUDE("app_common.cfg");
|
||||
|
||||
#include "app.h"
|
||||
|
||||
DOMAIN(TDOM_APP) {
|
||||
CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
|
||||
CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI, STACK_SIZE, NULL });
|
||||
|
||||
CRE_TSK(SUB_TASK, { TA_NULL, 0, sub_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
|
||||
|
||||
CRE_TSK(MUSIC_TASK, { TA_NULL, 0, music_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
|
||||
|
||||
// periodic task PRD_TSK_1 that will start automatically
|
||||
//CRE_TSK(PRD_TSK_1, { TA_NULL, 0, periodic_task_1, PRIORITY_PRD_TSK_1, STACK_SIZE, NULL });
|
||||
|
||||
365
app.cpp
365
app.cpp
@ -1,24 +1,23 @@
|
||||
#include "ev3cxx.h"
|
||||
#include "app.h"
|
||||
#include <time.h>
|
||||
#include <ctime>
|
||||
#include <math.h>
|
||||
#include <iomanip>
|
||||
#include <cstring>
|
||||
|
||||
/*
|
||||
CHANGELOG:
|
||||
|
||||
0.7.2 - 'URSULA' (#)
|
||||
|
||||
0.8.0 - 'MONIKA' (#85)
|
||||
Error correction on every straight drive
|
||||
Multiple turning step
|
||||
Complete code for building one tower
|
||||
|
||||
0.7.0 - 'URSULA' (#80)
|
||||
Done first short and long side
|
||||
Modified turn function
|
||||
Changed drive plan (long - long - middle - short - short - middle)
|
||||
|
||||
|
||||
0.6.0 - 'HELGA' (#75)
|
||||
0.6.0 - 'URSULA' (#75)
|
||||
Added Doxygen comments
|
||||
Added volume to display_all_values (diagnostics screen)
|
||||
|
||||
@ -109,7 +108,22 @@ struct note
|
||||
int32_t duration;
|
||||
};
|
||||
|
||||
/// @brief Function to calculate motor powers using quadratic equation, depending on battery power
|
||||
/// @struct all Values for diagnostic function
|
||||
struct allValues
|
||||
{
|
||||
version currentVersion;
|
||||
int volume;
|
||||
int lMotorPWR;
|
||||
int rMotorPWR;
|
||||
int SPEED_MODIFIER;
|
||||
int turningThreshold;
|
||||
int TURNING_FACTOR_CORRECTION;
|
||||
int CORRECTION_MULTIPLIER;
|
||||
int shortOneCycleLimit;
|
||||
int loneOneCycleLimit;
|
||||
};
|
||||
|
||||
/// @brief Function to calculate motor powers using quadratic equation, depending on battery powerMPWRSPlus
|
||||
/// @param leftMotor int: wanted power of left motor (default 85)
|
||||
/// @param rightMotor int: wante power or right motor (default 70)
|
||||
/// @return MPWRSPlus: motor powers and SPEED_MODIFIER
|
||||
@ -134,6 +148,28 @@ MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 60)
|
||||
return retVal;
|
||||
}
|
||||
|
||||
/// @brief Function to play starting melody
|
||||
void play_starting_melody()
|
||||
{
|
||||
// Play starting melody
|
||||
ev3_speaker_play_tone(NOTE_C5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_F5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_G5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_A5, 100);
|
||||
tslp_tsk(200);
|
||||
ev3_speaker_play_tone(NOTE_F5, 650);
|
||||
tslp_tsk(950);
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(110);
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(110);
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(200);
|
||||
}
|
||||
|
||||
/// @brief Function to clean display and write first two line
|
||||
void cleanAndTitle()
|
||||
{
|
||||
@ -143,17 +179,8 @@ void cleanAndTitle()
|
||||
}
|
||||
|
||||
/// @brief Diagnostic function to display all value on display with ability to show multiple pages
|
||||
/// @param currentVersion version: struct verion of currently defined version
|
||||
/// @param volume: int: current set volume
|
||||
/// @param lMotorPWR int: base set power for the left motor
|
||||
/// @param rMotorPWR int: base set power for the right motor
|
||||
/// @param SPEED_MODIFIER int: caluclated SPEED_MODIFIER
|
||||
/// @param turningThreshold int: turning accuracy in degrees
|
||||
/// @param TURNING_FACTOR_CORRECTION int: factor for changing final turning values due to gyro sensor placement
|
||||
/// @param CORRECTION_MULTIPLIER int: for driving straight
|
||||
/// @param shortOneCycleLimit int: how many program cycles to do when running first short side
|
||||
/// @param loneOneCycleLimit int: how many program cycles to do when running first long side
|
||||
void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit, int loneOneCycleLimit)
|
||||
/// @param allValues values: all values in allValues structure
|
||||
void displayAllValues(allValues values)
|
||||
{
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
|
||||
@ -172,27 +199,27 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, " DOBREMYSL ");
|
||||
ev3cxx::display.format(1, "*****************");
|
||||
ev3cxx::display.format(2, "Ver.: % .% .% ") % currentVersion.major % currentVersion.minor % currentVersion.patch;
|
||||
ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id;
|
||||
ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
|
||||
ev3cxx::display.format(5, "Volume: % %%") % volume;
|
||||
ev3cxx::display.format(6, "Rel.: % ") % std::asctime(¤tVersion.relDate);
|
||||
ev3cxx::display.format(2, "Ver.: % .% .% ") % values.currentVersion.major % values.currentVersion.minor % values.currentVersion.patch;
|
||||
ev3cxx::display.format(3, "Nr.: #% ") % values.currentVersion.id;
|
||||
ev3cxx::display.format(4, "Codename: % ") % values.currentVersion.codename;
|
||||
ev3cxx::display.format(5, "Volume: % %%") % values.volume;
|
||||
ev3cxx::display.format(6, "Rel.: % ") % std::asctime(&values.currentVersion.relDate);
|
||||
break;
|
||||
case 1:
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, "Bat.: % mV") % ev3_battery_voltage_mV();
|
||||
ev3cxx::display.format(1, "Mod.: % ") % SPEED_MODIFIER;
|
||||
ev3cxx::display.format(1, "Mod.: % ") % values.SPEED_MODIFIER;
|
||||
ev3cxx::display.format(2, "-------_T_-------");
|
||||
ev3cxx::display.format(3, "TT: % ") % turningThreshold;
|
||||
ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
|
||||
ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION;
|
||||
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: % ") % shortOneCycleLimit;
|
||||
ev3cxx::display.format(7, "LOCL: % ") % values.shortOneCycleLimit;
|
||||
break;
|
||||
case 2:
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, "-------_2_-------");
|
||||
ev3cxx::display.format(1, "LOCL: % ") % shortOneCycleLimit;
|
||||
ev3cxx::display.format(1, "LOCL: % ") % values.shortOneCycleLimit;
|
||||
break;
|
||||
case 3:
|
||||
ev3cxx::display.resetScreen();
|
||||
@ -235,6 +262,35 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Function to run starting phase of the program (selection of diagnostics screen)
|
||||
/// @param btnLeft ev3cxx::BrickButton: left brick button
|
||||
/// @param btnRight ev3cxx::BrickButton: right brick button
|
||||
/// @param touchS ev3cxx::TouchSensor: touch sensor
|
||||
void start_program_exe(ev3cxx::BrickButton btnLeft, ev3cxx::BrickButton btnEnter, ev3cxx::TouchSensor touchS, allValues values)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
// ev3_speaker_play_tone(NOTE_FS6, 20);
|
||||
// tslp_tsk(20);
|
||||
if (btnLeft.isPressed())
|
||||
{
|
||||
displayAllValues(values);
|
||||
}
|
||||
if (btnEnter.isPressed() || touchS.isPressed())
|
||||
{
|
||||
cleanAndTitle();
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
tslp_tsk(750);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
break;
|
||||
}
|
||||
// ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
tslp_tsk(50);
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Function to parse timestamp to std::tm
|
||||
/// @param timestampStr const char*: string form of the timestamp
|
||||
/// @return std::tm: parsed timestamp
|
||||
@ -357,7 +413,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
|
||||
|
||||
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
|
||||
// left, right, rotations (faster), brake, blocking, wait_after
|
||||
motors.on(-10, 25);
|
||||
motors.on(-25, 45);
|
||||
|
||||
bool rotating = true;
|
||||
int counter = 0;
|
||||
@ -526,9 +582,12 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
|
||||
bool run = true;
|
||||
bool error = false;
|
||||
int cycleCounter = 0;
|
||||
int lastError = -1;
|
||||
int errorStrike = 0;
|
||||
|
||||
while (run)
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
motors.on(motor_powers.lMotorPWR + 20, motor_powers.rMotorPWR - 20);
|
||||
tslp_tsk(50);
|
||||
|
||||
@ -543,16 +602,43 @@ 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 (angle < -2)
|
||||
if (lPower == 0 || rPower == 0 || angle < -2)
|
||||
{
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 100;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR - 20;
|
||||
// If error in previous cycle
|
||||
if (lastError == (cycleCounter - 1))
|
||||
{
|
||||
errorStrike++;
|
||||
}
|
||||
// Otherwise clear the error strike
|
||||
else
|
||||
{
|
||||
errorStrike = 0;
|
||||
}
|
||||
// Set lastError to this cycle
|
||||
lastError = cycleCounter;
|
||||
|
||||
// If this is third error cycle in row
|
||||
if (errorStrike == 3)
|
||||
{
|
||||
ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
// Reset counters etc.
|
||||
lastError = 0;
|
||||
errorStrike = 0;
|
||||
// Back up a bit
|
||||
motors.off(true);
|
||||
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20));
|
||||
tslp_tsk(350);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (angle > 5)
|
||||
{
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 40;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 35;
|
||||
ev3_speaker_play_tone(NOTE_D4, 50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
}
|
||||
|
||||
// Emergency break using middle button (BTN_ENTER)
|
||||
@ -566,6 +652,54 @@ 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);
|
||||
motors.off(true);
|
||||
|
||||
int s_cycleCounter = 0;
|
||||
int s_lastError = -1;
|
||||
int s_errorStrike = 0;
|
||||
int s_maxCycles = 60;
|
||||
|
||||
while (s_cycleCounter <= s_maxCycles)
|
||||
{
|
||||
s_cycleCounter++;
|
||||
int lPower = motors.leftMotor().currentPower();
|
||||
int rPower = motors.rightMotor().currentPower();
|
||||
if (lPower == 0 || rPower == 0)
|
||||
{
|
||||
// If error in previous cycle
|
||||
if (s_lastError == (s_cycleCounter - 1))
|
||||
{
|
||||
s_errorStrike++;
|
||||
}
|
||||
// Otherwise clear the error strike
|
||||
else
|
||||
{
|
||||
s_errorStrike = 0;
|
||||
}
|
||||
// Set lastError to this cycle
|
||||
s_lastError = s_cycleCounter;
|
||||
|
||||
// If this is third error cycle in row
|
||||
if (s_errorStrike == 3)
|
||||
{
|
||||
ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
// Reset counters etc.
|
||||
lastError = 0;
|
||||
errorStrike = 0;
|
||||
// Back up a bit
|
||||
motors.off(true);
|
||||
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR - 20), -ev3cxx::abs(motor_powers.lMotorPWR + 20));
|
||||
tslp_tsk(350);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
motors.on(100, 100);
|
||||
|
||||
tslp_tsk(3000);
|
||||
run = false;
|
||||
motors.off(true);
|
||||
}
|
||||
@ -616,10 +750,15 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
|
||||
bool run = true;
|
||||
bool error = false;
|
||||
|
||||
int cycleCounter = 0;
|
||||
int lastError = -1;
|
||||
int errorStrike = 0;
|
||||
|
||||
while (run)
|
||||
{
|
||||
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
|
||||
tslp_tsk(50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
// Reset both motor's powers
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
@ -655,6 +794,7 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
|
||||
int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
|
||||
motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
|
||||
ev3_speaker_play_tone(correction * 1000, 30);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
}
|
||||
// To the right
|
||||
else if (angle > RIGHT_THRESHOLD)
|
||||
@ -663,6 +803,38 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
|
||||
int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD);
|
||||
motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
|
||||
ev3_speaker_play_tone(correction * 1000, 30);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
}
|
||||
|
||||
if (lPower == 0 || rPower == 0 || angle < -2)
|
||||
{
|
||||
// If error in previous cycle
|
||||
if (lastError == (cycleCounter - 1))
|
||||
{
|
||||
errorStrike++;
|
||||
}
|
||||
// Otherwise clear the error strike
|
||||
else
|
||||
{
|
||||
errorStrike = 0;
|
||||
}
|
||||
// Set lastError to this cycle
|
||||
lastError = cycleCounter;
|
||||
|
||||
// If this is third error cycle in row
|
||||
if (errorStrike == 3)
|
||||
{
|
||||
ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
// Reset counters etc.
|
||||
lastError = 0;
|
||||
errorStrike = 0;
|
||||
// Back up a bit
|
||||
motors.off(true);
|
||||
motors.on(-ev3cxx::abs(motor_powers.rMotorPWR), -ev3cxx::abs(motor_powers.lMotorPWR));
|
||||
tslp_tsk(350);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -682,10 +854,30 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
|
||||
return !error;
|
||||
}
|
||||
|
||||
void music_task(intptr_t unused)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
ev3_speaker_play_tone(NOTE_F4, 600);
|
||||
tslp_tsk(1600);
|
||||
ev3_speaker_play_tone(NOTE_A4, 600);
|
||||
tslp_tsk(1600);
|
||||
}
|
||||
}
|
||||
|
||||
void sub_task(intptr_t unused)
|
||||
{
|
||||
// Sleep 89 seconds
|
||||
tslp_tsk(89 * 1000);
|
||||
// Terminate main task
|
||||
ter_tsk(MAIN_TASK);
|
||||
ter_tsk(SUB_TASK);
|
||||
}
|
||||
|
||||
void main_task(intptr_t unused)
|
||||
{
|
||||
const int CYCLE_LIMIT_1 = 80;
|
||||
const int CYCLE_LIMIT_2 = 80;
|
||||
const int CYCLE_LIMIT_1 = 250;
|
||||
const int CYCLE_LIMIT_2 = 350;
|
||||
const int THRESHOLD = 2;
|
||||
const int TURNING_THRESHOLD = 1;
|
||||
const int TURNING_FACTOR_CORRECTION = 5;
|
||||
@ -694,7 +886,7 @@ void main_task(intptr_t unused)
|
||||
int volume = 100;
|
||||
|
||||
// Create version info
|
||||
const version VERSION = createVersion(85, "URSULA", 0, 7, 2);
|
||||
const version VERSION = createVersion(85, "MONIKA", 0, 8, 0);
|
||||
|
||||
// Set-up screen
|
||||
ev3cxx::display.resetScreen();
|
||||
@ -735,51 +927,30 @@ void main_task(intptr_t unused)
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
}
|
||||
|
||||
ev3_speaker_set_volume(volume);
|
||||
play_starting_melody();
|
||||
|
||||
// Print version information on screen
|
||||
cleanAndTitle();
|
||||
ev3cxx::display.format(2, "% .% .% #% \nNAME: % \nBattery: % mV % \nPress ENTR to run") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER;
|
||||
|
||||
// Play starting melody
|
||||
ev3_speaker_set_volume(volume);
|
||||
ev3_speaker_play_tone(NOTE_C5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_F5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_G5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_A5, 100);
|
||||
tslp_tsk(200);
|
||||
ev3_speaker_play_tone(NOTE_F5, 650);
|
||||
tslp_tsk(950);
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(110);
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(110);
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(200);
|
||||
allValues values;
|
||||
values.currentVersion = VERSION;
|
||||
values.volume = volume;
|
||||
values.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
values.rMotorPWR = idealMPWRS.rMotorPWR;
|
||||
values.SPEED_MODIFIER = SPEED_MODIFIER;
|
||||
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;
|
||||
|
||||
// Start program
|
||||
while (true)
|
||||
{
|
||||
// ev3_speaker_play_tone(NOTE_FS6, 20);
|
||||
// tslp_tsk(20);
|
||||
if (btnLeft.isPressed())
|
||||
{
|
||||
displayAllValues(VERSION, volume, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1, CYCLE_LIMIT_2);
|
||||
}
|
||||
if (btnEnter.isPressed() || touchS.isPressed())
|
||||
{
|
||||
cleanAndTitle();
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
tslp_tsk(750);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
break;
|
||||
}
|
||||
// ev3_speaker_play_tone(NOTE_F6, 50);
|
||||
tslp_tsk(50);
|
||||
}
|
||||
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);
|
||||
@ -799,15 +970,18 @@ void main_task(intptr_t unused)
|
||||
ev3_speaker_play_tone(NOTE_FS4, 100);
|
||||
}
|
||||
|
||||
motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25);
|
||||
|
||||
// turn 90 degress left
|
||||
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
//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);
|
||||
|
||||
// Cross to the other side
|
||||
gyro.resetHard();
|
||||
bool crossing = unlimited_drive(motors, motor_powers, gyro, frontTouch, CORRECTION_MULTIPLIER, THRESHOLD);
|
||||
|
||||
// turn 90 degress left
|
||||
turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
if (!crossing)
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
@ -817,6 +991,16 @@ void main_task(intptr_t unused)
|
||||
ev3_speaker_play_tone(NOTE_FS4, 100);
|
||||
}
|
||||
|
||||
// turn 90 degress left
|
||||
gyro.resetHard();
|
||||
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
// reset gyro
|
||||
gyro.resetHard();
|
||||
// Run second long side
|
||||
@ -831,17 +1015,16 @@ void main_task(intptr_t unused)
|
||||
ev3_speaker_play_tone(NOTE_FS4, 100);
|
||||
}
|
||||
|
||||
// go to the middle
|
||||
motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 4.5);
|
||||
// open door
|
||||
open_door(hinge);
|
||||
motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 1);
|
||||
// reset gyro; turn 150 degrees left
|
||||
gyro.resetHard();
|
||||
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
|
||||
/*bool side_2 = run_short_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER, THRESHOLD);
|
||||
if (!side_2)
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
}*/
|
||||
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
|
||||
|
||||
open_door(hinge);
|
||||
|
||||
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
|
||||
motors.off(true);
|
||||
|
||||
act_tsk(MUSIC_TASK);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user