Moved to version 0.6.0 - 'URSULA'

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

BIN
app

Binary file not shown.

221
app.cpp
View File

@ -7,16 +7,22 @@
#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 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
0.4.2 - 'HELGA' (#67)
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
*/
/* Structure to hold current version (major, minor, patch, codename, id, release_date)
MAJOR - Major version number
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
/// @brief Struct for holding all informations about current version
struct version
{
/// @brief int: version id
int id;
/// @brief int: major
int major;
/// @brief int: minor
int minor;
/// @brief int: patch
int patch;
/// @brief std::tm: release date (date & time of compilation)
std::tm relDate;
/// @brief const char*: string codename of the current version
const char *codename;
};
// Structure to hold motor powers (left and right)
/// @struct MPWRS
/// @brief Structure to hold motor powers (left and right)
struct MPWRS
{
/// @brief int: left motor power
int lMotorPWR;
/// @brief int: right motor power
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
{
/// @brief MPWRS: motor powers
MPWRS motor_powers;
/// @brief int: speed modifier
int SPEED_MODIFIER;
};
// Structure to hold notes
/// @struct note
/// @brief Structure for holding note (frequency and duration)
struct note
{
/// @brief uint16_t: Frequency of the note
uint16_t frequency;
/// @brief int32_t: Duration of the note
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)
{
int batteryLevel = ev3_battery_voltage_mV();
@ -105,6 +126,7 @@ MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
return retVal;
}
/// @brief Function to clean display and write first two line
void cleanAndTitle()
{
ev3cxx::display.resetScreen();
@ -112,8 +134,17 @@ void cleanAndTitle()
ev3cxx::display.format(1, "*****************");
}
// diacnostic functin()
void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int batteryLevel, int SPEED_MODIFIER, int turningThreshold, int CORRECTION_MULTIPLIER, int shortOneCycleLimit)
/// @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
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.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(3, "Nr.: #% ") % currentVersion.id;
ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
ev3cxx::display.format(5, "Rel.: % ") % std::asctime(&currentVersion.relDate);
ev3cxx::display.format(7, "-----------------");
ev3cxx::display.format(5, "Volume: % %%") % volume;
ev3cxx::display.format(6, "Rel.: % ") % std::asctime(&currentVersion.relDate);
break;
case 1:
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(2, "-------_T_-------");
ev3cxx::display.format(3, "TT: % ") % turningThreshold;
ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
ev3cxx::display.format(5, "-------_1_-------");
ev3cxx::display.format(6, "SOCL: % ") % shortOneCycleLimit;
ev3cxx::display.format(7, "");
ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION;
ev3cxx::display.format(6, "-------_1_-------");
ev3cxx::display.format(7, "SOCL: % ") % shortOneCycleLimit;
break;
case 2:
ev3cxx::display.resetScreen();
ev3cxx::display.format(0, "% ");
ev3cxx::display.format(0, "");
ev3cxx::display.format(1, "");
ev3cxx::display.format(2, "");
break;
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 tmStruct = {};
@ -207,7 +241,15 @@ std::tm parseTimestamp(const char *timestampStr)
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 retVersion;
@ -229,52 +271,72 @@ version createVersion(int versionID, const char *codename, int major, int minor,
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)
{
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)
{
hinge.onForDegrees(25, 200);
}
/*
Function for turning
Robot can turn only left
*/
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2)
/// @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)
{
endAngle = endAngle - TFC;
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
motors.onForRotations(left_speed = -15,
right_speed = 30,
rotations = 2,
brake = true,
blocking = false,
wait_after_ms = 60);
// left, right, rotations (faster), brake, blocking, wait_after
motors.onForRotations(-10, 25, 1.5, true, false, 60);
bool rotating = true;
while (rotating)
int counter = 0;
while (rotating && counter < 250)
{
int currAngle = ev3cxx::abs(gyro.angle());
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle) &&
(currAngle < av3cxx::abs(endAngle + THRESHOLD)))
ev3cxx::display.format(3, "Angle: % ") % currAngle;
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;
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;
}
/*
Function to pick up all cubes on shorter side
return true if ran till the end and false if stopped by middle button
*/
/// @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
/// @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)
{
const int LEFT_THRESHOLD = -THRESHOLD;
@ -375,13 +437,14 @@ void main_task(intptr_t unused)
const int CYCLE_LIMIT_1 = 90;
const int THRESHOLD = 2;
const int TURNING_THRESHOLD = 1;
const int TURNING_FACTOR_CORRECTION = 5;
const int CORRECTION_MULTIPLIER = 20;
int volume = 100;
// 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)
const version VERSION = createVersion(72, "URSULA", 0, 5, 0);
const version VERSION = createVersion(73, "URSULA", 0, 6, 0);
// Set-up screen
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 btnDown(ev3cxx::BrickButtons::DOWN); // Down button (quiet mode)
if(btnDown.isPressed()){
volume = 10;
// 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);
}
// Print version information on screen
@ -438,68 +508,37 @@ void main_task(intptr_t unused)
tslp_tsk(200);
// Start program
while (1 == 1)
while (true)
{
// ev3_speaker_play_tone(NOTE_FS6, 20);
// tslp_tsk(20);
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())
{
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);
// ev3_speaker_play_tone(NOTE_F6, 50);
tslp_tsk(50);
}
ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(200);
for (int i = 0; i < 10; i++)
{
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);
bool side_1 = run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD);
if (!side_1)
{
// Something went wrong
return;
}
else
{
return;
}
turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
}