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

Moved to version 0.6.0 - 'URSULA'

Browse Source 
0.6.0 - 'HELGA' ()
Added Doxygen comments
Added volume to display_all_values (diagnostics screenS)

0.5.0 - 'HELGA' (#72)
Added diagnostics screen (opened by pressing left after boot)
Added secret quiet mode
Added turning_corection_factor for correcing gyro sensor measurements due to its bad placement

0.4.3 - 'HELGA' (#70)
Motor problems indicator with colors:
    left - red, right - orange,
    when motor is stuck change LED color
Added calculating motor powers based on battery level
This commit is contained in:
Blboun3 2023-11-15 08:44:48 +01:00
parent 64d3c294f0
commit b8cd70e1fa
2 changed files with 131 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
app
View File

Binary file not shown.

221
app.cpp
View File

@ -7,16 +7,22 @@
#include <cstring> #include <cstring>
/* /*
Change notes: CHANGELOG:
0.5.0 - 'HELGA' 0.6.0 - 'HELGA' ()
Added Doxygen comments
Added volume to display_all_values (diagnostics screenS)
0.5.0 - 'HELGA' (#72)
Added diagnostics screen (opened by pressing left after boot) Added diagnostics screen (opened by pressing left after boot)
Added secret quiet mode Added secret quiet mode
Added turning_corection_factor for correcing gyro sensor measurements due to its bad placement
0.4.3 - 'HELGA' (#70) 0.4.3 - 'HELGA' (#70)
Motor problems indicator with colors: Motor problems indicator with colors:
left - red, right - orange, left - red, right - orange,
when motor is stuck change LED color when motor is stuck change LED color
Added calculating motor powers based on battery level
0.4.2 - 'HELGA' (#67) 0.4.2 - 'HELGA' (#67)
Updated run_short() to run_short_side() Updated run_short() to run_short_side()
@ -45,45 +51,60 @@ Re-created structs for version, MPWRS, note
Re-created function for opening and closing door and function for generating Version ID Re-created function for opening and closing door and function for generating Version ID
*/ */
/* Structure to hold current version (major, minor, patch, codename, id, release_date) /// @struct version
MAJOR - Major version number /// @brief Struct for holding all informations about current version
MINOR - Minor changes in code
PATCH - Mostly just for testing
ID - Increment every time with new version
CODENAME - (String) Version name
RELEASE_DATE - when was current version released
*/
struct version struct version
{ {
/// @brief int: version id
int id; int id;
/// @brief int: major
int major; int major;
/// @brief int: minor
int minor; int minor;
/// @brief int: patch
int patch; int patch;
/// @brief std::tm: release date (date & time of compilation)
std::tm relDate; std::tm relDate;
/// @brief const char*: string codename of the current version
const char *codename; const char *codename;
}; };
// Structure to hold motor powers (left and right) /// @struct MPWRS
/// @brief Structure to hold motor powers (left and right)
struct MPWRS struct MPWRS
{ {
/// @brief int: left motor power
int lMotorPWR; int lMotorPWR;
/// @brief int: right motor power
int rMotorPWR; int rMotorPWR;
}; };
// Structure to hold motor powers and speed modifier /// @struct MPWRSPlus
/// @brief Structure to hold motor powers and speed modifier (extends MPWRS)
struct MPWRSPlus struct MPWRSPlus
{ {
/// @brief MPWRS: motor powers
MPWRS motor_powers; MPWRS motor_powers;
/// @brief int: speed modifier
int SPEED_MODIFIER; int SPEED_MODIFIER;
}; };
// Structure to hold notes
/// @struct note
/// @brief Structure for holding note (frequency and duration)
struct note struct note
{ {
/// @brief uint16_t: Frequency of the note
uint16_t frequency; uint16_t frequency;
/// @brief int32_t: Duration of the note
int32_t duration; int32_t duration;
}; };
/// @brief Function to calculate motor powers using quadratic equation, depending on battery power
/// @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
MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70) MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
{ {
int batteryLevel = ev3_battery_voltage_mV(); int batteryLevel = ev3_battery_voltage_mV();
@ -105,6 +126,7 @@ MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
return retVal; return retVal;
} }
/// @brief Function to clean display and write first two line
void cleanAndTitle() void cleanAndTitle()
{ {
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
@ -112,8 +134,17 @@ void cleanAndTitle()
ev3cxx::display.format(1, "*****************"); ev3cxx::display.format(1, "*****************");
} }
// diacnostic functin() /// @brief Diagnostic function to display all value on display with ability to show multiple pages
void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int batteryLevel, int SPEED_MODIFIER, int turningThreshold, int CORRECTION_MULTIPLIER, int shortOneCycleLimit) /// @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
void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit)
{ {
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
ev3cxx::display.setFont(EV3_FONT_MEDIUM); ev3cxx::display.setFont(EV3_FONT_MEDIUM);
@ -135,24 +166,25 @@ void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int
ev3cxx::display.format(2, "Ver.: % .% .% ") % currentVersion.major % currentVersion.minor % currentVersion.patch; ev3cxx::display.format(2, "Ver.: % .% .% ") % currentVersion.major % currentVersion.minor % currentVersion.patch;
ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id; ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id;
ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename; ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
ev3cxx::display.format(5, "Rel.: % ") % std::asctime(&currentVersion.relDate); ev3cxx::display.format(5, "Volume: % %%") % volume;
ev3cxx::display.format(7, "-----------------"); ev3cxx::display.format(6, "Rel.: % ") % std::asctime(&currentVersion.relDate);
break; break;
case 1: case 1:
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
ev3cxx::display.format(0, "Bat.: % mV") % batteryLevel; ev3cxx::display.format(0, "Bat.: % mV") % ev3_battery_voltage_mV();
ev3cxx::display.format(1, "Mod.: % ") % SPEED_MODIFIER; ev3cxx::display.format(1, "Mod.: % ") % SPEED_MODIFIER;
ev3cxx::display.format(2, "-------_T_-------"); ev3cxx::display.format(2, "-------_T_-------");
ev3cxx::display.format(3, "TT: % ") % turningThreshold; ev3cxx::display.format(3, "TT: % ") % turningThreshold;
ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER; ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
ev3cxx::display.format(5, "-------_1_-------"); ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION;
ev3cxx::display.format(6, "SOCL: % ") % shortOneCycleLimit; ev3cxx::display.format(6, "-------_1_-------");
ev3cxx::display.format(7, ""); ev3cxx::display.format(7, "SOCL: % ") % shortOneCycleLimit;
break; break;
case 2: case 2:
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
ev3cxx::display.format(0, "% "); ev3cxx::display.format(0, "");
ev3cxx::display.format(1, ""); ev3cxx::display.format(1, "");
ev3cxx::display.format(2, "");
break; break;
default: default:
@ -193,7 +225,9 @@ void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int
} }
} }
// TimeStamp parser /// @brief Function to parse timestamp to std::tm
/// @param timestampStr const char*: string form of the timestamp
/// @return std::tm: parsed timestamp
std::tm parseTimestamp(const char *timestampStr) std::tm parseTimestamp(const char *timestampStr)
{ {
std::tm tmStruct = {}; std::tm tmStruct = {};
@ -207,7 +241,15 @@ std::tm parseTimestamp(const char *timestampStr)
return tmStruct; return tmStruct;
} }
// Function to create new program version
/// @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
version createVersion(int versionID, const char *codename, int major, int minor, int patch) version createVersion(int versionID, const char *codename, int major, int minor, int patch)
{ {
version retVersion; version retVersion;
@ -229,52 +271,72 @@ version createVersion(int versionID, const char *codename, int major, int minor,
return retVersion; return retVersion;
} }
// Function to open door /// @brief Function to open 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.onForDegrees(-25, 200); hinge.onForDegrees(-25, 200);
} }
// Function to close door /// @brief Function to close 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.onForDegrees(25, 200);
} }
/*
Function for turning /// @brief Function for turning
Robot can turn only left /// !! ROBOT CAN TURN ONLY LEFT !!
*/ /// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2) /// @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)
{ {
endAngle = endAngle - TFC;
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);
motors.onForRotations(left_speed = -15, // left, right, rotations (faster), brake, blocking, wait_after
right_speed = 30, motors.onForRotations(-10, 25, 1.5, true, false, 60);
rotations = 2,
brake = true,
blocking = false,
wait_after_ms = 60);
bool rotating = true; bool rotating = true;
while (rotating) int counter = 0;
while (rotating && counter < 250)
{ {
int currAngle = ev3cxx::abs(gyro.angle()); int currAngle = ev3cxx::abs(gyro.angle());
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle) && ev3cxx::display.format(3, "Angle: % ") % currAngle;
(currAngle < av3cxx::abs(endAngle + THRESHOLD))) ev3cxx::display.format(4, "Counter: % ") % counter;
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
{ {
motors.off(brake = true);
int error = endAngle - currAngle;
ev3cxx::display.format(5, "Error: % deg.") % error;
motors.off(true);
rotating = false; rotating = false;
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
ev3_speaker_play_tone(NOTE_F5, 100);
return;
} }
tslp_tsk(20);
counter++;
} }
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
return; return;
} }
/* /// @brief Function to pick up all cubes on shorter side.
Function to pick up all cubes on shorter side /// Returns true if ran till the end and false if stopped by middle button
return 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 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)
/// @return bool: false if stopped by the middle button
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2) bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
{ {
const int LEFT_THRESHOLD = -THRESHOLD; const int LEFT_THRESHOLD = -THRESHOLD;
@ -375,13 +437,14 @@ void main_task(intptr_t unused)
const int CYCLE_LIMIT_1 = 90; const int CYCLE_LIMIT_1 = 90;
const int THRESHOLD = 2; const int THRESHOLD = 2;
const int TURNING_THRESHOLD = 1; const int TURNING_THRESHOLD = 1;
const int TURNING_FACTOR_CORRECTION = 5;
const int CORRECTION_MULTIPLIER = 20; const int CORRECTION_MULTIPLIER = 20;
int volume = 100; int volume = 100;
// Create version info // Create version info
// version createVersion(int versionID, const char *codename, int major, int minor, int patch, int day, int month, int year, int hour, int minute) // version createVersion(int versionID, const char *codename, int major, int minor, int patch, int day, int month, int year, int hour, int minute)
const version VERSION = createVersion(72, "URSULA", 0, 5, 0); const version VERSION = createVersion(73, "URSULA", 0, 6, 0);
// Set-up screen // Set-up screen
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
@ -410,8 +473,15 @@ void main_task(intptr_t unused)
ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::LEFT); // Left button (for entering diagnostics) ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::LEFT); // Left button (for entering diagnostics)
ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button (quiet mode) ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button (quiet mode)
if(btnDown.isPressed()){ // quiet mode activation
volume = 10; 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);
} }
// Print version information on screen // Print version information on screen
@ -438,68 +508,37 @@ void main_task(intptr_t unused)
tslp_tsk(200); tslp_tsk(200);
// Start program // Start program
while (1 == 1) while (true)
{ {
// ev3_speaker_play_tone(NOTE_FS6, 20); // ev3_speaker_play_tone(NOTE_FS6, 20);
// tslp_tsk(20); // tslp_tsk(20);
if (btnLeft.isPressed()) if (btnLeft.isPressed())
{ {
displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, ev3_battery_voltage_mV(), SPEED_MODIFIER, TURNING_THRESHOLD, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1); displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1);
} }
if (btnEnter.isPressed() || touchS.isPressed()) if (btnEnter.isPressed() || touchS.isPressed())
{ {
cleanAndTitle(); 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; break;
} }
ev3_speaker_play_tone(NOTE_F6, 50); // ev3_speaker_play_tone(NOTE_F6, 50);
tslp_tsk(50); tslp_tsk(50);
} }
ev3_speaker_play_tone(NOTE_F4, 100); ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(200); tslp_tsk(200);
for (int i = 0; i < 10; i++) bool side_1 = run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD);
{
gyro.resetHard();
if (gyro.angle() == 0)
{
ev3_speaker_play_tone(NOTE_F6, 200);
tslp_tsk(200);
}
turn(motors, gyro, 90, TURNING_THRESHOLD);
ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(200);
ev3_speaker_play_tone(NOTE_A4, 100);
tslp_tsk(200);
ev3_speaker_play_tone(NOTE_C4, 100);
tslp_tsk(200);
ev3_speaker_play_tone(NOTE_F4, 50);
tslp_tsk(100);
ev3_speaker_play_tone(NOTE_A4, 50);
tslp_tsk(100);
ev3_speaker_play_tone(NOTE_C4, 50);
tslp_tsk(100);
ev3_speaker_play_tone(NOTE_F4, 25);
tslp_tsk(50);
ev3_speaker_play_tone(NOTE_A4, 25);
tslp_tsk(50);
ev3_speaker_play_tone(NOTE_C4, 25);
tslp_tsk(50);
ev3_speaker_play_tone(NOTE_F4, 12);
tslp_tsk(25);
ev3_speaker_play_tone(NOTE_A4, 12);
tslp_tsk(25);
ev3_speaker_play_tone(NOTE_C4, 12);
tslp_tsk(25);
}
bool side_1 = false; // run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MLUTIPLIER, THRESHOLD);
if (!side_1) if (!side_1)
{ {
// Something went wrong
return; return;
} }
else
{ turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
return;
}
} }