0.7.0 - 'URSULA'
0.7.0 - 'URSULA' (#80) Done first short and long side Modified turn function Changed drive plan (long - long - middle - short - short - middle)
This commit is contained in:
Blboun3
parent
b8cd70e1fa
commit
482705794d
349
app.cpp
349
app.cpp
@ -9,9 +9,18 @@
|
||||
/*
|
||||
CHANGELOG:
|
||||
|
||||
0.6.0 - 'HELGA' ()
|
||||
0.7.2 - 'URSULA' (#)
|
||||
|
||||
|
||||
0.7.0 - 'URSULA' (#80)
|
||||
Done first short and long side
|
||||
Modified turn function
|
||||
Changed drive plan (long - long - middle - short - short - middle)
|
||||
|
||||
|
||||
0.6.0 - 'HELGA' (#75)
|
||||
Added Doxygen comments
|
||||
Added volume to display_all_values (diagnostics screenS)
|
||||
Added volume to display_all_values (diagnostics screen)
|
||||
|
||||
0.5.0 - 'HELGA' (#72)
|
||||
Added diagnostics screen (opened by pressing left after boot)
|
||||
@ -90,7 +99,6 @@ struct MPWRSPlus
|
||||
int SPEED_MODIFIER;
|
||||
};
|
||||
|
||||
|
||||
/// @struct note
|
||||
/// @brief Structure for holding note (frequency and duration)
|
||||
struct note
|
||||
@ -105,7 +113,7 @@ struct note
|
||||
/// @param leftMotor int: wanted power of left motor (default 85)
|
||||
/// @param rightMotor int: wante power or right motor (default 70)
|
||||
/// @return MPWRSPlus: motor powers and SPEED_MODIFIER
|
||||
MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
|
||||
MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 60)
|
||||
{
|
||||
int batteryLevel = ev3_battery_voltage_mV();
|
||||
|
||||
@ -144,7 +152,8 @@ void cleanAndTitle()
|
||||
/// @param TURNING_FACTOR_CORRECTION int: factor for changing final turning values due to gyro sensor placement
|
||||
/// @param CORRECTION_MULTIPLIER int: for driving straight
|
||||
/// @param shortOneCycleLimit int: how many program cycles to do when running first short side
|
||||
void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit)
|
||||
/// @param loneOneCycleLimit int: how many program cycles to do when running first long side
|
||||
void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMotorPWR, int SPEED_MODIFIER, int turningThreshold, int TURNING_FACTOR_CORRECTION, int CORRECTION_MULTIPLIER, int shortOneCycleLimit, int loneOneCycleLimit)
|
||||
{
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
|
||||
@ -153,7 +162,7 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
|
||||
ev3cxx::BrickButton btnUp(ev3cxx::BrickButtons::UP); // Up button
|
||||
ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN); // Down button
|
||||
|
||||
int pages = 2;
|
||||
int pages = 3;
|
||||
int page = 0;
|
||||
while (true)
|
||||
{
|
||||
@ -178,14 +187,15 @@ void displayAllValues(version currentVersion, int volume, int lMotorPWR, int rMo
|
||||
ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
|
||||
ev3cxx::display.format(5, "TFC: % ") % TURNING_FACTOR_CORRECTION;
|
||||
ev3cxx::display.format(6, "-------_1_-------");
|
||||
ev3cxx::display.format(7, "SOCL: % ") % shortOneCycleLimit;
|
||||
ev3cxx::display.format(7, "LOCL: % ") % shortOneCycleLimit;
|
||||
break;
|
||||
case 2:
|
||||
ev3cxx::display.resetScreen();
|
||||
ev3cxx::display.format(0, "");
|
||||
ev3cxx::display.format(1, "");
|
||||
ev3cxx::display.format(2, "");
|
||||
ev3cxx::display.format(0, "-------_2_-------");
|
||||
ev3cxx::display.format(1, "LOCL: % ") % shortOneCycleLimit;
|
||||
break;
|
||||
case 3:
|
||||
ev3cxx::display.resetScreen();
|
||||
|
||||
default:
|
||||
break;
|
||||
@ -241,7 +251,6 @@ std::tm parseTimestamp(const char *timestampStr)
|
||||
return tmStruct;
|
||||
}
|
||||
|
||||
|
||||
/// @brief Function to generate version struct of the current version
|
||||
/// Includes getting and parsing time of compilation using parseTimestamp function
|
||||
/// @param versionID int: versionID, increment with every new change
|
||||
@ -285,7 +294,6 @@ void close_door(ev3cxx::Motor hinge)
|
||||
hinge.onForDegrees(25, 200);
|
||||
}
|
||||
|
||||
|
||||
/// @brief Function for turning
|
||||
/// !! ROBOT CAN TURN ONLY LEFT !!
|
||||
/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
|
||||
@ -295,6 +303,7 @@ void close_door(ev3cxx::Motor hinge)
|
||||
/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
|
||||
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
|
||||
{
|
||||
cleanAndTitle();
|
||||
endAngle = endAngle - TFC;
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
@ -324,6 +333,54 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
|
||||
tslp_tsk(20);
|
||||
counter++;
|
||||
}
|
||||
if ((endAngle - THRESHOLD < ev3cxx::abs(gyro.angle())) &&
|
||||
(ev3cxx::abs(gyro.angle()) < endAngle + THRESHOLD))
|
||||
{
|
||||
turn(motors, gyro, endAngle, THRESHOLD, TFC);
|
||||
}
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
return;
|
||||
}
|
||||
|
||||
/// @brief Function for turning (alternative way XD)
|
||||
/// !! ROBOT CAN TURN ONLY LEFT !!
|
||||
/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
|
||||
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
||||
/// @param endAngle int: angle to turn to (wanted angle, will be modified internally)
|
||||
/// @param THRESHOLD int: turning accuracy in degrees (default 2)
|
||||
/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
|
||||
void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
|
||||
{
|
||||
cleanAndTitle();
|
||||
endAngle = endAngle - TFC;
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
// MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
|
||||
// left, right, rotations (faster), brake, blocking, wait_after
|
||||
motors.on(-10, 25);
|
||||
|
||||
bool rotating = true;
|
||||
int counter = 0;
|
||||
while (rotating && counter < 25000)
|
||||
{
|
||||
int currAngle = ev3cxx::abs(gyro.angle());
|
||||
ev3cxx::display.format(3, "Angle: % ") % currAngle;
|
||||
ev3cxx::display.format(4, "Counter: % ") % counter;
|
||||
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
|
||||
{
|
||||
|
||||
int error = endAngle - currAngle;
|
||||
ev3cxx::display.format(5, "Error: % deg.") % error;
|
||||
|
||||
motors.off(true);
|
||||
rotating = false;
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
ev3_speaker_play_tone(NOTE_F5, 100);
|
||||
return;
|
||||
}
|
||||
tslp_tsk(20);
|
||||
counter++;
|
||||
}
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
return;
|
||||
}
|
||||
@ -333,12 +390,14 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
|
||||
/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
|
||||
/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
|
||||
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
||||
/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
|
||||
/// @param CYCLE_LIMIT int: how many cycle to run (default 90)
|
||||
/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
|
||||
/// @param THRESHOLD int: when to start correcting drive (in degrees) (default 2)
|
||||
/// @return bool: false if stopped by the middle button
|
||||
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
|
||||
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
|
||||
{
|
||||
cleanAndTitle();
|
||||
const int LEFT_THRESHOLD = -THRESHOLD;
|
||||
const int RIGHT_THRESHOLD = THRESHOLD;
|
||||
|
||||
@ -377,6 +436,13 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
|
||||
motors.off(true);
|
||||
}
|
||||
|
||||
// Exit when bumper hit
|
||||
if (bumper.isPressed())
|
||||
{
|
||||
run = false;
|
||||
motors.off(true);
|
||||
}
|
||||
|
||||
// Check gyro angle and change driving speed to fix the angle
|
||||
// To the left
|
||||
if (angle < LEFT_THRESHOLD)
|
||||
@ -390,6 +456,9 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
ev3_speaker_play_tone(NOTE_A5, 250);
|
||||
run = false;
|
||||
error = true;
|
||||
motors.off(true);
|
||||
}
|
||||
|
||||
// To the right
|
||||
@ -405,6 +474,9 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
|
||||
ev3_speaker_play_tone(NOTE_A4, 250);
|
||||
run = false;
|
||||
error = true;
|
||||
motors.off(true);
|
||||
}
|
||||
}
|
||||
|
||||
@ -431,10 +503,189 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
|
||||
return !error;
|
||||
}
|
||||
|
||||
/// @brief Function to pick up all cubes on longer side.
|
||||
/// Returns true if ran till the end and false if stopped by middle button
|
||||
/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
|
||||
/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
|
||||
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
|
||||
/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
|
||||
/// @param CYCLE_LIMIT int: how many cycle to run (default 180)
|
||||
/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
|
||||
/// @return bool: false if stopped by the middle button
|
||||
bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 180, int CORRECTION_MULTIPLIER = 20)
|
||||
{
|
||||
cleanAndTitle();
|
||||
|
||||
gyro.resetHard();
|
||||
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
||||
MPWRS motor_powers;
|
||||
// Reset both motor's powers
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
||||
|
||||
bool run = true;
|
||||
bool error = false;
|
||||
int cycleCounter = 0;
|
||||
|
||||
while (run)
|
||||
{
|
||||
motors.on(motor_powers.lMotorPWR + 20, motor_powers.rMotorPWR - 20);
|
||||
tslp_tsk(50);
|
||||
|
||||
// Reset both motor's powers
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
||||
|
||||
// Get power of both motors
|
||||
int lPower = motors.leftMotor().currentPower();
|
||||
int rPower = motors.rightMotor().currentPower();
|
||||
int angle = gyro.angle();
|
||||
|
||||
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
|
||||
|
||||
if (angle < -2)
|
||||
{
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 100;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR - 20;
|
||||
}
|
||||
|
||||
if (angle > 5)
|
||||
{
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 40;
|
||||
}
|
||||
|
||||
// Emergency break using middle button (BTN_ENTER)
|
||||
if (btnEnter.isPressed())
|
||||
{
|
||||
run = false;
|
||||
error = true;
|
||||
motors.off(true);
|
||||
}
|
||||
|
||||
// Exit when front bumper is hit
|
||||
if (bumper.isPressed())
|
||||
{
|
||||
run = false;
|
||||
motors.off(true);
|
||||
}
|
||||
|
||||
cycleCounter++;
|
||||
|
||||
if (cycleCounter == CYCLE_LIMIT)
|
||||
{
|
||||
run = false;
|
||||
}
|
||||
}
|
||||
motors.off(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;
|
||||
}
|
||||
|
||||
/// @brief Function to drive forward till bumper button is pressed
|
||||
/// @param motors ev3cxx::MotorTank motors to use for driving
|
||||
/// @param idealMPWRS MPWRS ideal motor powers for the motors
|
||||
/// @param gyro ev3cxx::GyroSensor gyro sensor to use for error correction
|
||||
/// @param bumper ev3cxx::TouchSensor touch sensor to use a bumper for stoping when wall is hit
|
||||
/// @param CORRECTION_MULTIPLIER int for correcting errors in direction
|
||||
/// @return bool: false if stopped by the middle button
|
||||
bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CORRECTION_MULTIPLIER = 2, int THRESHOLD = 2)
|
||||
{
|
||||
cleanAndTitle();
|
||||
const int LEFT_THRESHOLD = -THRESHOLD;
|
||||
const int RIGHT_THRESHOLD = THRESHOLD;
|
||||
|
||||
gyro.resetHard();
|
||||
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
||||
MPWRS motor_powers;
|
||||
// Reset both motor's powers
|
||||
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
|
||||
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
|
||||
|
||||
bool run = true;
|
||||
bool error = false;
|
||||
|
||||
while (run)
|
||||
{
|
||||
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;
|
||||
|
||||
// Get power of both motors
|
||||
int lPower = motors.leftMotor().currentPower();
|
||||
int rPower = motors.rightMotor().currentPower();
|
||||
int angle = gyro.angle();
|
||||
|
||||
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle;
|
||||
|
||||
// Emergency break using middle button (BTN_ENTER)
|
||||
if (btnEnter.isPressed())
|
||||
{
|
||||
run = false;
|
||||
error = true;
|
||||
motors.off(true);
|
||||
}
|
||||
|
||||
// Exit when bumper hit
|
||||
if (bumper.isPressed())
|
||||
{
|
||||
run = false;
|
||||
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);
|
||||
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);
|
||||
}
|
||||
// To the right
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
motors.off(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)
|
||||
{
|
||||
|
||||
const int CYCLE_LIMIT_1 = 90;
|
||||
const int CYCLE_LIMIT_1 = 80;
|
||||
const int CYCLE_LIMIT_2 = 80;
|
||||
const int THRESHOLD = 2;
|
||||
const int TURNING_THRESHOLD = 1;
|
||||
const int TURNING_FACTOR_CORRECTION = 5;
|
||||
@ -443,8 +694,7 @@ void main_task(intptr_t unused)
|
||||
int volume = 100;
|
||||
|
||||
// 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(73, "URSULA", 0, 6, 0);
|
||||
const version VERSION = createVersion(85, "URSULA", 0, 7, 2);
|
||||
|
||||
// Set-up screen
|
||||
ev3cxx::display.resetScreen();
|
||||
@ -463,10 +713,11 @@ void main_task(intptr_t unused)
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
|
||||
// Set up motors
|
||||
ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::LARGE); // Hinge motor
|
||||
ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
|
||||
ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::MEDIUM); // Hinge motor
|
||||
ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors (Left - B; Right - C)
|
||||
ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor
|
||||
ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4); // Touch sensor
|
||||
ev3cxx::TouchSensor frontTouch(ev3cxx::SensorPort::S3); // Touch sensor (bumper)
|
||||
|
||||
// Set up buttons
|
||||
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
|
||||
@ -514,7 +765,7 @@ void main_task(intptr_t unused)
|
||||
// tslp_tsk(20);
|
||||
if (btnLeft.isPressed())
|
||||
{
|
||||
displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1);
|
||||
displayAllValues(VERSION, volume, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, SPEED_MODIFIER, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1, CYCLE_LIMIT_2);
|
||||
}
|
||||
if (btnEnter.isPressed() || touchS.isPressed())
|
||||
{
|
||||
@ -533,12 +784,64 @@ void main_task(intptr_t unused)
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
tslp_tsk(200);
|
||||
|
||||
bool side_1 = run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD);
|
||||
// Close the door before begining the drive
|
||||
close_door(hinge);
|
||||
|
||||
// Run first long side
|
||||
bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER);
|
||||
// If something happened
|
||||
if (!side_1)
|
||||
{
|
||||
// Something went wrong
|
||||
return;
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
ev3_speaker_play_tone(NOTE_FS4, 100);
|
||||
}
|
||||
|
||||
// turn 90 degress left
|
||||
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
|
||||
// Cross to the other side
|
||||
gyro.resetHard();
|
||||
bool crossing = unlimited_drive(motors, motor_powers, gyro, frontTouch, CORRECTION_MULTIPLIER, THRESHOLD);
|
||||
|
||||
// turn 90 degress left
|
||||
turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
|
||||
if (!crossing)
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
ev3_speaker_play_tone(NOTE_FS4, 100);
|
||||
}
|
||||
|
||||
// reset gyro
|
||||
gyro.resetHard();
|
||||
// Run second long side
|
||||
bool side_2 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER);
|
||||
// If something happened
|
||||
if (!side_2)
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
ev3_speaker_play_tone(NOTE_F4, 100);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
ev3_speaker_play_tone(NOTE_FS4, 100);
|
||||
}
|
||||
|
||||
// go to the middle
|
||||
motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 4.5);
|
||||
// open door
|
||||
open_door(hinge);
|
||||
motors.onForRotations(idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, 1);
|
||||
|
||||
/*bool side_2 = run_short_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER, THRESHOLD);
|
||||
if (!side_2)
|
||||
{
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
|
||||
tslp_tsk(200);
|
||||
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
|
||||
}*/
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user