0.4.2 - 'HELGA' (#67)

Updated run_short() to run_short_side()
Added gyro
Done calculations with gyro to EC driving straight
TODO: problems with battery
This commit is contained in:
Blboun3 2023-11-10 15:43:21 +01:00
parent 3d971d9f55
commit f3928650a5
2 changed files with 55 additions and 38 deletions

BIN
app

Binary file not shown.

67
app.cpp
View File

@ -8,8 +8,11 @@
/* /*
Change notes: Change notes:
0.4.0 - 'HELGA' (#50) 0.4.2 - 'HELGA' (#64)
Fixed Updated run_short() to run_short_side()
Added gyro
Done calculations with gyro to EC driving straight
TODO: problems with battery
0.3.0 - 'INGRID' (#42) 0.3.0 - 'INGRID' (#42)
Some weird magic in calculations of motor speeds Some weird magic in calculations of motor speeds
@ -116,6 +119,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
bool run = true; bool run = true;
bool error = false;
int cycleCounter = 0; int cycleCounter = 0;
while (run) while (run)
@ -123,7 +127,6 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
tslp_tsk(50); tslp_tsk(50);
// Reset both motor's powers // Reset both motor's powers
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
@ -139,30 +142,36 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
if (btnEnter.isPressed()) if (btnEnter.isPressed())
{ {
run = false; run = false;
return false; error = true;
motors.off(true);
} }
// Check gyro angle and change driving speed to fix the angle // Check gyro angle and change driving speed to fix the angle
// To the left // To the left
if (angle < LEFT_THRESHOLD) { if (angle < LEFT_THRESHOLD)
{
// ev3_speaker_play_tone(NOTE_A5, 250); // ev3_speaker_play_tone(NOTE_A5, 250);
int correction = ev3cxx::abs(angle - LEFT_THRESHOLD); int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction); motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
ev3_speaker_play_tone(correction * 1000, 30); ev3_speaker_play_tone(correction * 1000, 30);
// Check if the motor is stuck // Check if the motor is stuck
if(lPower == 0){ if (lPower == 0)
{
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
ev3_speaker_play_tone(NOTE_A5, 250); ev3_speaker_play_tone(NOTE_A5, 250);
} }
// To the right // To the right
} else if (angle > RIGHT_THRESHOLD) { }
else if (angle > RIGHT_THRESHOLD)
{
// ev3_speaker_play_tone(NOTE_A4, 250); // ev3_speaker_play_tone(NOTE_A4, 250);
int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD); int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD);
motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER; motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
ev3_speaker_play_tone(correction * 1000, 30); ev3_speaker_play_tone(correction * 1000, 30);
// Check if the motor is stuck // Check if the motor is stuck
if(rPower == 0){ if (rPower == 0)
{
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
ev3_speaker_play_tone(NOTE_A4, 250); ev3_speaker_play_tone(NOTE_A4, 250);
} }
@ -170,27 +179,39 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
cycleCounter++; cycleCounter++;
if(cycleCounter == CYCLE_LIMIT) { if (cycleCounter == CYCLE_LIMIT)
{
run = false; run = false;
} }
} }
return true; 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);
ev3_speaker_play_tone(NOTE_C4, 250);
tslp_tsk(250);
ev3_speaker_play_tone(NOTE_B4, 750);
tslp_tsk(750);
ev3_speaker_play_tone(NOTE_G4, 750);
tslp_tsk(750);
return !error;
} }
void main_task(intptr_t unused) void main_task(intptr_t unused)
{ {
// Create version info // 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) // 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(62, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40); const version VERSION = createVersion(67, "HELGA", 0, 4, 2, 10, 11, 2023, 14, 40);
// Set-up screen // Set-up screen
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
ev3cxx::display.setFont(EV3_FONT_MEDIUM); ev3cxx::display.setFont(EV3_FONT_MEDIUM);
// Print version information on screen // Print version information on screen
ev3cxx::display.format(" DOBREMYSL\nVERSION: % .% .% #% \nNAME: % ") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename; ev3cxx::display.format(" DOBREMYSL\nVERSION: % .% .% #% \nNAME: % \nBattery: % mV") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV();
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN); ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
@ -214,10 +235,10 @@ void main_task(intptr_t unused)
tslp_tsk(200); tslp_tsk(200);
// Set up motor powers // Set up motor powers
const int SPEED_MODIFIER = 30; const int SPEED_MODIFIER = 10;
MPWRS idealMPWRS; MPWRS idealMPWRS;
idealMPWRS.lMotorPWR = 70 - SPEED_MODIFIER; idealMPWRS.lMotorPWR = 75 - SPEED_MODIFIER;
idealMPWRS.rMotorPWR = 50 - SPEED_MODIFIER; idealMPWRS.rMotorPWR = 60 - SPEED_MODIFIER;
MPWRS motor_powers; MPWRS motor_powers;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
@ -237,16 +258,12 @@ void main_task(intptr_t unused)
// turn_left(motors); // turn_left(motors);
bool side_1 = run_short_side(motors, motor_powers, gyro); bool side_1 = run_short_side(motors, motor_powers, gyro);
ev3_speaker_play_tone(NOTE_C4, 250); if (!side_1)
ev3_speaker_play_tone(NOTE_C4, 125); {
ev3_speaker_play_tone(NOTE_D4, 250);
ev3_speaker_play_tone(NOTE_C4, 250);
ev3_speaker_play_tone(NOTE_B4, 750);
if(!side_1) {
return; return;
} else { }
ev3_speaker_play_tone(NOTE_G5, 500); else
{
return; return;
} }
} }