ENVIO DE DATOS DE SENSORES POR BLUETOOTH A UN PC CON UN PROGRAMA EN C#

[peencchoo]

Buenas, tengo un problema, estoy intentando enviar, los datos que mide mi raspberry pi de unos sensores VL53l0x, a través de Bluetooth, y que estos datos sean leidos en tiempo real en un programa en C#.
El objetivo es que tengo un vehículo autónomo que funciona con un programa en c++ y me gustaria que me dibujase unas graficas con los datos en el pc, utilizando bluethooth y un código en c# para dibujar las gráficas, ya tengo el código en c#, el único problema esque no consigo recibir los datos.
Espero atento vuestras respuestas. Cualquier información adicional, preguntadme y os la facilito.
Gracias de antemano.

[egrueda]

¿Y en qué punto estás? ¿Qué necesitas exactamente?
Porque más que solucionar un problema, suena como si necesitases que alguien te hiciese el proyecto :-D
¿Has establecido al menos el listado de tareas a llevar a cabo y el orden?
¿Y para cada tarea, los requisitos de almacenamiento, I/O, log, etc?
¿Has emparejado los dispositivos bluetooth?

[peencchoo]

Tengo un programa en c++ que inicializo en la Raspberry, el cual establece la conexión Bluetooth con mi portátil, y el cual puede enviar datos del portátil a la Raspberry, pero no soy capaz de hacer enviar los datos de los sensores al portátil .

Si conocieses algún sitio donde explicasen bien esto o hubiese información útil, te agradecería que me la compartas, porque he buscado por todos sitios y no encuentro nada parecido.

Adjunto los códigos.
Este es el de c++ que ejecuto en la raspberry.

Código: Seleccionar todo

 
#include <stdio.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <time.h>
#include <sys/time.h>
#include <csignal>
#include <string.h>
#include <wiringPi.h>

/*
* 16 12 5
* this is the CCITT CRC 16 polynomial X + X + X + 1.
* This is 0x1021 when x is 2, but the way the algorithm works
* we use 0x8408 (the reverse of the bit pattern). The high
* bit is always assumed to be set, thus we only use 16 bits to
* represent the 17 bit value.
//*/

#define POLY 0x8408 /* 1021H bit reversed*/
#define PACKET_BYTES_COUNT 72 // Paketo ilgis

#define PIN_A 4		//16 / L298n IN3
#define PIN_B 5		//18 / L298n IN4
#define PIN_PWM 23	//33 / L298n ENB
bool vaziuoti = false;
bool buvusiBusena = false;

unsigned short crcu16(unsigned char *data_p, unsigned short length) {
unsigned char i;
unsigned int data;
unsigned int crc = 0xffff;

if (length == 0)
	return (~crc);

do {
	for (i=0, data=(unsigned int)0xff & *data_p++; i < 8; i++, data >>= 1) {
		if ((crc & 0x0001) ^ (data & 0x0001))
			crc = (crc >> 1) ^ POLY;
		else
			crc >>= 1;
	}
}

while (--length);
	return (crc);
}

//Funkcija laikui skaiciuoti

long long current_timestamp() {
    struct timeval te;
    gettimeofday(&te, NULL); // get current time
    long long milliseconds = te.tv_sec*1000LL + te.tv_usec/1000; // calculate milliseconds
    //printf("milliseconds: %lld \n", milliseconds);
    return milliseconds;
}

