0.9.0 - 'KATRIN' (#90)

0.9.0 - 'KATRIN' (#90)
now I have no idea what am I doing... but it does something
This commit is contained in:
Blboun3 2023-11-20 09:58:49 +01:00
parent f73ac6eb88
commit 5dae59930a
4 changed files with 130 additions and 56 deletions

View File

@ -9,7 +9,7 @@ Members:
*Postavit a naprogramovat robota tak, aby samostatně, bez jakékoli další pomoci sebral co nejvíc barevných kostek ze soutěžní plochy (hřiště) a umístil je dovnitř žlutě vyznačeného obdélníku na hracím hřišti a současně postavil z těchto barevných kostek co nejvyšší věž. Pro snazší orientaci jsou na hřišti vyznačeny naváděcí černé čáry. O vítězství rozhoduje počet získaných bodů. Body robot získává za způsob umístění barevných kostek uvnitř žlutě vyznačeného obdélníku. Většina soutěže bude organizována jako vzájemný zápas dvou robotů na dvou samostatných hřištích*
zdroj: https://robosoutez.fel.cvut.cz/zadani-soutezni-ulohy-vez-tower
zdroj: [https://robosoutez.fel.cvut.cz/zadani-soutezni-ulohy-vez-tower](https://robosoutez.fel.cvut.cz/zadani-soutezni-ulohy-vez-tower)
## > Konstrukce
Konstrukce se zásobníkem na 8 kostek, a pásem na přepravu kostek do zásobníku. Jeden motor pohání pás a levé kolo, druhý motor pohání přední kola k nabírání kostek a pravé kolo. Servo ovládá zadní dveře.

BIN
app

Binary file not shown.

171
app.cpp
View File

@ -7,6 +7,9 @@
/*
CHANGELOG:
0.9.0 - 'KATRIN' (#90)
now I have no idea what am I doing... but it does something
0.8.1 - 'MONIKA' (#86)
Fixed error correction when brick stuck (changed motor speeds to fix the angle)
@ -123,7 +126,9 @@ struct allValues
int TURNING_FACTOR_CORRECTION;
int CORRECTION_MULTIPLIER;
int shortOneCycleLimit;
int loneOneCycleLimit;
int longOneCycleLimit;
int longTwoCycleLimit;
double RATIO;
};
/// @brief Function to calculate motor powers using quadratic equation, depending on battery powerMPWRSPlus
@ -216,13 +221,15 @@ void displayAllValues(allValues values)
ev3cxx::display.format(3, "TT: % ") % values.turningThreshold;
ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER;
ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION;
ev3cxx::display.format(6, "-------_1_-------");
ev3cxx::display.format(7, "LOCL: % ") % values.shortOneCycleLimit;
ev3cxx::display.format(6, "-------_G_-------");
ev3cxx::display.format(7, "RATIO: % ") % values.RATIO;
break;
case 2:
ev3cxx::display.resetScreen();
ev3cxx::display.format(0, "-------_2_-------");
ev3cxx::display.format(1, "LOCL: % ") % values.shortOneCycleLimit;
ev3cxx::display.format(0, "-------_1_-------");
ev3cxx::display.format(1, "LOCL: % ") % values.longOneCycleLimit;
ev3cxx::display.format(2, "-------_2_-------");
ev3cxx::display.format(3, "LOCL: % ") % values.longTwoCycleLimit;
break;
case 3:
ev3cxx::display.resetScreen();
@ -339,12 +346,24 @@ version createVersion(int versionID, const char *codename, int major, int minor,
return retVersion;
}
/// @brief Function to calculate faster motor speed by using ratio and slower motor speed
/// @param slower_motor_speed int: slower motor speed
/// @param RATIO double: ration betwenn faster and slower wheel
/// @return int[2]: [slower motor speed, faster motor speed]
int* calculate_ratio(int slower_motor_speed, double RATIO)
{
int* retVal = new int[2];
retVal[0] = slower_motor_speed;
retVal[1] = (int)round(slower_motor_speed * RATIO);
return retVal;
}
/// @brief Function to open door
/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door
void open_door(ev3cxx::Motor hinge)
{
hinge.off();
hinge.onForDegrees(-25, 200);
hinge.onForDegrees(-25, 180);
}
/// @brief Function to close door
@ -352,9 +371,9 @@ void open_door(ev3cxx::Motor hinge)
void close_door(ev3cxx::Motor hinge)
{
hinge.off();
hinge.onForDegrees(25, 200, false, false);
ev3_motor_set_power(HINGE_MOTOR, 25);
tslp_tsk(1000);
hinge.off(true);
ev3_motor_set_power(HINGE_MOTOR, 0);
}
/// @brief Function for turning
@ -378,7 +397,7 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
int counter = 0;
while (rotating && counter < 250)
{
int currAngle = ev3cxx::abs(gyro.angle());
int currAngle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
ev3cxx::display.format(3, "Angle: % ") % currAngle;
ev3cxx::display.format(4, "Counter: % ") % counter;
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
@ -396,8 +415,8 @@ 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))
if ((endAngle - THRESHOLD < ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT))) &&
(ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)) < endAngle + THRESHOLD))
{
turn(motors, gyro, endAngle, THRESHOLD, TFC);
}
@ -428,7 +447,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
int counter = 0;
while (counter < 4000)
{
int angle = ev3cxx::abs(gyro.angle());
int angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
ev3cxx::display.format(3, "angle: % ") % angle;
if ((min < angle) && (angle < max))
@ -443,7 +462,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
motors.on(15, 5);
while (innerRun)
{
int inner_angle = ev3cxx::abs(gyro.angle());
int inner_angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
ev3cxx::display.format(3, "angle: % ") % inner_angle;
if (inner_angle < (endAngle + THRESHOLD))
{
@ -459,7 +478,7 @@ void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngl
bool innerRun = true;
while (innerRun)
{
int inner_angle = ev3cxx::abs(gyro.angle());
int inner_angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
ev3cxx::display.format(3, "angle: % ") % inner_angle;
if (inner_angle > (endAngle - THRESHOLD))
{
@ -518,7 +537,7 @@ bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSens
// Get power of both motors
int lPower = motors.leftMotor().currentPower();
int rPower = motors.rightMotor().currentPower();
int angle = gyro.angle();
int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
@ -610,14 +629,11 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
{
cleanAndTitle();
const int LEFT_THRESHOLD = -(THRESHOLD * 3);
const int RIGHT_THRESHOLD = (THRESHOLD * 3);
gyro.resetHard();
ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
MPWRS motor_powers;
// Reset both motor's powers
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR + 10;
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
bool run = true;
@ -629,19 +645,21 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
while (run)
{
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
// motors.on(motor_powers.lMotorPWR + 10, motor_powers.rMotorPWR);
ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR);
ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR);
tslp_tsk(50);
// Reset both motor's powers
motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 5;
// Get power of both motors
int lPower = motors.leftMotor().currentPower();
int rPower = motors.rightMotor().currentPower();
int angle = gyro.angle();
int lPower = ev3_motor_get_power(LEFT_MOTOR);
int rPower = ev3_motor_get_power(RIGHT_MOTOR);
int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;
ev3cxx::display.format(4, "Cycles: % \nAngle: % ") % cycleCounter % angle;
if (lPower == 0 || rPower == 0)
{
@ -659,9 +677,9 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
lastError = cycleCounter;
// If this is third error cycle in row
if (errorStrike == 3)
if (errorStrike == 5)
{
ev3_speaker_play_tone(NOTE_F6, 50);
ev3_speaker_play_tone(NOTE_G5, 50);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
// Reset counters etc.
lastError = 0;
@ -669,17 +687,29 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
// Back up a bit
motors.off(true);
motors.on(-80, -40);
tslp_tsk(350);
tslp_tsk(450);
continue;
}
}
if (angle < -2)
// Turning left
if (angle < -3)
{
motor_powers.lMotorPWR = 100;
motor_powers.rMotorPWR = 40;
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
tslp_tsk(100);
// Stop the right wheel and boost left wheel
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
ev3_speaker_play_tone(NOTE_F4, 100);
motor_powers.lMotorPWR += 30;
motor_powers.rMotorPWR -= 20;
}
// Turning right
if (angle > 5)
{
// Stop the left wheel and boost right wheel
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
ev3_speaker_play_tone(NOTE_F6, 100);
motor_powers.lMotorPWR -= 10;
motor_powers.rMotorPWR += 30;
}
// Emergency break using middle button (BTN_ENTER)
@ -693,7 +723,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
// Exit when front bumper is hit
if (bumper.isPressed())
{
ev3_speaker_play_tone(NOTE_F4, 100);
ev3_speaker_play_tone(NOTE_C4, 100);
motors.off(true);
// Drive to the wall annd try to pickup all the bricks or get them to the container
@ -734,7 +764,7 @@ bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSenso
// Back up a bit
motors.off(true);
motors.on(-80, -40);
tslp_tsk(350);
tslp_tsk(450);
continue;
}
}
@ -797,7 +827,8 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
while (run)
{
motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR);
ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR);
tslp_tsk(50);
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
@ -806,10 +837,9 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;
// Get power of both motors
int lPower = motors.leftMotor().currentPower();
int rPower = motors.rightMotor().currentPower();
int angle = gyro.angle();
int lPower = ev3_motor_get_power(LEFT_MOTOR);
int rPower = ev3_motor_get_power(RIGHT_MOTOR);
int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle;
// Emergency break using middle button (BTN_ENTER)
@ -847,7 +877,7 @@ bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSen
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
}
if (lPower == 0 || rPower == 0 || angle < -2)
if (lPower == 0 || rPower == 0)
{
// If error in previous cycle
if (lastError == (cycleCounter - 1))
@ -909,7 +939,7 @@ void music_task(intptr_t unused)
{
int PLAY_SPEED = 250;
note erika[72];
note erika[95];
//
erika[0] = {NOTE_E4, 3};
erika[1] = {NOTE_F4, 1};
@ -998,7 +1028,34 @@ void music_task(intptr_t unused)
erika[69] = {NOTE_F5, 2};
erika[70] = {NOTE_E5, 5};
//
erika[71] = {0, 3};
erika[71] = {NOTE_E4, 3};
erika[72] = {NOTE_F4, 1};
erika[73] = {NOTE_G4, 2};
erika[74] = {NOTE_G4, 2};
erika[75] = {NOTE_G4, 2};
erika[76] = {NOTE_C5, 2};
erika[77] = {NOTE_C5, 2};
erika[78] = {NOTE_E5, 2};
erika[79] = {NOTE_E5, 3};
erika[80] = {NOTE_D5, 1};
erika[81] = {NOTE_C5, 1};
//
erika[82] = {0, 1};
erika[83] = {0, 2};
erika[84] = {0, 4};
//
erika[85] = {NOTE_B4, 2};
erika[86] = {NOTE_C5, 2};
erika[87] = {NOTE_D5, 1};
//
erika[88] = {0, 1};
erika[89] = {0, 2};
erika[90] = {0, 4};
//
erika[91] = {NOTE_E5, 3};
erika[92] = {NOTE_D5, 1};
erika[93] = {NOTE_C5, 1};
erika[94] = {0, 3};
int i = 0;
for (i = 0; i < 1000; i++)
@ -1007,9 +1064,9 @@ void music_task(intptr_t unused)
tslp_tsk(2);
}
for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito-=2)
for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito -= 2)
{
ev3_speaker_play_tone((250 +ito), 2);
ev3_speaker_play_tone((250 + ito), 2);
tslp_tsk(2);
}
@ -1053,6 +1110,7 @@ void main_task(intptr_t unused)
{
const int CYCLE_LIMIT_1 = 350;
const int CYCLE_LIMIT_2 = 350;
const double RATIO = 1.42;
const int THRESHOLD = 2;
const int TURNING_THRESHOLD = 1;
const int TURNING_FACTOR_CORRECTION = 5;
@ -1061,14 +1119,15 @@ void main_task(intptr_t unused)
int volume = 100;
// Create version info
const version VERSION = createVersion(85, "MONIKA", 0, 8, 1);
const version VERSION = createVersion(90, "KATRIN", 0, 9, 0);
// Set-up screen
ev3cxx::display.resetScreen();
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
// Set up motor powers
MPWRSPlus calcedPWRS = calculate_motor_pwrs(71, 50);
int* rationalized_speeds = calculate_ratio(45, RATIO);
MPWRSPlus calcedPWRS = calculate_motor_pwrs(rationalized_speeds[1], rationalized_speeds[0]);
MPWRS idealMPWRS = calcedPWRS.motor_powers;
const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;
@ -1082,7 +1141,9 @@ void main_task(intptr_t unused)
// Set up 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)
@ -1118,14 +1179,15 @@ void main_task(intptr_t unused)
values.turningThreshold = TURNING_THRESHOLD;
values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION;
values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER;
values.shortOneCycleLimit = CYCLE_LIMIT_1;
values.loneOneCycleLimit = CYCLE_LIMIT_2;
values.shortOneCycleLimit = 0;
values.longOneCycleLimit = CYCLE_LIMIT_1;
values.longTwoCycleLimit = CYCLE_LIMIT_2;
values.RATIO = RATIO;
// Start program
start_program_exe(btnLeft, btnEnter, touchS, values);
act_tsk(SUB_TASK);
// act_tsk(MUSIC_TASK);
ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(200);
@ -1146,8 +1208,13 @@ void main_task(intptr_t unused)
}
gyro.resetHard();
motors.onForRotations(-(motor_powers.rMotorPWR + 25), -motor_powers.lMotorPWR, 3.25);
motors.onForRotations(-(motor_powers.rMotorPWR + 25), -(motor_powers.lMotorPWR - 25), 3.25);
// turn 90 degress left
turn_forever(motors, gyro, 30, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR);
ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR);
tslp_tsk(250);
motors.off(true);
turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
// motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
@ -1201,5 +1268,5 @@ void main_task(intptr_t unused)
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
motors.off(true);
act_tsk(MUSIC_TASK);
tslp_tsk(120000);
}

7
app.h
View File

@ -75,6 +75,13 @@ extern "C" {
#define PRIORITY_PRD_TSK_1 TMIN_APP_TPRI
#define PRIORITY_PRD_TSK_2 TMIN_APP_TPRI
#define GYRO_PORT EV3_PORT_1
#define BUTTON_UP EV3_PORT_4
#define BUTTON_BUMPER EV3_PORT_3
#define LEFT_MOTOR EV3_PORT_B
#define RIGHT_MOTOR EV3_PORT_C
#define HINGE_MOTOR EV3_PORT_A
/**
* Task periods in ms
*/