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

Denk proof backup #3

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

BIN
app
View File

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)
{
const int LEFT_THRESHOLD = -3;
const int RIGHT_THRESHOLD = 3;
const int LEFT_THRESHOLD = -2;
const int RIGHT_THRESHOLD = 2;
const int CORRECTION_MULTIPLIER = 2;
const int CYCLE_LIMIT = 0;//75;
const int CYCLE_LIMIT = 95;
gyro.resetHard();
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.rMotorPWR = idealMPWRS.rMotorPWR;
int lCounter = 0;
int rCounter = 0;
bool run = true;
int cycleCounter = 0;
int lastRProblem = 0;
int lastLProblem = 0;
while (run)
{
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
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
// To the left
if (angle > LEFT_THRESHOLD) {
if (angle < LEFT_THRESHOLD) {
//ev3_speaker_play_tone(NOTE_A5, 250);
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);
// Check if the motor is stuck
if(lPower == 0){
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
} else if (angle < RIGHT_THRESHOLD) {
} else if (angle > RIGHT_THRESHOLD) {
//ev3_speaker_play_tone(NOTE_A4, 250);
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);
// Check if the motor is stuck
if(rPower == 0){
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
// 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
ev3cxx::display.resetScreen();
@ -256,9 +210,10 @@ void main_task(intptr_t unused)
tslp_tsk(200);
// Set up motor powers
const int SPEED_MODIFIER = 30;
MPWRS idealMPWRS;
idealMPWRS.lMotorPWR = 85;
idealMPWRS.rMotorPWR = 50;
idealMPWRS.lMotorPWR = 70 - SPEED_MODIFIER;
idealMPWRS.rMotorPWR = 50 - SPEED_MODIFIER;
MPWRS motor_powers;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;