// Funkcija duomenims issiusti
int SendData ( int OpticalSensor1,  int OpticalSensor2, int OpticalSensor3, int OpticalSensor4,
int OpticalSensor5, int OpticalSensor6, int GyroscopeX, int GyroscopeY, int GyroscopeZ, int AccelerometerX,
int AccelerometerY, int AccelerometerZ, int Temperature1, int Temperature2,int EncoderLeft, int EncoderRight,int client)
{
unsigned char buffer [PACKET_BYTES_COUNT];
int TCP_CLIENT_BUFFER_LENGTH=72;
int status = 0;

buffer[0] = 0xF1; buffer[1] = 0xF2; // begin stream
buffer[2] = 0; buffer[3] = 12; // header (?)
// ETH values

buffer [7] = (OpticalSensor1>>24) & 0xFF;
buffer [6] = (OpticalSensor1>>16) & 0xFF;
buffer [5] = (OpticalSensor1>>8) & 0xFF;
buffer [4] = OpticalSensor1 & 0xFF;

buffer[11] = (OpticalSensor2>>24) & 0xFF;
buffer[10] = (OpticalSensor2>>16) & 0xFF;
buffer [9] = (OpticalSensor2>>8) & 0xFF;
buffer [8] = OpticalSensor2 & 0xFF;

buffer [15] = (OpticalSensor3>>24) & 0xFF;
buffer [14] = (OpticalSensor3>>16) & 0xFF;
buffer [13] = (OpticalSensor3>>8) & 0xFF;
buffer [12] = OpticalSensor3 & 0xFF;

buffer [19] = (OpticalSensor4>>24) & 0xFF;
buffer [18] = (OpticalSensor4>>16) & 0xFF;
buffer [17] = (OpticalSensor4>>8) & 0xFF;
buffer [16] = OpticalSensor4 & 0xFF;

buffer [23] = (OpticalSensor5>>24) & 0xFF;
buffer [22] = (OpticalSensor5>>16) & 0xFF;
buffer [21] = (OpticalSensor5>>8) & 0xFF;
buffer [20] = OpticalSensor5 & 0xFF;

buffer [27] = (OpticalSensor6>>24) & 0xFF;
buffer [26] = (OpticalSensor6>>16) & 0xFF;
buffer [25] = (OpticalSensor6>>8) & 0xFF;
buffer [24] = OpticalSensor6 & 0xFF;

buffer [31] = (GyroscopeX>>24) & 0xFF;
buffer [30] = (GyroscopeX>>16) & 0xFF;
buffer [29] = (GyroscopeX>>8) & 0xFF;
buffer [28] = GyroscopeX & 0xFF;

buffer [35] = (GyroscopeY>>24) & 0xFF;
buffer [34] = (GyroscopeY>>16) & 0xFF;
buffer [33]= (GyroscopeY>>8) & 0xFF;
buffer [32] = GyroscopeY & 0xFF;

buffer [39] = (GyroscopeZ>>24) & 0xFF;
buffer [38] = (GyroscopeZ>>16) & 0xFF;
buffer [37] = (GyroscopeZ>>8) & 0xFF;
buffer [36] = GyroscopeZ & 0xFF;

buffer [43] = (AccelerometerX>>24) & 0xFF;
buffer [42] = (AccelerometerX>>16) & 0xFF;
buffer [41] = (AccelerometerX>>8) & 0xFF;
buffer [40] = AccelerometerX & 0xFF;

buffer [47] = (AccelerometerY>>24) & 0xFF;
buffer [46] = (AccelerometerY>>16) & 0xFF;
buffer [45] = (AccelerometerY>>8) & 0xFF;
buffer [44] = AccelerometerY & 0xFF;

buffer [51] = (AccelerometerZ>>24) & 0xFF;
buffer [50] = (AccelerometerZ>>16) & 0xFF;
buffer [49] = (AccelerometerZ>>8) & 0xFF;
buffer [48] = AccelerometerZ & 0xFF;

buffer [55] = (Temperature1>>24) & 0xFF;
buffer [54] = (Temperature1>>16) & 0xFF;
buffer [53] = (Temperature1>>8) & 0xFF;
buffer [52] = Temperature1 & 0xFF;

buffer [59] = (Temperature2>>24) & 0xFF;
buffer [58] = (Temperature2>>16) & 0xFF;
buffer [57] = (Temperature2>>8) & 0xFF;
buffer [56] = Temperature2 & 0xFF;

buffer [63] = (EncoderLeft>>24) & 0xFF;
buffer [62] = (EncoderLeft>>16) & 0xFF;
buffer [61] = (EncoderLeft>>8) & 0xFF;
buffer [60] = EncoderLeft& 0xFF;

buffer [67] = (EncoderRight>>24) & 0xFF;
buffer [66] = (EncoderRight>>16) & 0xFF;
buffer [65] = (EncoderRight>>8) & 0xFF;
buffer [64] = EncoderRight& 0xFF;

unsigned short crc16code = crcu16(&buffer[0], 68);
buffer[68] = (unsigned char)(crc16code & 0xFF);
buffer[69] = (unsigned char)((crc16code >> 8) & 0xFF);
buffer[70] = 0xE3; buffer[71] = 0xE4;

status = write(client, buffer, TCP_CLIENT_BUFFER_LENGTH);
printf("Buffer sent \n");
	return status;
if(status < 0)
{
	perror("error 404");
	return status;
}
}

