bite

.vscode/settings.json

 {
     "files.associations": {
         "iostream": "cpp",
-        "limits": "cpp"
+        "limits": "cpp",
+        "cmath": "cpp"
     }
 }
\ No newline at end of file

LiDAR/LIDAR.cpp

@@ -9,9 +9,11 @@
 #include <iostream>
 #include <time.h>
 
+#include "../main.c"
+
 using namespace sl;
 
-int main(int argc, char const *argv[])
+int main_lidar(int argc, char const *argv[])
 {
     // Create communication instance channel
     IChannel *channel;
@@ -81,6 +83,7 @@ int main(int argc, char const *argv[])
     {
         fprintf(file, "%.3f\t%.3f\n",nodes[i].angle_z_q14 * 90.f / (1 << 14), nodes[i].dist_mm_q2 / 1000.f / (1 << 2));
         // printf("mesure %i : theta = %.3f°, dist = %.3f m\n",i,nodes[i].angle_z_q14 * 90.f / (1 << 14), nodes[i].dist_mm_q2 / 1000.f / (1 << 2));
+        
     }
     
     fclose(file);

dieg.cpp

@@ -4,13 +4,18 @@
 #include <stdio.h>
 #include <unistd.h>
 #include <stdlib.h>
+#include <math.h>
+#include <time.h>
 
 #include <conio.h>
 
 const int PWM_PIN_RIGHT = 23;
-const int M1DIR_RIGHT   = 21;
+const int M1DIR_RIGHT   = 22;
 const int PWM_PIN_LEFT  = 26;
-const int M2DIR_LEFT    = 22;
+const int M2DIR_LEFT    = 21;
+int size_buffer = 4;
+
+const float TOSPEED = 2*M_PI * 2.95e-2 / (1920. * 0.01); // 2*PI*R / (TICS_1_TURN*delta t)
 
 int main(int argc, char const *argv[])
 {
@@ -24,20 +29,23 @@ int main(int argc, char const *argv[])
     pinMode(M1DIR_RIGHT, OUTPUT);
     pinMode(M2DIR_LEFT, OUTPUT);
 
-    digitalWrite(M1DIR_RIGHT, 1);
+    digitalWrite(M1DIR_RIGHT, 0);
+    digitalWrite(M2DIR_LEFT, 1);
+
 
     int fd;
 
     fd = wiringPiSPISetup(1, 1000000);
 
-    unsigned char buffer [4];
+    unsigned char buffer [size_buffer];
 
     buffer[0] = 0x00;
     buffer[1] = 0x00;
     buffer[2] = 0x00;
     buffer[3] = 0x00;
+    //buffer[4] = 0x00;
 
-    wiringPiSPIDataRW(1, buffer, 4);
+    wiringPiSPIDataRW(1, buffer, size_buffer);
 
     sleep(1);
 
@@ -45,24 +53,54 @@ int main(int argc, char const *argv[])
     buffer[1] = 0x00;
     buffer[2] = 0x00;
     buffer[3] = 0x01;
+    //buffer[4] = 0x00;
+
+    wiringPiSPIDataRW(1, buffer, size_buffer);
+
+    pwmWrite(PWM_PIN_RIGHT, 600);
+    pwmWrite(PWM_PIN_LEFT, 570);
 
-    wiringPiSPIDataRW(1, buffer, 4);
+    int vit = 0;
+    float speed=0;
+    float count=0;
+    float count_d=0;
+    int toDecimal = 0;
+    int nbre_tics;
 
-    pwmWrite(PWM_PIN_RIGHT, 560);
+    FILE *f = fopen("speed600.csv", "w");
+    fprintf(f, "Time [s]\tSpeed [m/s]\n");
+    clock_t start = clock();
 
     while (1)
     {
-        if (kbhit()) goto end;
-        buffer[0] = 0x00;
-        buffer[1] = 0x00;
-        buffer[2] = 0x00;
-        buffer[3] = 0x01;
-        wiringPiSPIDataRW(1,buffer, 4);
-        printf("%x %x %x %x\n", buffer[0],buffer[1],buffer[2],buffer[3]);
-        sleep(1); 
-    }
+            if (kbhit()) goto end;
+            buffer[0] = 0x00;
+            buffer[1] = 0x00;
+            buffer[2] = 0x00;
+            buffer[3] = 0x01;
+            // int result = wiringPiSPIDataRW(1,buffer, size_buffer);
+            //printf("%x %x %x %x\r", buffer[0],buffer[1],buffer[2],buffer[3]);
+            // count=(buffer[0]*pow(16, 4))+(buffer[1]*pow(16, 2))+(buffer[2]*pow(16, 0));
+            // speed=(count-count_d);//(60/(2*3.1415))*(2*3.1415/960)*
+            // printf("%lf\r", count);
+            // fflush(stdout);
+
+            wiringPiSPIDataRW(1, buffer, size_buffer);
+
+            // printf("%x %x %x %x\n",buffer[0],buffer[1],buffer[2],buffer[3]);
+
+            nbre_tics = (signed char) (buffer[3] | (buffer[2] << 8) | (buffer[1] << 16) | (buffer[0] << 24));
+            speed = nbre_tics * TOSPEED;
+            
+            printf("%.3f [m/s]\n", speed);
+    
+            fprintf(f, "%.6f\t%.6f\n",((double) (clock()-start)) / CLOCKS_PER_SEC, speed);
 
+    }
 end:
+    fclose(f);
     pwmWrite(PWM_PIN_RIGHT, 0);
+    pwmWrite(PWM_PIN_LEFT, 0);
+
     return 0;
 }

encoder.cpp

@@ -51,16 +51,16 @@ int main(int argc, char const *argv[])
         //fflush(stdout);
         //printf("\n");
         count=(rawData[0]*pow(16, 4))+(rawData[1]*pow(16, 2))+(rawData[2]*pow(16, 0));
-        //printf("\r%lf", count);
+        printf("\r%lf", count);
         //printf("\n%d %d %d %d", rawData[0], rawData[1], rawData[2], rawData[3]); 
         //fflush(stdout);
 
         //fflush(stdout);
         //clock_count=(rawData[2]*pow(16, 2))+(rawData[3]*pow(16, 0));
         
-        speed=(60/(2*3.1415))*(2*3.1415/res)*50000000/(count-count_delayed);
-        printf("\r%lf", speed); 
-        fflush(stdout);
+        //speed=(60/(2*3.1415))*(2*3.1415/res)*50000000/(count-count_delayed);
+        //printf("\r%lf", speed); 
+        //fflush(stdout);
         count_delayed=count;
         //printf("\n");
         //sleep(1);

lidar_detect.c

+
+/*
+//#include "LIDAR.cpp"
+#include "main.c"
+int int main(int argc, const char** argv) {
+    return 0;
+}
+
+*/

main.c

+/*
+
+# include <stdio.h>
+# include <stdlib.h>
+# include <math.h>
+# include <string.h>
+# include <assert.h>
+
+//# include "lidar_detect.c"
+
+# define res_lidar 1068  
+
+typedef struct {
+    double *dist_beacon;
+    double *angle_beacon;
+} lidarinfo; 
+
+int main() {
+    lidarinfo *mylidarinfo = malloc(sizeof(lidarinfo));
+
+    mylidarinfo->angle_beacon=malloc(sizeof(double)*res_lidar);
+    mylidarinfo->dist_beacon=malloc(sizeof(double)*res_lidar);
+
+    
+    free(mylidarinfo->angle_beacon);
+    free(mylidarinfo->dist_beacon);
+    free(mylidarinfo);
+
+       return 0;
+}
+
+*/
\ No newline at end of file