2023-11-09 15:24:40 +01:00
|
|
|
#include "ev3cxx.h"
|
|
|
|
#include "app.h"
|
|
|
|
#include <ctime>
|
2023-11-10 10:07:39 +01:00
|
|
|
#include <math.h>
|
2023-11-14 16:45:45 +01:00
|
|
|
#include <iomanip>
|
2023-11-09 15:24:40 +01:00
|
|
|
|
|
|
|
/*
|
2023-11-15 08:44:48 +01:00
|
|
|
CHANGELOG:
|
2023-11-09 15:24:40 +01:00
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
0.8.0 - 'MONIKA' (#85)
|
|
|
|
Error correction on every straight drive
|
|
|
|
Multiple turning step
|
|
|
|
Complete code for building one tower
|
2023-11-16 09:07:53 +01:00
|
|
|
|
|
|
|
0.7.0 - 'URSULA' (#80)
|
|
|
|
Done first short and long side
|
|
|
|
Modified turn function
|
|
|
|
Changed drive plan (long - long - middle - short - short - middle)
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
0.6.0 - 'URSULA' (#75)
|
2023-11-15 08:44:48 +01:00
|
|
|
Added Doxygen comments
|
2023-11-16 09:07:53 +01:00
|
|
|
Added volume to display_all_values (diagnostics screen)
|
2023-11-15 08:44:48 +01:00
|
|
|
|
|
|
|
0.5.0 - 'HELGA' (#72)
|
2023-11-14 16:45:45 +01:00
|
|
|
Added diagnostics screen (opened by pressing left after boot)
|
|
|
|
Added secret quiet mode
|
2023-11-15 08:44:48 +01:00
|
|
|
Added turning_corection_factor for correcing gyro sensor measurements due to its bad placement
|
2023-11-14 16:45:45 +01:00
|
|
|
|
|
|
|
0.4.3 - 'HELGA' (#70)
|
2023-11-14 11:14:18 +01:00
|
|
|
Motor problems indicator with colors:
|
|
|
|
left - red, right - orange,
|
|
|
|
when motor is stuck change LED color
|
2023-11-15 08:44:48 +01:00
|
|
|
Added calculating motor powers based on battery level
|
2023-11-14 11:14:18 +01:00
|
|
|
|
2023-11-10 15:51:25 +01:00
|
|
|
0.4.2 - 'HELGA' (#67)
|
2023-11-10 15:43:21 +01:00
|
|
|
Updated run_short() to run_short_side()
|
|
|
|
Added gyro
|
|
|
|
Done calculations with gyro to EC driving straight
|
|
|
|
TODO: problems with battery
|
2023-11-09 15:24:40 +01:00
|
|
|
|
|
|
|
0.3.0 - 'INGRID' (#42)
|
|
|
|
Some weird magic in calculations of motor speeds
|
|
|
|
|
|
|
|
0.2.7 - 'ERIKA' (#36)
|
|
|
|
Created function run_short for picking up all cubes on short sides
|
|
|
|
|
|
|
|
0.2.5 - 'ERIKA' (#27)
|
|
|
|
Added "boot-up melody"
|
|
|
|
Added btnEnter.waitForClick() for program start
|
|
|
|
Fixed display printouts
|
|
|
|
HW change: gearbox on the left gear changed to 1:1
|
|
|
|
|
|
|
|
0.2.0 - 'ERIKA' (#15)
|
|
|
|
Tested functions for opening and closing doors, updated speeds
|
|
|
|
Added idealMPWRS - base MPWRS to update back to, testing with different speeds for left and right wheels
|
|
|
|
|
|
|
|
0.1.1 - 'ERIKA' (#2)
|
|
|
|
Re-created structs for version, MPWRS, note
|
|
|
|
Re-created function for opening and closing door and function for generating Version ID
|
|
|
|
*/
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @struct version
|
|
|
|
/// @brief Struct for holding all informations about current version
|
2023-11-09 15:24:40 +01:00
|
|
|
struct version
|
|
|
|
{
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int: version id
|
2023-11-09 15:24:40 +01:00
|
|
|
int id;
|
2023-11-16 09:07:53 +01:00
|
|
|
/// @brief int: major
|
2023-11-09 15:24:40 +01:00
|
|
|
int major;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int: minor
|
2023-11-09 15:24:40 +01:00
|
|
|
int minor;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int: patch
|
2023-11-09 15:24:40 +01:00
|
|
|
int patch;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief std::tm: release date (date & time of compilation)
|
2023-11-09 15:24:40 +01:00
|
|
|
std::tm relDate;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief const char*: string codename of the current version
|
2023-11-09 15:24:40 +01:00
|
|
|
const char *codename;
|
|
|
|
};
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @struct MPWRS
|
|
|
|
/// @brief Structure to hold motor powers (left and right)
|
2023-11-09 15:24:40 +01:00
|
|
|
struct MPWRS
|
|
|
|
{
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int: left motor power
|
2023-11-09 15:24:40 +01:00
|
|
|
int lMotorPWR;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int: right motor power
|
2023-11-09 15:24:40 +01:00
|
|
|
int rMotorPWR;
|
|
|
|
};
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @struct MPWRSPlus
|
|
|
|
/// @brief Structure to hold motor powers and speed modifier (extends MPWRS)
|
2023-11-14 11:14:18 +01:00
|
|
|
struct MPWRSPlus
|
|
|
|
{
|
2023-11-15 08:44:48 +01:00
|
|
|
|
|
|
|
/// @brief MPWRS: motor powers
|
2023-11-14 11:14:18 +01:00
|
|
|
MPWRS motor_powers;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int: speed modifier
|
2023-11-14 11:14:18 +01:00
|
|
|
int SPEED_MODIFIER;
|
|
|
|
};
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @struct note
|
|
|
|
/// @brief Structure for holding note (frequency and duration)
|
2023-11-09 15:24:40 +01:00
|
|
|
struct note
|
|
|
|
{
|
2023-11-16 09:07:53 +01:00
|
|
|
/// @brief uint16_t: Frequency of the note
|
2023-11-09 15:24:40 +01:00
|
|
|
uint16_t frequency;
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief int32_t: Duration of the note
|
2023-11-09 15:24:40 +01:00
|
|
|
int32_t duration;
|
|
|
|
};
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
/// @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
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @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
|
2023-11-16 09:07:53 +01:00
|
|
|
MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 60)
|
2023-11-14 16:45:45 +01:00
|
|
|
{
|
|
|
|
int batteryLevel = ev3_battery_voltage_mV();
|
|
|
|
|
|
|
|
double squared = (double)0.00003 * (double)(batteryLevel * batteryLevel);
|
|
|
|
double linear = -0.42 * (double)batteryLevel;
|
|
|
|
double constant = 1470;
|
|
|
|
double modifier = squared + linear + constant;
|
|
|
|
int SPEED_MODIFIER = 30 - ((int)round(modifier));
|
|
|
|
// ev3cxx::display.format(4, "Q: % \nL: % \nC: % \nSM: % ") % squared % linear % constant % modifier;
|
|
|
|
|
|
|
|
MPWRS idealMPWRS;
|
|
|
|
idealMPWRS.lMotorPWR = leftMotor + SPEED_MODIFIER;
|
|
|
|
idealMPWRS.rMotorPWR = rightMotor + SPEED_MODIFIER;
|
|
|
|
|
|
|
|
MPWRSPlus retVal;
|
|
|
|
retVal.motor_powers = idealMPWRS;
|
|
|
|
retVal.SPEED_MODIFIER = SPEED_MODIFIER;
|
|
|
|
return retVal;
|
|
|
|
}
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
/// @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);
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function to clean display and write first two line
|
2023-11-14 16:45:45 +01:00
|
|
|
void cleanAndTitle()
|
|
|
|
{
|
|
|
|
ev3cxx::display.resetScreen();
|
|
|
|
ev3cxx::display.format(0, " DOBREMYSL ");
|
|
|
|
ev3cxx::display.format(1, "*****************");
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Diagnostic function to display all value on display with ability to show multiple pages
|
2023-11-17 10:48:54 +01:00
|
|
|
/// @param allValues values: all values in allValues structure
|
|
|
|
void displayAllValues(allValues values)
|
2023-11-14 16:45:45 +01:00
|
|
|
{
|
|
|
|
ev3cxx::display.resetScreen();
|
|
|
|
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
|
|
|
|
|
|
|
|
ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::RIGHT); // Right button
|
|
|
|
ev3cxx::BrickButton btnUp(ev3cxx::BrickButtons::UP); // Up button
|
|
|
|
ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
int pages = 3;
|
2023-11-14 16:45:45 +01:00
|
|
|
int page = 0;
|
|
|
|
while (true)
|
|
|
|
{
|
|
|
|
switch (page)
|
|
|
|
{
|
|
|
|
case 0:
|
|
|
|
ev3cxx::display.resetScreen();
|
|
|
|
ev3cxx::display.format(0, " DOBREMYSL ");
|
|
|
|
ev3cxx::display.format(1, "*****************");
|
2023-11-17 10:48:54 +01:00
|
|
|
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);
|
2023-11-14 16:45:45 +01:00
|
|
|
break;
|
|
|
|
case 1:
|
|
|
|
ev3cxx::display.resetScreen();
|
2023-11-15 08:44:48 +01:00
|
|
|
ev3cxx::display.format(0, "Bat.: % mV") % ev3_battery_voltage_mV();
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::display.format(1, "Mod.: % ") % values.SPEED_MODIFIER;
|
2023-11-14 16:45:45 +01:00
|
|
|
ev3cxx::display.format(2, "-------_T_-------");
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::display.format(3, "TT: % ") % values.turningThreshold;
|
|
|
|
ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER;
|
|
|
|
ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION;
|
2023-11-15 08:44:48 +01:00
|
|
|
ev3cxx::display.format(6, "-------_1_-------");
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::display.format(7, "LOCL: % ") % values.shortOneCycleLimit;
|
2023-11-14 16:45:45 +01:00
|
|
|
break;
|
|
|
|
case 2:
|
|
|
|
ev3cxx::display.resetScreen();
|
2023-11-16 09:07:53 +01:00
|
|
|
ev3cxx::display.format(0, "-------_2_-------");
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::display.format(1, "LOCL: % ") % values.shortOneCycleLimit;
|
2023-11-14 16:45:45 +01:00
|
|
|
break;
|
2023-11-16 09:07:53 +01:00
|
|
|
case 3:
|
|
|
|
ev3cxx::display.resetScreen();
|
2023-11-14 16:45:45 +01:00
|
|
|
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
while (true)
|
|
|
|
{
|
|
|
|
if (btnLeft.isPressed())
|
|
|
|
{
|
|
|
|
ev3cxx::display.resetScreen();
|
|
|
|
cleanAndTitle();
|
|
|
|
ev3cxx::display.format(2, "Press ENTR to run");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (btnUp.isPressed())
|
|
|
|
{
|
|
|
|
page -= 1;
|
|
|
|
if (page < 0)
|
|
|
|
{
|
|
|
|
page = 0;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
else if (btnDown.isPressed())
|
|
|
|
{
|
|
|
|
page += 1;
|
|
|
|
if (page > pages)
|
|
|
|
{
|
|
|
|
page = pages;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
tslp_tsk(500);
|
|
|
|
}
|
|
|
|
tslp_tsk(200);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
/// @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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function to parse timestamp to std::tm
|
|
|
|
/// @param timestampStr const char*: string form of the timestamp
|
|
|
|
/// @return std::tm: parsed timestamp
|
2023-11-14 16:45:45 +01:00
|
|
|
std::tm parseTimestamp(const char *timestampStr)
|
|
|
|
{
|
|
|
|
std::tm tmStruct = {};
|
|
|
|
std::istringstream iss(timestampStr);
|
|
|
|
|
|
|
|
// The format of __TIMESTAMP__ is implementation-dependent
|
|
|
|
// The example below assumes the format "Www Mmm dd hh:mm:ss yyyy"
|
|
|
|
// Adjust the format string based on your compiler's __TIMESTAMP__ format
|
|
|
|
iss >> std::get_time(&tmStruct, "%a %b %d %H:%M:%S %Y");
|
|
|
|
|
|
|
|
return tmStruct;
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function to generate version struct of the current version
|
|
|
|
/// Includes getting and parsing time of compilation using parseTimestamp function
|
|
|
|
/// @param versionID int: versionID, increment with every new change
|
|
|
|
/// @param codename const char*: string codename of the current version
|
|
|
|
/// @param major int: major
|
|
|
|
/// @param minor int: minor
|
|
|
|
/// @param patch int: patch
|
|
|
|
/// @return version: current version of the code, with date and time of compilation included
|
2023-11-14 16:45:45 +01:00
|
|
|
version createVersion(int versionID, const char *codename, int major, int minor, int patch)
|
2023-11-09 15:24:40 +01:00
|
|
|
{
|
|
|
|
version retVersion;
|
|
|
|
retVersion.id = versionID;
|
|
|
|
retVersion.major = major;
|
|
|
|
retVersion.minor = minor;
|
|
|
|
retVersion.patch = patch;
|
|
|
|
retVersion.codename = codename;
|
2023-11-14 16:45:45 +01:00
|
|
|
/*retVersion.relDate.tm_sec = timestamp_tm.tm_sec;
|
|
|
|
retVersion.relDate.tm_min = timestamp_tm.tm_min;
|
|
|
|
retVersion.relDate.tm_hour = timestamp_tm.tm_hour;
|
|
|
|
retVersion.relDate.tm_mday = timestamp_tm.tm_mday;
|
|
|
|
retVersion.relDate.tm_mon = timestamp_tm.tm_mon;*/
|
|
|
|
retVersion.relDate = parseTimestamp(__TIMESTAMP__);
|
|
|
|
retVersion.relDate.tm_year = 2023 - 1900;
|
2023-11-09 15:24:40 +01:00
|
|
|
retVersion.relDate.tm_isdst = 0;
|
|
|
|
std::mktime(&retVersion.relDate);
|
|
|
|
|
|
|
|
return retVersion;
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function to open door
|
|
|
|
/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door
|
2023-11-09 15:24:40 +01:00
|
|
|
void open_door(ev3cxx::Motor hinge)
|
|
|
|
{
|
|
|
|
hinge.onForDegrees(-25, 200);
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function to close door
|
2023-11-16 09:07:53 +01:00
|
|
|
/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door
|
2023-11-09 15:24:40 +01:00
|
|
|
void close_door(ev3cxx::Motor hinge)
|
|
|
|
{
|
|
|
|
hinge.onForDegrees(25, 200);
|
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function for turning
|
|
|
|
/// !! ROBOT CAN TURN ONLY LEFT !!
|
|
|
|
/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
|
|
|
|
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
|
|
|
/// @param endAngle int: angle to turn to (wanted angle, will be modified internally)
|
|
|
|
/// @param THRESHOLD int: turning accuracy in degrees (default 2)
|
|
|
|
/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
|
|
|
|
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
|
2023-11-14 11:14:18 +01:00
|
|
|
{
|
2023-11-16 09:07:53 +01:00
|
|
|
cleanAndTitle();
|
2023-11-15 08:44:48 +01:00
|
|
|
endAngle = endAngle - TFC;
|
2023-11-14 16:45:45 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
|
|
|
|
|
|
|
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
|
2023-11-15 08:44:48 +01:00
|
|
|
// left, right, rotations (faster), brake, blocking, wait_after
|
|
|
|
motors.onForRotations(-10, 25, 1.5, true, false, 60);
|
2023-11-14 16:45:45 +01:00
|
|
|
|
|
|
|
bool rotating = true;
|
2023-11-15 08:44:48 +01:00
|
|
|
int counter = 0;
|
|
|
|
while (rotating && counter < 250)
|
2023-11-14 16:45:45 +01:00
|
|
|
{
|
|
|
|
int currAngle = ev3cxx::abs(gyro.angle());
|
2023-11-15 08:44:48 +01:00
|
|
|
ev3cxx::display.format(3, "Angle: % ") % currAngle;
|
|
|
|
ev3cxx::display.format(4, "Counter: % ") % counter;
|
|
|
|
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
|
2023-11-14 16:45:45 +01:00
|
|
|
{
|
2023-11-15 08:44:48 +01:00
|
|
|
|
|
|
|
int error = endAngle - currAngle;
|
|
|
|
ev3cxx::display.format(5, "Error: % deg.") % error;
|
|
|
|
|
|
|
|
motors.off(true);
|
2023-11-14 16:45:45 +01:00
|
|
|
rotating = false;
|
2023-11-15 08:44:48 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
|
|
|
ev3_speaker_play_tone(NOTE_F5, 100);
|
|
|
|
return;
|
2023-11-14 16:45:45 +01:00
|
|
|
}
|
2023-11-15 08:44:48 +01:00
|
|
|
tslp_tsk(20);
|
|
|
|
counter++;
|
2023-11-14 11:14:18 +01:00
|
|
|
}
|
2023-11-16 09:07:53 +01:00
|
|
|
if ((endAngle - THRESHOLD < ev3cxx::abs(gyro.angle())) &&
|
|
|
|
(ev3cxx::abs(gyro.angle()) < endAngle + THRESHOLD))
|
|
|
|
{
|
|
|
|
turn(motors, gyro, endAngle, THRESHOLD, TFC);
|
|
|
|
}
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
/// @brief Function for turning (alternative way XD)
|
|
|
|
/// !! ROBOT CAN TURN ONLY LEFT !!
|
|
|
|
/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
|
|
|
|
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
|
|
|
/// @param endAngle int: angle to turn to (wanted angle, will be modified internally)
|
|
|
|
/// @param THRESHOLD int: turning accuracy in degrees (default 2)
|
|
|
|
/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
|
|
|
|
void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
|
|
|
|
{
|
|
|
|
cleanAndTitle();
|
|
|
|
endAngle = endAngle - TFC;
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
|
|
|
|
|
|
|
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
|
|
|
|
// left, right, rotations (faster), brake, blocking, wait_after
|
2023-11-17 10:48:54 +01:00
|
|
|
motors.on(-25, 45);
|
2023-11-16 09:07:53 +01:00
|
|
|
|
|
|
|
bool rotating = true;
|
|
|
|
int counter = 0;
|
|
|
|
while (rotating && counter < 25000)
|
|
|
|
{
|
|
|
|
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 error = endAngle - currAngle;
|
|
|
|
ev3cxx::display.format(5, "Error: % deg.") % error;
|
|
|
|
|
|
|
|
motors.off(true);
|
|
|
|
rotating = false;
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
|
|
|
ev3_speaker_play_tone(NOTE_F5, 100);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
tslp_tsk(20);
|
|
|
|
counter++;
|
|
|
|
}
|
2023-11-15 08:44:48 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
2023-11-14 16:45:45 +01:00
|
|
|
return;
|
2023-11-14 11:14:18 +01:00
|
|
|
}
|
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @brief Function to pick up all cubes on shorter side.
|
|
|
|
/// Returns true if ran till the end and false if stopped by middle button
|
|
|
|
/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
|
|
|
|
/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
|
|
|
|
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
2023-11-16 09:07:53 +01:00
|
|
|
/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
|
2023-11-15 08:44:48 +01:00
|
|
|
/// @param CYCLE_LIMIT int: how many cycle to run (default 90)
|
|
|
|
/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
|
|
|
|
/// @param THRESHOLD int: when to start correcting drive (in degrees) (default 2)
|
2023-11-16 09:07:53 +01:00
|
|
|
/// @return bool: false if stopped by the middle button
|
|
|
|
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
|
2023-11-09 15:24:40 +01:00
|
|
|
{
|
2023-11-16 09:07:53 +01:00
|
|
|
cleanAndTitle();
|
2023-11-14 16:45:45 +01:00
|
|
|
const int LEFT_THRESHOLD = -THRESHOLD;
|
|
|
|
const int RIGHT_THRESHOLD = THRESHOLD;
|
2023-11-09 15:24:40 +01:00
|
|
|
|
|
|
|
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.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
|
|
|
bool run = true;
|
2023-11-10 15:43:21 +01:00
|
|
|
bool error = false;
|
2023-11-09 15:24:40 +01:00
|
|
|
int cycleCounter = 0;
|
2023-11-10 13:27:12 +01:00
|
|
|
|
2023-11-09 15:24:40 +01:00
|
|
|
while (run)
|
|
|
|
{
|
|
|
|
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
|
|
|
|
tslp_tsk(50);
|
|
|
|
|
|
|
|
// Reset both motor's powers
|
|
|
|
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
|
|
|
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
|
|
|
// Get power of both motors
|
|
|
|
int lPower = motors.leftMotor().currentPower();
|
|
|
|
int rPower = motors.rightMotor().currentPower();
|
|
|
|
int angle = gyro.angle();
|
|
|
|
|
|
|
|
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
|
|
|
|
|
|
|
|
// Emergency break using middle button (BTN_ENTER)
|
|
|
|
if (btnEnter.isPressed())
|
|
|
|
{
|
|
|
|
run = false;
|
2023-11-10 15:43:21 +01:00
|
|
|
error = true;
|
|
|
|
motors.off(true);
|
2023-11-09 15:24:40 +01:00
|
|
|
}
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
// Exit when bumper hit
|
|
|
|
if (bumper.isPressed())
|
|
|
|
{
|
|
|
|
run = false;
|
|
|
|
motors.off(true);
|
|
|
|
}
|
|
|
|
|
2023-11-09 15:24:40 +01:00
|
|
|
// Check gyro angle and change driving speed to fix the angle
|
|
|
|
// To the left
|
2023-11-10 15:43:21 +01:00
|
|
|
if (angle < LEFT_THRESHOLD)
|
|
|
|
{
|
|
|
|
// ev3_speaker_play_tone(NOTE_A5, 250);
|
2023-11-10 14:49:37 +01:00
|
|
|
int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
|
2023-11-10 15:43:21 +01:00
|
|
|
motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
|
|
|
|
ev3_speaker_play_tone(correction * 1000, 30);
|
2023-11-09 15:24:40 +01:00
|
|
|
// Check if the motor is stuck
|
2023-11-10 15:43:21 +01:00
|
|
|
if (lPower == 0)
|
|
|
|
{
|
2023-11-10 14:22:50 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
2023-11-10 15:43:21 +01:00
|
|
|
ev3_speaker_play_tone(NOTE_A5, 250);
|
2023-11-16 09:07:53 +01:00
|
|
|
run = false;
|
|
|
|
error = true;
|
|
|
|
motors.off(true);
|
2023-11-09 15:24:40 +01:00
|
|
|
}
|
2023-11-10 15:43:21 +01:00
|
|
|
|
|
|
|
// To the right
|
|
|
|
}
|
|
|
|
else if (angle > RIGHT_THRESHOLD)
|
|
|
|
{
|
|
|
|
// ev3_speaker_play_tone(NOTE_A4, 250);
|
2023-11-10 14:49:37 +01:00
|
|
|
int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD);
|
2023-11-10 15:43:21 +01:00
|
|
|
motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
|
|
|
|
ev3_speaker_play_tone(correction * 1000, 30);
|
2023-11-09 15:24:40 +01:00
|
|
|
// Check if the motor is stuck
|
2023-11-10 15:43:21 +01:00
|
|
|
if (rPower == 0)
|
|
|
|
{
|
2023-11-10 14:22:50 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
2023-11-10 15:43:21 +01:00
|
|
|
ev3_speaker_play_tone(NOTE_A4, 250);
|
2023-11-16 09:07:53 +01:00
|
|
|
run = false;
|
|
|
|
error = true;
|
|
|
|
motors.off(true);
|
2023-11-09 15:24:40 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
cycleCounter++;
|
|
|
|
|
2023-11-10 15:43:21 +01:00
|
|
|
if (cycleCounter == CYCLE_LIMIT)
|
|
|
|
{
|
2023-11-09 15:24:40 +01:00
|
|
|
run = false;
|
|
|
|
}
|
|
|
|
}
|
2023-11-14 11:14:18 +01:00
|
|
|
motors.off(true);
|
2023-11-10 15:43:21 +01:00
|
|
|
ev3_speaker_play_tone(NOTE_C4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 125);
|
|
|
|
tslp_tsk(125);
|
|
|
|
ev3_speaker_play_tone(NOTE_D4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_B4, 750);
|
|
|
|
tslp_tsk(750);
|
|
|
|
ev3_speaker_play_tone(NOTE_G4, 750);
|
|
|
|
tslp_tsk(750);
|
|
|
|
return !error;
|
2023-11-09 15:24:40 +01:00
|
|
|
}
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
/// @brief Function to pick up all cubes on longer side.
|
|
|
|
/// Returns true if ran till the end and false if stopped by middle button
|
|
|
|
/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
|
|
|
|
/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
|
|
|
|
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
|
|
|
/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
|
|
|
|
/// @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)
|
|
|
|
{
|
|
|
|
cleanAndTitle();
|
|
|
|
|
|
|
|
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.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
|
|
|
bool run = true;
|
|
|
|
bool error = false;
|
|
|
|
int cycleCounter = 0;
|
2023-11-17 10:48:54 +01:00
|
|
|
int lastError = -1;
|
|
|
|
int errorStrike = 0;
|
2023-11-16 09:07:53 +01:00
|
|
|
|
|
|
|
while (run)
|
|
|
|
{
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
2023-11-16 09:07:53 +01:00
|
|
|
motors.on(motor_powers.lMotorPWR + 20, motor_powers.rMotorPWR - 20);
|
|
|
|
tslp_tsk(50);
|
|
|
|
|
|
|
|
// Reset both motor's powers
|
|
|
|
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
|
|
|
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
|
|
|
// Get power of both motors
|
|
|
|
int lPower = motors.leftMotor().currentPower();
|
|
|
|
int rPower = motors.rightMotor().currentPower();
|
|
|
|
int angle = gyro.angle();
|
|
|
|
|
|
|
|
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
if (lPower == 0 || rPower == 0 || angle < -2)
|
2023-11-16 09:07:53 +01:00
|
|
|
{
|
2023-11-17 10:48:54 +01:00
|
|
|
// 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;
|
|
|
|
}
|
2023-11-16 09:07:53 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
if (angle > 5)
|
|
|
|
{
|
|
|
|
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
2023-11-17 10:48:54 +01:00
|
|
|
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 35;
|
|
|
|
ev3_speaker_play_tone(NOTE_D4, 50);
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
2023-11-16 09:07:53 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
// Emergency break using middle button (BTN_ENTER)
|
|
|
|
if (btnEnter.isPressed())
|
|
|
|
{
|
|
|
|
run = false;
|
|
|
|
error = true;
|
|
|
|
motors.off(true);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Exit when front bumper is hit
|
|
|
|
if (bumper.isPressed())
|
|
|
|
{
|
2023-11-17 10:48:54 +01:00
|
|
|
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);
|
2023-11-16 09:07:53 +01:00
|
|
|
run = false;
|
|
|
|
motors.off(true);
|
|
|
|
}
|
|
|
|
|
|
|
|
cycleCounter++;
|
|
|
|
|
|
|
|
if (cycleCounter == CYCLE_LIMIT)
|
|
|
|
{
|
|
|
|
run = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
motors.off(true);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 125);
|
|
|
|
tslp_tsk(125);
|
|
|
|
ev3_speaker_play_tone(NOTE_D4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_B4, 750);
|
|
|
|
tslp_tsk(750);
|
|
|
|
ev3_speaker_play_tone(NOTE_G4, 750);
|
|
|
|
tslp_tsk(750);
|
|
|
|
return !error;
|
|
|
|
}
|
|
|
|
|
|
|
|
/// @brief Function to drive forward till bumper button is pressed
|
|
|
|
/// @param motors ev3cxx::MotorTank motors to use for driving
|
|
|
|
/// @param idealMPWRS MPWRS ideal motor powers for the motors
|
|
|
|
/// @param gyro ev3cxx::GyroSensor gyro sensor to use for error correction
|
|
|
|
/// @param bumper ev3cxx::TouchSensor touch sensor to use a bumper for stoping when wall is hit
|
|
|
|
/// @param CORRECTION_MULTIPLIER int for correcting errors in direction
|
|
|
|
/// @return bool: false if stopped by the middle button
|
|
|
|
bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CORRECTION_MULTIPLIER = 2, int THRESHOLD = 2)
|
2023-11-14 11:14:18 +01:00
|
|
|
{
|
2023-11-16 09:07:53 +01:00
|
|
|
cleanAndTitle();
|
|
|
|
const int LEFT_THRESHOLD = -THRESHOLD;
|
|
|
|
const int RIGHT_THRESHOLD = THRESHOLD;
|
|
|
|
|
|
|
|
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.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
|
|
|
bool run = true;
|
|
|
|
bool error = false;
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
int cycleCounter = 0;
|
|
|
|
int lastError = -1;
|
|
|
|
int errorStrike = 0;
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
while (run)
|
|
|
|
{
|
|
|
|
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
|
|
|
|
tslp_tsk(50);
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
2023-11-16 09:07:53 +01:00
|
|
|
|
|
|
|
// Reset both motor's powers
|
|
|
|
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
|
|
|
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
|
|
|
// Get power of both motors
|
|
|
|
int lPower = motors.leftMotor().currentPower();
|
|
|
|
int rPower = motors.rightMotor().currentPower();
|
|
|
|
int angle = gyro.angle();
|
|
|
|
|
|
|
|
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle;
|
|
|
|
|
|
|
|
// Emergency break using middle button (BTN_ENTER)
|
|
|
|
if (btnEnter.isPressed())
|
|
|
|
{
|
|
|
|
run = false;
|
|
|
|
error = true;
|
|
|
|
motors.off(true);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Exit when bumper hit
|
|
|
|
if (bumper.isPressed())
|
|
|
|
{
|
|
|
|
run = false;
|
|
|
|
motors.off(true);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Check gyro angle and change driving speed to fix the angle
|
|
|
|
// To the left
|
|
|
|
if (angle < LEFT_THRESHOLD)
|
|
|
|
{
|
|
|
|
// ev3_speaker_play_tone(NOTE_A5, 250);
|
|
|
|
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);
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
2023-11-16 09:07:53 +01:00
|
|
|
}
|
|
|
|
// To the right
|
|
|
|
else if (angle > RIGHT_THRESHOLD)
|
|
|
|
{
|
|
|
|
// ev3_speaker_play_tone(NOTE_A4, 250);
|
|
|
|
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);
|
2023-11-17 10:48:54 +01:00
|
|
|
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;
|
|
|
|
}
|
2023-11-16 09:07:53 +01:00
|
|
|
}
|
|
|
|
}
|
2023-11-14 11:14:18 +01:00
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
motors.off(true);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 125);
|
|
|
|
tslp_tsk(125);
|
|
|
|
ev3_speaker_play_tone(NOTE_D4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_C4, 250);
|
|
|
|
tslp_tsk(250);
|
|
|
|
ev3_speaker_play_tone(NOTE_B4, 750);
|
|
|
|
tslp_tsk(750);
|
|
|
|
ev3_speaker_play_tone(NOTE_G4, 750);
|
|
|
|
tslp_tsk(750);
|
|
|
|
return !error;
|
|
|
|
}
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
void main_task(intptr_t unused)
|
|
|
|
{
|
2023-11-17 10:48:54 +01:00
|
|
|
const int CYCLE_LIMIT_1 = 250;
|
|
|
|
const int CYCLE_LIMIT_2 = 350;
|
2023-11-14 16:45:45 +01:00
|
|
|
const int THRESHOLD = 2;
|
|
|
|
const int TURNING_THRESHOLD = 1;
|
2023-11-15 08:44:48 +01:00
|
|
|
const int TURNING_FACTOR_CORRECTION = 5;
|
2023-11-14 16:45:45 +01:00
|
|
|
const int CORRECTION_MULTIPLIER = 20;
|
2023-11-14 11:14:18 +01:00
|
|
|
|
2023-11-14 16:45:45 +01:00
|
|
|
int volume = 100;
|
2023-11-09 15:24:40 +01:00
|
|
|
|
|
|
|
// Create version info
|
2023-11-17 10:48:54 +01:00
|
|
|
const version VERSION = createVersion(85, "MONIKA", 0, 8, 0);
|
2023-11-09 15:24:40 +01:00
|
|
|
|
|
|
|
// Set-up screen
|
|
|
|
ev3cxx::display.resetScreen();
|
|
|
|
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
|
|
|
|
|
2023-11-14 11:14:18 +01:00
|
|
|
// Set up motor powers
|
|
|
|
MPWRSPlus calcedPWRS = calculate_motor_pwrs();
|
|
|
|
|
|
|
|
MPWRS idealMPWRS = calcedPWRS.motor_powers;
|
|
|
|
const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
|
|
|
|
|
|
|
|
MPWRS motor_powers;
|
|
|
|
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
|
|
|
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
|
|
|
|
2023-11-10 14:49:37 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
|
|
|
|
2023-11-14 16:45:45 +01:00
|
|
|
// Set up motors
|
2023-11-16 09:07:53 +01:00
|
|
|
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)
|
2023-11-14 16:45:45 +01:00
|
|
|
ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor
|
|
|
|
ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4); // Touch sensor
|
2023-11-16 09:07:53 +01:00
|
|
|
ev3cxx::TouchSensor frontTouch(ev3cxx::SensorPort::S3); // Touch sensor (bumper)
|
2023-11-14 16:45:45 +01:00
|
|
|
|
|
|
|
// Set up buttons
|
|
|
|
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
|
|
|
ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::LEFT); // Left button (for entering diagnostics)
|
2023-11-15 08:44:48 +01:00
|
|
|
ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button (quiet mode)
|
2023-11-14 16:45:45 +01:00
|
|
|
|
2023-11-15 08:44:48 +01:00
|
|
|
// quiet mode activation
|
|
|
|
if (btnDown.isPressed())
|
|
|
|
{
|
|
|
|
volume = 0;
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
|
|
|
tslp_tsk(200);
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
|
|
|
tslp_tsk(200);
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
2023-11-14 16:45:45 +01:00
|
|
|
}
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
ev3_speaker_set_volume(volume);
|
|
|
|
play_starting_melody();
|
|
|
|
|
2023-11-14 16:45:45 +01:00
|
|
|
// 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;
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
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;
|
2023-11-09 15:24:40 +01:00
|
|
|
|
|
|
|
// Start program
|
2023-11-17 10:48:54 +01:00
|
|
|
start_program_exe(btnLeft, btnEnter, touchS, values);
|
|
|
|
|
|
|
|
act_tsk(SUB_TASK);
|
|
|
|
// act_tsk(MUSIC_TASK);
|
2023-11-14 16:45:45 +01:00
|
|
|
|
|
|
|
ev3_speaker_play_tone(NOTE_F4, 100);
|
|
|
|
tslp_tsk(200);
|
2023-11-09 15:24:40 +01:00
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
// Close the door before begining the drive
|
|
|
|
close_door(hinge);
|
|
|
|
|
|
|
|
// Run first long side
|
|
|
|
bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER);
|
|
|
|
// If something happened
|
2023-11-10 15:43:21 +01:00
|
|
|
if (!side_1)
|
|
|
|
{
|
2023-11-16 09:07:53 +01:00
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
|
|
|
ev3_speaker_play_tone(NOTE_F4, 100);
|
|
|
|
tslp_tsk(200);
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
|
|
|
ev3_speaker_play_tone(NOTE_FS4, 100);
|
2023-11-10 15:43:21 +01:00
|
|
|
}
|
2023-11-15 08:44:48 +01:00
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25);
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
// turn 90 degress left
|
2023-11-17 10:48:54 +01:00
|
|
|
//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);
|
2023-11-16 09:07:53 +01:00
|
|
|
|
|
|
|
// Cross to the other side
|
|
|
|
gyro.resetHard();
|
|
|
|
bool crossing = unlimited_drive(motors, motor_powers, gyro, frontTouch, CORRECTION_MULTIPLIER, THRESHOLD);
|
|
|
|
|
|
|
|
if (!crossing)
|
|
|
|
{
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
|
|
|
ev3_speaker_play_tone(NOTE_F4, 100);
|
|
|
|
tslp_tsk(200);
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
|
|
|
ev3_speaker_play_tone(NOTE_FS4, 100);
|
|
|
|
}
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
// 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);
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
// reset gyro
|
|
|
|
gyro.resetHard();
|
|
|
|
// Run second long side
|
|
|
|
bool side_2 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER);
|
|
|
|
// If something happened
|
|
|
|
if (!side_2)
|
|
|
|
{
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
|
|
|
ev3_speaker_play_tone(NOTE_F4, 100);
|
|
|
|
tslp_tsk(200);
|
|
|
|
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
|
|
|
ev3_speaker_play_tone(NOTE_FS4, 100);
|
|
|
|
}
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
// reset gyro; turn 150 degrees left
|
|
|
|
gyro.resetHard();
|
|
|
|
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
|
|
|
|
|
|
|
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
|
|
|
|
|
2023-11-16 09:07:53 +01:00
|
|
|
open_door(hinge);
|
|
|
|
|
2023-11-17 10:48:54 +01:00
|
|
|
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
|
|
|
|
motors.off(true);
|
|
|
|
|
|
|
|
act_tsk(MUSIC_TASK);
|
2023-11-09 15:24:40 +01:00
|
|
|
}
|