int ReadData(int client){

	char zodis1[10];
        char zodis2[10];
        strcpy(zodis1, "Start");
        strcpy(zodis2, "Stop");

	char buf[10] = {0};
	memset(buf, 0, sizeof(buf));
	int read_bytes = 0;
	read_bytes = recv(client, buf, sizeof(buf), MSG_DONTWAIT);
	if(read_bytes > 0){
		printf("received [%s]\n", buf);
		if(strcmp(buf, zodis1) == 0)
		{
			vaziuoti = true;
			buvusiBusena = false;
		}
		else if(strcmp(buf, zodis2) == 0)
		{
			vaziuoti = false;
			buvusiBusena = true;
		}
		return 0;
}
	else
		return 0;
}
int main(int argc, char **argv)
{
    wiringPiSetup ();
    pinMode(PIN_A, OUTPUT);
    pinMode(PIN_B, OUTPUT);
    pinMode(PIN_PWM, PWM_OUTPUT);

    digitalWrite(PIN_A, HIGH);
    digitalWrite(PIN_B, LOW);
    pwmWrite(PIN_PWM, 0);
    delay(1);
    struct sockaddr_rc loc_addr = { 0 }, rem_addr = { 0 };

    char buf[1024] = { 0 };
    //char send[1024] = { 0 };

    int s, client;

    socklen_t opt = sizeof(rem_addr);

    bdaddr_t my_bdaddr_any = { 0, 0, 0, 0, 0, 0 };

// Tariami nuskaityti duomenys:

//Atstumo jutikliu duomenys

int val1 = 420, val2 = 911, val3 = 112, val4 = 123, val5 = 321, val6 = 444;

//Gyroskopo duomenys

int val7 = 100, val8 = 100, val9 = 100, val10 = 100, val11 = 100, val12 = 100;

//Temperaturos duomenys

int val13 = 10,val14 = 1000;

//Enkoderio duomenys

int val15=1, val16=2;


    // allocate socket

    s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);

    // bind socket to port 1 of the first available

    // local bluetooth adapter

    loc_addr.rc_family = AF_BLUETOOTH;

    loc_addr.rc_bdaddr = my_bdaddr_any;

    loc_addr.rc_channel = (uint8_t) 1;

    bind(s, (struct sockaddr *)&loc_addr, sizeof(loc_addr));


    // put socket into listening mode

    listen(s, 1);

    printf("Put into listening mode\n");



    // accept one connection

    client = accept(s, (struct sockaddr *)&rem_addr, &opt);

    ba2str( &rem_addr.rc_bdaddr, buf );

    fprintf(stderr, "Accepted connection from %s\n", buf);

    memset(buf, 0, sizeof(buf));

    // send a message

long long milliseconds;
long long previous_ms=0;
long long delay_time = 500L;

int checkStatus;

while(1){

     usleep(10000);

     ReadData(client);

     if(vaziuoti == false && buvusiBusena == true)
     {
	pwmWrite(PIN_PWM, 0);
	digitalWrite(PIN_A, LOW);;
	digitalWrite(PIN_B, HIGH);
	pwmWrite(PIN_PWM, 1024);
	delay(225);
	pwmWrite(PIN_PWM, 0);
	delay(1);
	digitalWrite(PIN_B, LOW);
	digitalWrite(PIN_A, HIGH);
        buvusiBusena = false;

     }
     else if(vaziuoti == true && buvusiBusena == false)
     {
	pwmWrite(PIN_PWM, 1024);
	delay(1);
	buvusiBusena = true;
     }
     milliseconds=current_timestamp();

     if (milliseconds-previous_ms>delay_time)
     {
         previous_ms=milliseconds;

	checkStatus = SendData(val1,val2,val3,val4,val5,val6,val7,val8,val9,val10,val11,val12,val13,val14,val15,val16,client);

	 if(checkStatus < 0)
             break;
     }
}
    // close connection

    close(client);

    close(s);

    return 0;
}

Y este es el de c# que inicializo en el portatil:

Código: Seleccionar todo

#include <stdio.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <time.h>
#include <sys/time.h>
#include <pthread.h>
#include <csignal>
#include <string.h>
#include <wiringPi.h>
#include <sys/signalfd.h>
#include <signal.h>
#include <chrono>
#include <iostream>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/types.h>
extern "C" {
#include "linux/i2c.h"
#include "i2c/smbus.h"
#include "linux/i2c-dev.h"
}

