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

Denk proof backup #too_many

Browse Source
This commit is contained in:
Blboun3 2023-11-14 11:14:18 +01:00
parent 37d921b914
commit 095aef2d3f
2 changed files with 93 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
app
View File

Binary file not shown.

108
app.cpp
View File

@ -4,10 +4,17 @@
#include <functional>
#include <sstream>
#include <math.h>
#include <cfloat>
#include <limits.h>
/*
Change notes:
0.4.3 - 'HELGA' (#)
Motor problems indicator with colors:
left - red, right - orange,
when motor is stuck change LED color
0.4.2 - 'HELGA' (#67)
Updated run_short() to run_short_side()
Added gyro
@ -60,6 +67,13 @@ struct MPWRS
int rMotorPWR;
};
// Structure to hold motor powers and speed modifier
struct MPWRSPlus
{
MPWRS motor_powers;
int SPEED_MODIFIER;
};
// Structure to hold notes
struct note
{
@ -100,6 +114,21 @@ void close_door(ev3cxx::Motor hinge)
hinge.onForDegrees(25, 200);
}
void turn(ev3cxx::MotorTank naVodu, ev3cxx::GyroSensor naklonoMer)
{
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);
}
}
/*
Function to pick up all cubes on shorter side
return true if ran till the end and false if stopped by middle button
@ -108,7 +137,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
{
const int LEFT_THRESHOLD = -2;
const int RIGHT_THRESHOLD = 2;
const int CORRECTION_MULTIPLIER = 10;
const int CORRECTION_MULTIPLIER = 20;
const int CYCLE_LIMIT = 90;
gyro.resetHard();
@ -184,6 +213,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
run = false;
}
}
motors.off(true);
ev3_speaker_play_tone(NOTE_C4, 250);
tslp_tsk(250);
ev3_speaker_play_tone(NOTE_C4, 125);
@ -199,19 +229,50 @@ 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)
{
// 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(67, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
const version VERSION = createVersion(70, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
// Set-up screen
ev3cxx::display.resetScreen();
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
// Set up motor powers
MPWRSPlus calcedPWRS = calculate_motor_pwrs();
MPWRS idealMPWRS = calcedPWRS.motor_powers;
const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
MPWRS motor_powers;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
// Print version information on screen
ev3cxx::display.format(" DOBREMYSL\nVERSION: % .% .% #% \nNAME: % \nBattery: % mV") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV();
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);
@ -234,16 +295,6 @@ void main_task(intptr_t unused)
ev3_speaker_play_tone(NOTE_F4, 75);
tslp_tsk(200);
// Set up motor powers
const int SPEED_MODIFIER = 10;
MPWRS idealMPWRS;
idealMPWRS.lMotorPWR = 75 - SPEED_MODIFIER;
idealMPWRS.rMotorPWR = 60 - SPEED_MODIFIER;
MPWRS motor_powers;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
// 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
@ -255,8 +306,35 @@ void main_task(intptr_t unused)
// Start program
btnEnter.waitForClick();
// turn_left(motors);
bool side_1 = run_short_side(motors, motor_powers, gyro);
for (int i = 0; i < 10; i++)
{
turn(motors, gyro);
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);
if (!side_1)
{