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

RS START

Browse Source
This commit is contained in:
Blboun3 2023-11-21 09:21:12 +01:00
parent 5dae59930a
commit b8fdd81a88
4 changed files with 93 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
app
View File

Binary file not shown.

8
app.cfg
View File

@ -3,11 +3,13 @@
#include "app.h"
DOMAIN(TDOM_APP) {
CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI, STACK_SIZE, NULL });
CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
CRE_TSK(SUB_TASK, { TA_NULL, 0, sub_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
CRE_TSK(SUB_TASK, { TA_NULL, 0, sub_task, TMIN_APP_TPRI, STACK_SIZE, NULL });
CRE_TSK(MUSIC_TASK, { TA_NULL, 0, music_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
CRE_TSK(MUSIC_TASK, { TA_NULL, 0, music_task, TMIN_APP_TPRI + 3, STACK_SIZE, NULL });
CRE_TSK(CSV_TASK, { TA_NULL, 0, csv_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
// periodic task PRD_TSK_1 that will start automatically
//CRE_TSK(PRD_TSK_1, { TA_NULL, 0, periodic_task_1, PRIORITY_PRD_TSK_1, STACK_SIZE, NULL });

121
app.cpp
View File

@ -3,6 +3,8 @@
#include <ctime>
#include <math.h>
#include <iomanip>
#include <stdio.h>
#include <string.h>
/*
CHANGELOG:
@ -350,9 +352,9 @@ version createVersion(int versionID, const char *codename, int major, int minor,
/// @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 *calculate_ratio(int slower_motor_speed, double RATIO)
{
int* retVal = new int[2];
int *retVal = new int[2];
retVal[0] = slower_motor_speed;
retVal[1] = (int)round(slower_motor_speed * RATIO);
return retVal;
@ -373,7 +375,6 @@ void close_door(ev3cxx::Motor hinge)
hinge.off();
ev3_motor_set_power(HINGE_MOTOR, 25);
tslp_tsk(1000);
ev3_motor_set_power(HINGE_MOTOR, 0);
}
/// @brief Function for turning
@ -397,7 +398,7 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
int counter = 0;
while (rotating && counter < 250)
{
int currAngle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
int currAngle = ev3cxx::abs(gyro.angle());
ev3cxx::display.format(3, "Angle: % ") % currAngle;
ev3cxx::display.format(4, "Counter: % ") % counter;
if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
@ -415,6 +416,7 @@ void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90,
tslp_tsk(20);
counter++;
}
if ((endAngle - THRESHOLD < ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT))) &&
(ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)) < endAngle + THRESHOLD))
{
@ -1080,14 +1082,78 @@ void music_task(intptr_t unused)
}
}
void csv_task(intptr_t unused)
{
FILE *file;
const char *filename = "/ev3rt/data.csv";
// Open the file for writing
file = fopen(filename, "w");
if (file == NULL)
{
ev3_speaker_play_tone(NOTE_E6, 10000);
tslp_tsk(10000);
}
const char *text = "sep=;\ntime;battery;current;gyro_angle;gyro_rate;left_motor;left_motor_counts;right_motor;right_motor_counts;hinge_motor;top;bumper\n";
fwrite(text, sizeof(char), strlen(text), file);
// Close the file
fclose(file);
// Write to the file
while (true)
{
FILE *file;
// Open the file for writing
file = fopen(filename, "a");
if (file == NULL)
{
ev3_speaker_play_tone(NOTE_E6, 10000);
tslp_tsk(10000);
}
SYSTIM currentTime;
get_tim(&currentTime);
int gyro_angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
int gyro_rate = ev3_gyro_sensor_get_rate(GYRO_PORT);
int left_motor = ev3_motor_get_power(LEFT_MOTOR);
int left_motor_counts = ev3_motor_get_counts(LEFT_MOTOR);
int right_motor = ev3_motor_get_power(RIGHT_MOTOR);
int right_motor_counts = ev3_motor_get_counts(RIGHT_MOTOR);
int hinge_motor = ev3_motor_get_power(HINGE_MOTOR);
bool top = ev3_touch_sensor_is_pressed(BUTTON_UP);
bool bumper = ev3_touch_sensor_is_pressed(BUTTON_BUMPER);
char dataString[250];
sprintf(dataString, "%lu;%d;%d%d;%d;%d;%d;%d;%d;%d;%d;%d\n", currentTime, ev3_battery_voltage_mV(), ev3_battery_current_mA(), gyro_angle, gyro_rate, left_motor, left_motor_counts, right_motor, right_motor_counts, hinge_motor, top, bumper);
fwrite(dataString, sizeof(char), strlen(dataString), file);
// Close the file
fclose(file);
tslp_tsk(1000);
}
}
void sub_task(intptr_t unused)
{
// Sleep 85 seconds
int time = 0;
while (time < 85)
SYSTIM startTime;
get_tim(&startTime);
bool run = true;
while (run)
{
time++;
ev3cxx::display.format(2, "Time: % s") % time;
SYSTIM currentTime;
get_tim(&currentTime);
if((currentTime - startTime) >= 85000){
run = false;
}
ev3cxx::display.format(2, "Time: % s") % (int)round((currentTime-startTime)/1000);
tslp_tsk(1000);
}
// Make sure the volume is up and play the final beep
@ -1126,7 +1192,7 @@ void main_task(intptr_t unused)
ev3cxx::display.setFont(EV3_FONT_MEDIUM);
// Set up motor powers
int* rationalized_speeds = calculate_ratio(45, RATIO);
int *rationalized_speeds = calculate_ratio(45, RATIO);
MPWRSPlus calcedPWRS = calculate_motor_pwrs(rationalized_speeds[1], rationalized_speeds[0]);
MPWRS idealMPWRS = calcedPWRS.motor_powers;
@ -1188,7 +1254,7 @@ void main_task(intptr_t unused)
start_program_exe(btnLeft, btnEnter, touchS, values);
act_tsk(SUB_TASK);
act_tsk(CSV_TASK);
ev3_speaker_play_tone(NOTE_F4, 100);
tslp_tsk(200);
@ -1207,16 +1273,18 @@ void main_task(intptr_t unused)
ev3_speaker_play_tone(NOTE_FS4, 100);
}
gyro.resetHard();
motors.onForRotations(-(motor_powers.rMotorPWR + 25), -(motor_powers.lMotorPWR - 25), 3.25);
// till here it works:: 20.11 14:34
//gyro.resetHard();
// 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);
motors.onForRotations(-50,-70, 3.25);
tslp_tsk(350);
turn(motors, gyro, 20, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 1);
turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
// this also works, code above doesnt
// Cross to the other side
gyro.resetHard();
@ -1254,19 +1322,4 @@ void main_task(intptr_t unused)
ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
ev3_speaker_play_tone(NOTE_FS4, 100);
}
gyro.resetHard();
motors.onForRotations(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 3.25);
// reset gyro; turn 150 degrees left
gyro.resetHard();
turn_forever(motors, gyro, 150, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 6);
open_door(hinge);
motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 8.12);
motors.off(true);
tslp_tsk(120000);
}

1
app.h
View File

@ -103,6 +103,7 @@ extern "C" {
extern void main_task(intptr_t);
extern void sub_task(intptr_t);
extern void music_task(intptr_t);
extern void csv_task(intptr_t);
// extern void periodic_task_1(intptr_t);
// extern void periodic_task_2(intptr_t);