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:
parent
3d971d9f55
commit
f3928650a5
67
app.cpp
67
app.cpp
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Reference in New Issue
Block a user