Denk proof
This commit is contained in:
Blboun3
parent
095aef2d3f
commit
64d3c294f0
290
app.cpp
290
app.cpp
@ -1,16 +1,19 @@
|
||||
#include "ev3cxx.h"
|
||||
#include "app.h"
|
||||
#include <time.h>
|
||||
#include <ctime>
|
||||
#include <functional>
|
||||
#include <sstream>
|
||||
#include <math.h>
|
||||
#include <cfloat>
|
||||
#include <limits.h>
|
||||
#include <iomanip>
|
||||
#include <cstring>
|
||||
|
||||
/*
|
||||
Change notes:
|
||||
|
||||
0.4.3 - 'HELGA' (#)
|
||||
0.5.0 - 'HELGA'
|
||||
Added diagnostics screen (opened by pressing left after boot)
|
||||
Added secret quiet mode
|
||||
|
||||
0.4.3 - 'HELGA' (#70)
|
||||
Motor problems indicator with colors:
|
||||
left - red, right - orange,
|
||||
when motor is stuck change LED color
|
||||
@ -42,7 +45,7 @@ Re-created structs for version, MPWRS, note
|
||||
Re-created function for opening and closing door and function for generating Version ID
|
||||
*/
|
||||
|
||||
/* Structure to hole current version (major, minor, patch, codename, id, release_date)
|
||||
/* 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
|
||||
@ -81,8 +84,131 @@ struct note
|
||||
int32_t duration;
|
||||
};
|
||||
|
||||
MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
void cleanAndTitle()
|
||||
{
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, " DOBREMYSL ");
|
||||
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)
|
||||
{
|
||||
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
|
||||
|
||||
int pages = 2;
|
||||
int page = 0;
|
||||
while (true)
|
||||
{
|
||||
switch (page)
|
||||
{
|
||||
case 0:
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, " DOBREMYSL ");
|
||||
ev3cxx::display.format(1, "*****************");
|
||||
ev3cxx::display.format(2, "Ver.: % .% .% ") % currentVersion.major % currentVersion.minor % currentVersion.patch;
|
||||
ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id;
|
||||
ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
|
||||
ev3cxx::display.format(5, "Rel.: % ") % std::asctime(¤tVersion.relDate);
|
||||
ev3cxx::display.format(7, "-----------------");
|
||||
break;
|
||||
case 1:
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, "Bat.: % mV") % batteryLevel;
|
||||
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, "");
|
||||
break;
|
||||
case 2:
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, "% ");
|
||||
ev3cxx::display.format(1, "");
|
||||
break;
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// TimeStamp parser
|
||||
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;
|
||||
}
|
||||
|
||||
// Function to create new program version
|
||||
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)
|
||||
{
|
||||
version retVersion;
|
||||
retVersion.id = versionID;
|
||||
@ -90,12 +216,13 @@ version createVersion(int versionID, const char *codename, int major, int minor,
|
||||
retVersion.minor = minor;
|
||||
retVersion.patch = patch;
|
||||
retVersion.codename = codename;
|
||||
retVersion.relDate.tm_sec = 0;
|
||||
retVersion.relDate.tm_min = minute;
|
||||
retVersion.relDate.tm_hour = hour;
|
||||
retVersion.relDate.tm_mday = day;
|
||||
retVersion.relDate.tm_mon = month;
|
||||
retVersion.relDate.tm_year = year - 1900;
|
||||
/*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;
|
||||
retVersion.relDate.tm_isdst = 0;
|
||||
std::mktime(&retVersion.relDate);
|
||||
|
||||
@ -114,31 +241,44 @@ void close_door(ev3cxx::Motor hinge)
|
||||
hinge.onForDegrees(25, 200);
|
||||
}
|
||||
|
||||
void turn(ev3cxx::MotorTank naVodu, ev3cxx::GyroSensor naklonoMer)
|
||||
/*
|
||||
Function for turning
|
||||
Robot can turn only left
|
||||
*/
|
||||
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2)
|
||||
{
|
||||
int startAngle = naklonoMer.angle();
|
||||
int endAngle = startAngle - 90;
|
||||
naVodu.onForRotations(-25, 50, 1.25);
|
||||
if(ev3cxx::abs(naklonoMer.angle()) > ev3cxx::abs(endAngle) + 2 || ev3cxx::abs(naklonoMer.angle()) < ev3cxx::abs(endAngle) - 2) { // !!!! NEFUNGUJEž
|
||||
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);
|
||||
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);
|
||||
|
||||
bool rotating = true;
|
||||
while (rotating)
|
||||
{
|
||||
int currAngle = ev3cxx::abs(gyro.angle());
|
||||
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle) &&
|
||||
(currAngle < av3cxx::abs(endAngle + THRESHOLD)))
|
||||
{
|
||||
motors.off(brake = true);
|
||||
rotating = false;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
Function to pick up all cubes on shorter side
|
||||
return true if ran till the end and false if stopped by middle button
|
||||
*/
|
||||
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro)
|
||||
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 = -2;
|
||||
const int RIGHT_THRESHOLD = 2;
|
||||
const int CORRECTION_MULTIPLIER = 20;
|
||||
const int CYCLE_LIMIT = 90;
|
||||
const int LEFT_THRESHOLD = -THRESHOLD;
|
||||
const int RIGHT_THRESHOLD = THRESHOLD;
|
||||
|
||||
gyro.resetHard();
|
||||
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
||||
@ -229,33 +369,19 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
|
||||
return !error;
|
||||
}
|
||||
|
||||
MPWRSPlus calculate_motor_pwrs()
|
||||
{
|
||||
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 = 85 + SPEED_MODIFIER;
|
||||
idealMPWRS.rMotorPWR = 70 + SPEED_MODIFIER;
|
||||
|
||||
MPWRSPlus retVal;
|
||||
retVal.motor_powers = idealMPWRS;
|
||||
retVal.SPEED_MODIFIER = SPEED_MODIFIER;
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void main_task(intptr_t unused)
|
||||
{
|
||||
|
||||
const int CYCLE_LIMIT_1 = 90;
|
||||
const int THRESHOLD = 2;
|
||||
const int TURNING_THRESHOLD = 1;
|
||||
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(70, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
|
||||
const version VERSION = createVersion(72, "URSULA", 0, 5, 0);
|
||||
|
||||
// Set-up screen
|
||||
ev3cxx::display.resetScreen();
|
||||
@ -271,13 +397,29 @@ void main_task(intptr_t unused)
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
||||
|
||||
// Print version information on screen
|
||||
ev3cxx::display.format(" DOBREMYSL\n% .% .% #% \nNAME: % \nBattery: % mV % \n % ") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER;
|
||||
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
// Set up motors
|
||||
ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE); // Hinge motor
|
||||
ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
|
||||
ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor
|
||||
ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4); // Touch sensor
|
||||
|
||||
// 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)
|
||||
|
||||
if(btnDown.isPressed()){
|
||||
volume = 10;
|
||||
}
|
||||
|
||||
// Print version information on screen
|
||||
cleanAndTitle();
|
||||
ev3cxx::display.format(2, "% .% .% #% \nNAME: % \nBattery: % mV % \nPress ENTR to run") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER;
|
||||
|
||||
// Play starting melody
|
||||
ev3_speaker_set_volume(100);
|
||||
ev3_speaker_set_volume(volume);
|
||||
ev3_speaker_play_tone(NOTE_C5, 400);
|
||||
tslp_tsk(500);
|
||||
ev3_speaker_play_tone(NOTE_F5, 400);
|
||||
@ -295,20 +437,36 @@ void main_task(intptr_t unused)
|
||||
ev3_speaker_play_tone(NOTE_F4, 75);
|
||||
tslp_tsk(200);
|
||||
|
||||
// Set up motors
|
||||
ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE); // Hinge motor
|
||||
ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
|
||||
ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor
|
||||
|
||||
// Set up buttons
|
||||
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
||||
|
||||
// Start program
|
||||
btnEnter.waitForClick();
|
||||
while (1 == 1)
|
||||
{
|
||||
// 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);
|
||||
}
|
||||
if (btnEnter.isPressed() || touchS.isPressed())
|
||||
{
|
||||
cleanAndTitle();
|
||||
break;
|
||||
}
|
||||
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++)
|
||||
{
|
||||
turn(motors, gyro);
|
||||
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);
|
||||
@ -334,7 +492,7 @@ void main_task(intptr_t unused)
|
||||
ev3_speaker_play_tone(NOTE_C4, 12);
|
||||
tslp_tsk(25);
|
||||
}
|
||||
bool side_1 = false;//run_short_side(motors, motor_powers, gyro);
|
||||
bool side_1 = false; // run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MLUTIPLIER, THRESHOLD);
|
||||
|
||||
if (!side_1)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user