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

0.4.2 - 'HELGA' (#67)

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
app
View File

Binary file not shown.

85
app.cpp
View File

@ -8,8 +8,11 @@
/*
Change notes:
0.4.0 - 'HELGA' (#50)
Fixed
0.4.2 - 'HELGA' (#64)
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)
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;
bool run = true;
bool error = false;
int cycleCounter = 0;
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);
tslp_tsk(50);
// Reset both motor's powers
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
@ -139,58 +142,76 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
if (btnEnter.isPressed())
{
run = false;
return false;
error = true;
motors.off(true);
}
// Check gyro angle and change driving speed to fix the angle
// To the left
if (angle < LEFT_THRESHOLD) {
//ev3_speaker_play_tone(NOTE_A5, 250);
if (angle < LEFT_THRESHOLD)
{
// ev3_speaker_play_tone(NOTE_A5, 250);
int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER);//(int)pow(CORRECTION_MULTIPLIER, correction);
ev3_speaker_play_tone(correction*1000,30);
motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
ev3_speaker_play_tone(correction * 1000, 30);
// Check if the motor is stuck
if(lPower == 0){
if (lPower == 0)
{
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
ev3_speaker_play_tone(NOTE_A5,250);
ev3_speaker_play_tone(NOTE_A5, 250);
}
// 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);
motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER);//(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
ev3_speaker_play_tone(correction*1000,30);
motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
ev3_speaker_play_tone(correction * 1000, 30);
// Check if the motor is stuck
if(rPower == 0){
if (rPower == 0)
{
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
ev3_speaker_play_tone(NOTE_A4,250);
ev3_speaker_play_tone(NOTE_A4, 250);
}
}
cycleCounter++;
if(cycleCounter == CYCLE_LIMIT) {
if (cycleCounter == CYCLE_LIMIT)
{
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)
{
// 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(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
ev3cxx::display.resetScreen();
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
// 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);
@ -214,10 +235,10 @@ void main_task(intptr_t unused)
tslp_tsk(200);
// Set up motor powers
const int SPEED_MODIFIER = 30;
const int SPEED_MODIFIER = 10;
MPWRS idealMPWRS;
idealMPWRS.lMotorPWR = 70 - SPEED_MODIFIER;
idealMPWRS.rMotorPWR = 50 - SPEED_MODIFIER;
idealMPWRS.lMotorPWR = 75 - SPEED_MODIFIER;
idealMPWRS.rMotorPWR = 60 - SPEED_MODIFIER;
MPWRS motor_powers;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
@ -234,19 +255,15 @@ void main_task(intptr_t unused)
// Start program
btnEnter.waitForClick();
//turn_left(motors);
// turn_left(motors);
bool side_1 = run_short_side(motors, motor_powers, gyro);
ev3_speaker_play_tone(NOTE_C4, 250);
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) {
if (!side_1)
{
return;
} else {
ev3_speaker_play_tone(NOTE_G5, 500);
}
else
{
return;
}
}