diff --git a/app b/app index 233f02d..b2ba4c6 100644 Binary files a/app and b/app differ diff --git a/app.cfg b/app.cfg index 9789e57..3723274 100644 --- a/app.cfg +++ b/app.cfg @@ -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 }); diff --git a/app.cpp b/app.cpp index a138e1b..17f3cf9 100644 --- a/app.cpp +++ b/app.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include /* 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(¤tTime); + + 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(¤tTime); + 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); } diff --git a/app.h b/app.h index 1b9c571..97be12c 100644 --- a/app.h +++ b/app.h @@ -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);