dieg.cpp

#include <wiringPi.h>
#include <wiringPiSPI.h>

#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   = 22;
const int PWM_PIN_LEFT  = 26;
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[])
{
    if (wiringPiSetup() == -1) {
        printf("WiringPiSetup failed\n");
        exit(EXIT_FAILURE);
    }

    pinMode(PWM_PIN_RIGHT, PWM_OUTPUT);
    pinMode(PWM_PIN_LEFT, PWM_OUTPUT);
    pinMode(M1DIR_RIGHT, OUTPUT);
    pinMode(M2DIR_LEFT, OUTPUT);

    digitalWrite(M1DIR_RIGHT, 0);
    digitalWrite(M2DIR_LEFT, 1);


    int fd;

    fd = wiringPiSPISetup(1, 1000000);

    unsigned char buffer [size_buffer];

    buffer[0] = 0x00;
    buffer[1] = 0x00;
    buffer[2] = 0x00;
    buffer[3] = 0x00;
    //buffer[4] = 0x00;

    wiringPiSPIDataRW(1, buffer, size_buffer);

    sleep(1);

    buffer[0] = 0x00;
    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);

    int vit = 0;
    float speed=0;
    float count=0;
    float count_d=0;
    int toDecimal = 0;
    int nbre_tics;

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