#define ACCEL_XOUT 0x3B
#define ACCEL_YOUT 0x3D
#define ACCEL_ZOUT 0x3F
#define GYRO_XOUT  0x43
#define GYRO_YOUT  0x45
#define GYRO_ZOUT  0x47
#define MPU_POWER1   0x6b
#define SMPLRT_DIV   0x19
#define CONFIG       0x1A
#define GYRO_CONFIG  0x1B
#define ACCEL_CONFIG 0x1C


#define POLY 0x8408 /* 1021H bit reversed*/
#define PACKET_BYTES_COUNT 72 // Paketo ilgis

#define servo 18
#define dc_f 13
#define dc_b 19
volatile sig_atomic_t exitFlag = 0;
int komanda;
int buvusikomanda;
int fd1;
const char *file_name = "/dev/i2c-1";
int  mpuAddress = 0x68;

int read_raw_data(int addr){ //MPU uses two 8Bit registers to store data 16bit data value.  //Reads two registers and returns 16bit value.
                              
    short high_byte,low_byte,value;
    high_byte = i2c_smbus_read_byte_data(fd1, addr);
    usleep(50);
    low_byte = i2c_smbus_read_byte_data(fd1, addr+1);
    value = (high_byte << 8) | low_byte;
    return value;
 }
void MPU6050_Init(){
    
        
        i2c_smbus_write_byte_data (fd1, MPU_POWER1, 0x01);   //Disable sleep mode
        i2c_smbus_write_byte_data (fd1, SMPLRT_DIV, 0x07);
        i2c_smbus_write_byte_data (fd1, ACCEL_CONFIG, 0x00);
        i2c_smbus_write_byte_data (fd1, GYRO_CONFIG, 0x00);
       
        usleep(2);
     
       
    
    
    }



void I2C_Init()
{
    if ((fd1 = open(file_name, O_RDWR)) < 0) {
        printf("Failed to open i2c port\n");
        exit(1);
    }
    
    if (ioctl(fd1, I2C_SLAVE, mpuAddress) < 0) {
        printf("Unable to get bus access to talk to slave\n");
        exit(1);
    }
}

void sigintHandler(int)
{
	exitFlag = 1;
}

unsigned short crcu16(unsigned char *data_p, unsigned short length) {
unsigned char i;
unsigned int data;
unsigned int crc = 0xffff;

if (length == 0)
	return (~crc);

do {
	for (i=0, data=(unsigned int)0xff & *data_p++; i < 8; i++, data >>= 1) {
		if ((crc & 0x0001) ^ (data & 0x0001))
			crc = (crc >> 1) ^ POLY;
		else
			crc >>= 1;
	}
}

while (--length);
	return (crc);
}



long long current_timestamp() {
    struct timeval te;
    gettimeofday(&te, NULL); // get current time
    long long milliseconds = te.tv_sec*1000LL + te.tv_usec/1000; // calculate milliseconds
    //printf("milliseconds: %lld \n", milliseconds);
    return milliseconds;
}

