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
parent
64d3c294f0
commit
b8cd70e1fa
223
app.cpp
223
app.cpp
@ -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(¤tVersion.relDate);
|
||||
ev3cxx::display.format(7, "-----------------");
|
||||
ev3cxx::display.format(5, "Volume: % %%") % volume;
|
||||
ev3cxx::display.format(6, "Rel.: % ") % std::asctime(¤tVersion.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();
|
||||
@ -408,10 +471,17 @@ void main_task(intptr_t unused)
|
||||
// Set up buttons
|
||||
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
||||
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()){
|
||||
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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user