Denk proof backup #3

This commit is contained in:
Blboun3 2023-11-10 13:27:12 +01:00
parent 42af19d4ce
commit b065369fac
2 changed files with 12 additions and 57 deletions

BIN
app

Binary file not shown.

69
app.cpp
View File

@ -103,10 +103,10 @@ return true if ran till the end and false if stopped by middle button
*/ */
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro) bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro)
{ {
const int LEFT_THRESHOLD = -3; const int LEFT_THRESHOLD = -2;
const int RIGHT_THRESHOLD = 3; const int RIGHT_THRESHOLD = 2;
const int CORRECTION_MULTIPLIER = 2; const int CORRECTION_MULTIPLIER = 2;
const int CYCLE_LIMIT = 0;//75; const int CYCLE_LIMIT = 95;
gyro.resetHard(); gyro.resetHard();
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
@ -115,16 +115,11 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR; motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
int lCounter = 0;
int rCounter = 0;
bool run = true; bool run = true;
int cycleCounter = 0; int cycleCounter = 0;
int lastRProblem = 0;
int lastLProblem = 0;
while (run) while (run)
{ {
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR); motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
tslp_tsk(50); tslp_tsk(50);
@ -149,66 +144,25 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
// 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 = angle - LEFT_THRESHOLD; int correction = angle - LEFT_THRESHOLD;
motor_powers.lMotorPWR += (int)pow(CORRECTION_MULTIPLIER, correction); motor_powers.lMotorPWR += correction * CORRECTION_MULTIPLIER;//(int)pow(CORRECTION_MULTIPLIER, correction);
ev3_speaker_play_tone(correction*1000,50); ev3_speaker_play_tone(correction*1000,50);
// Check if the motor is stuck // Check if the motor is stuck
if(lPower == 0){ if(lPower == 0){
ev3_speaker_play_tone(NOTE_A5,250); ev3_speaker_play_tone(NOTE_A5,250);
// Set lastProblem na cycleCounter because there is a problem
lastLProblem = cycleCounter;
// Check if lastProblem was in previous cycle
if(cycleCounter - 1 == lastLProblem){
lCounter += 1;
// If 5 problems occured than try to fix the problem
if(lCounter == 6){
ev3_speaker_play_tone(NOTE_C4, 250);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
motors.on(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR);
tslp_tsk(250);
motors.off();
lCounter = 0;
}
// Reset counter
} else {
lCounter = 0;
}
} }
// 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 = angle - RIGHT_THRESHOLD; int correction = angle - RIGHT_THRESHOLD;
motor_powers.rMotorPWR += (int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER; motor_powers.rMotorPWR += correction * CORRECTION_MULTIPLIER;//(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
ev3_speaker_play_tone(correction*1000,50); ev3_speaker_play_tone(correction*1000,50);
// Check if the motor is stuck // Check if the motor is stuck
if(rPower == 0){ if(rPower == 0){
ev3_speaker_play_tone(NOTE_A4,250); ev3_speaker_play_tone(NOTE_A4,250);
// Set lastProblem na cycleCounter because there is a problem
lastRProblem = cycleCounter;
// Check if lastProblem was in previous cycle
if(cycleCounter - 1 == lastRProblem){
rCounter += 1;
// If 5 problems occured than try to fix the problem
if(rCounter == 6){
ev3_speaker_play_tone(NOTE_C5, 250);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
motors.on(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR);
tslp_tsk(250);
motors.off();
rCounter = 0;
}
// Reset counter
} else {
rCounter = 0;
}
} }
} }
@ -227,7 +181,7 @@ 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(51, "HELGA", 0, 4, 0, 8, 11, 2023, 15, 40); const version VERSION = createVersion(54, "HELGA", 0, 4, 1, 10, 11, 2023, 12, 10);
// Set-up screen // Set-up screen
ev3cxx::display.resetScreen(); ev3cxx::display.resetScreen();
@ -256,9 +210,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;
MPWRS idealMPWRS; MPWRS idealMPWRS;
idealMPWRS.lMotorPWR = 85; idealMPWRS.lMotorPWR = 70 - SPEED_MODIFIER;
idealMPWRS.rMotorPWR = 50; idealMPWRS.rMotorPWR = 50 - SPEED_MODIFIER;
MPWRS motor_powers; MPWRS motor_powers;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR; motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;