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.

93
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
@ -97,7 +100,7 @@ void close_door(ev3cxx::Motor hinge)
hinge.onForDegrees(25, 200); hinge.onForDegrees(25, 200);
} }
/* /*
Function to pick up all cubes on shorter side Function to pick up all cubes on shorter side
return true if ran till the end and false if stopped by middle button return true if ran till the end and false if stopped by middle button
*/ */
@ -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,58 +142,76 @@ 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) { }
//ev3_speaker_play_tone(NOTE_A4, 250); else if (angle > RIGHT_THRESHOLD)
{
// 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);
} }
} }
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;
@ -226,7 +247,7 @@ void main_task(intptr_t unused)
// Set up motors // Set up motors
ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE); // Hinge motor ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE); // Hinge motor
ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor
// Set up buttons // Set up buttons
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
@ -234,19 +255,15 @@ void main_task(intptr_t unused)
// Start program // Start program
btnEnter.waitForClick(); btnEnter.waitForClick();
//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;
} }
} }