// Funkcija duomenims issiusti
int SendData ( int OpticalSensor1,  int OpticalSensor2, int OpticalSensor3, int OpticalSensor4,
int OpticalSensor5, int OpticalSensor6, int GyroscopeX, int GyroscopeY, int GyroscopeZ, int AccelerometerX,
int AccelerometerY, int AccelerometerZ, int Temperature1, int Temperature2,int EncoderLeft, int EncoderRight,int client)
{
unsigned char buffer [PACKET_BYTES_COUNT];
int TCP_CLIENT_BUFFER_LENGTH=72;
int status = 0;

buffer[0] = 0xF1; buffer[1] = 0xF2; // begin stream
buffer[2] = 0; buffer[3] = 12; // header (?)
// ETH values

buffer [7] = (OpticalSensor1>>24) & 0xFF;
buffer [6] = (OpticalSensor1>>16) & 0xFF;
buffer [5] = (OpticalSensor1>>8) & 0xFF;
buffer [4] = OpticalSensor1 & 0xFF;

buffer[11] = (OpticalSensor2>>24) & 0xFF;
buffer[10] = (OpticalSensor2>>16) & 0xFF;
buffer [9] = (OpticalSensor2>>8) & 0xFF;
buffer [8] = OpticalSensor2 & 0xFF;

buffer [15] = (OpticalSensor3>>24) & 0xFF;
buffer [14] = (OpticalSensor3>>16) & 0xFF;
buffer [13] = (OpticalSensor3>>8) & 0xFF;
buffer [12] = OpticalSensor3 & 0xFF;

buffer [19] = (OpticalSensor4>>24) & 0xFF;
buffer [18] = (OpticalSensor4>>16) & 0xFF;
buffer [17] = (OpticalSensor4>>8) & 0xFF;
buffer [16] = OpticalSensor4 & 0xFF;

buffer [23] = (OpticalSensor5>>24) & 0xFF;
buffer [22] = (OpticalSensor5>>16) & 0xFF;
buffer [21] = (OpticalSensor5>>8) & 0xFF;
buffer [20] = OpticalSensor5 & 0xFF;

buffer [27] = (OpticalSensor6>>24) & 0xFF;
buffer [26] = (OpticalSensor6>>16) & 0xFF;
buffer [25] = (OpticalSensor6>>8) & 0xFF;
buffer [24] = OpticalSensor6 & 0xFF;

buffer [31] = (GyroscopeX>>24) & 0xFF;
buffer [30] = (GyroscopeX>>16) & 0xFF;
buffer [29] = (GyroscopeX>>8) & 0xFF;
buffer [28] = GyroscopeX & 0xFF;

buffer [35] = (GyroscopeY>>24) & 0xFF;
buffer [34] = (GyroscopeY>>16) & 0xFF;
buffer [33]= (GyroscopeY>>8) & 0xFF;
buffer [32] = GyroscopeY & 0xFF;

buffer [39] = (GyroscopeZ>>24) & 0xFF;
buffer [38] = (GyroscopeZ>>16) & 0xFF;
buffer [37] = (GyroscopeZ>>8) & 0xFF;
buffer [36] = GyroscopeZ & 0xFF;

buffer [43] = (AccelerometerX>>24) & 0xFF;
buffer [42] = (AccelerometerX>>16) & 0xFF;
buffer [41] = (AccelerometerX>>8) & 0xFF;
buffer [40] = AccelerometerX & 0xFF;

buffer [47] = (AccelerometerY>>24) & 0xFF;
buffer [46] = (AccelerometerY>>16) & 0xFF;
buffer [45] = (AccelerometerY>>8) & 0xFF;
buffer [44] = AccelerometerY & 0xFF;

buffer [51] = (AccelerometerZ>>24) & 0xFF;
buffer [50] = (AccelerometerZ>>16) & 0xFF;
buffer [49] = (AccelerometerZ>>8) & 0xFF;
buffer [48] = AccelerometerZ & 0xFF;

buffer [55] = (Temperature1>>24) & 0xFF;
buffer [54] = (Temperature1>>16) & 0xFF;
buffer [53] = (Temperature1>>8) & 0xFF;
buffer [52] = Temperature1 & 0xFF;

buffer [59] = (Temperature2>>24) & 0xFF;
buffer [58] = (Temperature2>>16) & 0xFF;
buffer [57] = (Temperature2>>8) & 0xFF;
buffer [56] = Temperature2 & 0xFF;

buffer [63] = (EncoderLeft>>24) & 0xFF;
buffer [62] = (EncoderLeft>>16) & 0xFF;
buffer [61] = (EncoderLeft>>8) & 0xFF;
buffer [60] = EncoderLeft& 0xFF;

buffer [67] = (EncoderRight>>24) & 0xFF;
buffer [66] = (EncoderRight>>16) & 0xFF;
buffer [65] = (EncoderRight>>8) & 0xFF;
buffer [64] = EncoderRight& 0xFF;

unsigned short crc16code = crcu16(&buffer[0], 68);
buffer[68] = (unsigned char)(crc16code & 0xFF);
buffer[69] = (unsigned char)((crc16code >> 8) & 0xFF);
buffer[70] = 0xE3; buffer[71] = 0xE4;

status = write(client, buffer, TCP_CLIENT_BUFFER_LENGTH);
printf("Buffer sent \n");
	return status;
if(status < 0)
{
	perror("error 404");
	return status;
}
}
void delay_ms(int delay__ms) // funkcija mikrosekundem gauti
{	
	long int start_time;
	long int time_difference;
	struct timespec gettime_now;
	clock_gettime(CLOCK_REALTIME, &gettime_now);
	start_time = gettime_now.tv_nsec; //Get nS value
	//printf("start time: %ld\n", start_time);
	while(1)
	{			
		clock_gettime(CLOCK_REALTIME, &gettime_now);
		time_difference = gettime_now.tv_nsec - start_time;
		if (time_difference < 0)
			time_difference += 1000000000;	//(Rolls over every 1 second)
		if (time_difference >= (delay__ms* 1000000))	//Delay for # nS
			break;
	}
}


