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

Denk proof

Browse Source
This commit is contained in:
Blboun3
2023-11-14 16:45:45 +01:00
parent 095aef2d3f
commit 64d3c294f0
2 changed files with 224 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
app
View File

Binary file not shown.

290
app.cpp
View File

@@ -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(&currentVersion.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)
{