RS START

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 });

app.cpp

@@ -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;
+    SYSTIM startTime;
-    while (time < 85)
+    get_tim(&startTime);
+    bool run = true;
+    while (run)
     {
-        time++;
+        SYSTIM currentTime;
-        ev3cxx::display.format(2, "Time: % s") % time;
+        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();
+    // till here it works:: 20.11 14:34
-    motors.onForRotations(-(motor_powers.rMotorPWR + 25), -(motor_powers.lMotorPWR - 25), 3.25);
+
+    //gyro.resetHard();
     // turn 90 degress left
-    turn_forever(motors, gyro, 30, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+    motors.onForRotations(-50,-70, 3.25);
-    ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR);
+    tslp_tsk(350);
-    ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR);
+    turn(motors, gyro, 20, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
-    tslp_tsk(250);
+    motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 1);
-    motors.off(true);
+    turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
-    turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
+    motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);
-    // 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);
 }

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);