void *servocontrol(void *arg)
{
wiringPiSetupGpio ();
signal(SIGINT, sigintHandler);
pinMode (servo, PWM_OUTPUT);
pinMode (dc_f, PWM_OUTPUT);
pinMode (dc_b, PWM_OUTPUT);
pwmSetMode (PWM_MODE_MS);
pwmSetClock (192); 
pwmSetRange (2000);
int speed_default = 1300;	
while(!exitFlag)
{	
	usleep(10);
	if (komanda != buvusikomanda) 
	{
		if((buvusikomanda==2 && (komanda==3 || komanda==1))||(buvusikomanda==1 && komanda == 3)||(buvusikomanda==3 && komanda == 1))
		{
		delay_ms(500);
		printf("laukiame,kol sustosime\n");
		}
	printf("komanda: [%d]\n", komanda);
	printf("buvusikomanda: [%d]\n", buvusikomanda);	
	switch(komanda)
	{	
		case 0:			
			buvusikomanda = komanda;
			break;
		case 1:		
			pinMode (dc_f, PWM_OUTPUT);
			pinMode (dc_b, INPUT);
			pwmSetMode (PWM_MODE_MS);
			pwmSetClock (192); 
			pwmSetRange (2000);
			pwmWrite(dc_f, speed_default);	
			buvusikomanda = komanda;
			break;
		case 2: 
			pinMode (dc_b, INPUT);
			pinMode(dc_f, INPUT);
			pwmSetMode (PWM_MODE_MS);
			pwmSetClock (192); 
			pwmSetRange (2000);
			buvusikomanda = komanda;
			break;
		case 3: 
			pinMode (dc_b, PWM_OUTPUT);
			pinMode (dc_f, INPUT);
			pwmSetMode (PWM_MODE_MS);
			pwmSetClock (192); 
			pwmSetRange (2000);
			pwmWrite(dc_b, speed_default);		
			buvusikomanda = komanda;
			break;
		case 5:	
			pwmWrite(servo, 185);
			buvusikomanda = komanda;
			break;
		case 7:	
			pwmWrite(servo, 135);		
			buvusikomanda = komanda;
			break;
		case 8:	
			pwmWrite(servo, 160);	
			buvusikomanda = komanda;
			break;
		case 9: 
			
			speed_default += 100;
			buvusikomanda = komanda;
			break;
		case 10:
			buvusikomanda = komanda;
			break;
		case 11: 
			speed_default -= 100;
			buvusikomanda = komanda;//printf("speed: %d\n", speed);
			break;
		case 12: 
			buvusikomanda = komanda;
			break;	
			
		default:
			break;
	}
	}
}
  return 0;
}



int ReadData(int client){
	char zodis1[20];
	char zodis2[20];
	char zodis3[20];
	char zodis4[20];
	char zodis5[20];
	char zodis6[20];
	char zodis7[20];
	char zodis8[20];
	char zodis9[20];
	char zodis10[20];
	char zodis11[20];
	char zodis12[20];

    strcpy(zodis1, "StartFoward");
    strcpy(zodis2, "StopFoward");
    strcpy(zodis3, "StartBackwards");
    strcpy(zodis4, "StopBackwards");
    strcpy(zodis5, "StartLeft");
    strcpy(zodis6, "StopLeft");
    strcpy(zodis7, "StartRight");
    strcpy(zodis8, "StopRight");
    strcpy(zodis9, "StartSpeedIncrease");
    strcpy(zodis10, "StopSpeedIncrease");
    strcpy(zodis11, "StartSpeedDecrease");
    strcpy(zodis12, "StopSpeedDecrease");

	char buf[20] = {0};
	memset(buf, 0, sizeof(buf));
	int read_bytes = 0;
	read_bytes = recv(client, buf, sizeof(buf), MSG_DONTWAIT);
	
	if(read_bytes > 0)
	{
		printf("received [%s]\n", buf);
		if(strcmp(buf, zodis1) == 0 )
		{
			komanda = 1;
		}
		else if(strcmp(buf, zodis2) == 0)
		{
			komanda = 2;
		}
		if(strcmp(buf, zodis3) == 0)
		{
			komanda = 3;
		}
		else if(strcmp(buf, zodis4) == 0)
		{
			komanda = 2;
			
		}
		if(strcmp(buf, zodis5) == 0)
		{
			komanda = 5;
		}
		else if(strcmp(buf, zodis6) == 0)
		{
			komanda = 8;
		}
		if(strcmp(buf, zodis7) == 0)
		{
			komanda = 7;
		
		}
		else if(strcmp(buf, zodis8) == 0)
		{
			komanda = 8;
		}
		if(strcmp(buf, zodis9) == 0)
		{
			komanda = 9;
		}
		else if(strcmp(buf, zodis10) == 0)
		{
			komanda = 10;
		}
		if(strcmp(buf, zodis11) == 0)
		{
			komanda = 11;
		}
		else if(strcmp(buf, zodis12) == 0)
		{
			komanda = 12;
		}
		return komanda;
	}
	return 0;
}


void *BluetoothConnection(void *arg)
{	
 signal(SIGINT, sigintHandler);	 
 struct sockaddr_rc loc_addr = { 0 }, rem_addr = { 0 };
 char buf[1024] = { 0 };
 //char send[1024] = { 0 };
 int s, client;
 socklen_t opt = sizeof(rem_addr);
 bdaddr_t my_bdaddr_any = { 0, 0, 0, 0, 0, 0 };
 
 //Sensores
 int val1 = 420, val2 = 911, val3 = 112, val4 = 123, val5 = 321, val6 = 444;
 //Gyroscopio
 int val7 = 100, val8 = 100, val9 = 100, val10 = 100, val11 = 100, val12 = 100;
 //Temperature
 int val13 = 10,val14 = 1000;
 //Encoder
 int val15=1, val16=2;
 // allocate socket
 s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 // bind socket to port 1 of the first available
 // local bluetooth adapter
 loc_addr.rc_family = AF_BLUETOOTH;
 loc_addr.rc_bdaddr = my_bdaddr_any;
 loc_addr.rc_channel = (uint8_t) 1;
 bind(s, (struct sockaddr *)&loc_addr, sizeof(loc_addr));
 // put socket into listening mode
 listen(s, 1);
 printf("Put into listening mode\n");
 // accept one connection
 client = accept(s, (struct sockaddr *)&rem_addr, &opt);
 ba2str( &rem_addr.rc_bdaddr, buf );
 fprintf(stderr, "Accepted connection from %s\n", buf);
 memset(buf, 0, sizeof(buf));
 // send a message
	long long milliseconds;
	long long previous_ms=0;
	long long delay_time = 500L;
	int checkStatus;

	while(!exitFlag)
	{
		usleep(10);
		ReadData(client);
		milliseconds=current_timestamp();
		if (milliseconds-previous_ms>delay_time)
		{
        previous_ms=milliseconds;
		checkStatus = SendData(val1,val2,val3,val4,val5,val6,val7,val8,val9,val10,val11,val12,val13,val14,val15,val16,client);
		if(checkStatus < 0)
               break;
		}
	}
		// close connection
		close(client);
		close(s);
		return 0;	
}


int main(int argc, char **argv)
{	
 pthread_t pwmThread;
 pthread_t bluetoothThread;
 signal(SIGINT, sigintHandler);	 
 usleep(1000);
 pthread_create(&bluetoothThread, NULL, &BluetoothConnection, NULL);
 pthread_create(&pwmThread, NULL, &servocontrol, NULL);
 pthread_join(pwmThread, NULL);
 pthread_join(bluetoothThread, NULL);
  
  while (!exitFlag)
  {
    sleep(10000);
  }   
return 0;
}

[egrueda]

Bueno, veo un asunto más relacionado con un problema de programación (o una carencia) que con el tema raspberry.
Me sorprende que hayas creado ese programa en c++ pero no comprendas el funcionamiento de la función SendData() que se usa dentro del loop.
Creo que necesitas comprender el código que estás usando, y poder depurarlo para saber qué está haciendo y hasta dónde llega.
No parece un problema de sensores o de bluetooth, sino de programación enteramente.
Y por la información que ofreces, creo que en este punto no sabes siquiera si está leyendo los valores de los sensores o si está establecida la conexión bluetooth.
Quizá puedas encontrar ayuda en un foro de programación, que será más específico que este.