C linux NXT

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/rfcomm.h>

#define INQUIRY_LEN       8
#define MAX_RSP           5
#define READ_NAME_TIMEOUT 0
#define NAME_BUFFER_LEN   255

char bt_address[19]             = {0};
char bt_name[NAME_BUFFER_LEN]   = {0};
struct sockaddr_rc sockaddr     = {0};
int dev_id, dev_sock, bt_socket, num_rsp, i, ret;
int bytes_sent, bytes_received;
inquiry_info *ii = NULL;
long inquiry_flags;

void signal_handler(int s);

//NXT
enum NXT_PORT
{
    PORT_A = 0x0,
    PORT_B = 0x1,
    PORT_C = 0x2
};

enum MOTOR_MODE
{
    MOTORON   = 0x01,
    BRAKE     = 0x02,
    REGULATED = 0x04
};

enum REGULATION_MODE
{
    REGULATION_MODE_IDLE        = 0x00,
    REGULATION_MODE_MOTOR_SPEED = 0x01,
    REGULATION_MODE_MOTOR_SYNC  = 0x02
};

enum RUNSTATE
{
    MOTOR_RUN_STATE_IDLE     = 0x00,
    MOTOR_RUN_STATE_RAMPUMP  = 0x10,
    MOTOR_RUN_STATE_RUNNING  = 0x20,
    MOTOR_RUN_STATE_RAMPDOWN = 0x40
};

int play_tone(short frequency, short duration);
int set_output_state(enum NXT_PORT port, int8_t power, enum MOTOR_MODE mode, enum REGULATION_MODE regulation_mode,
                     int8_t turn_ratio, enum RUNSTATE run_state, uint32_t tacho_limit);


int main(int argc, char **argv)
{
    (void) signal(SIGINT, signal_handler);

    /*dev_id = hci_get_route(NULL);
    if(dev_id < 0)
    {
        perror("no bt adapters connected");
    }

    dev_sock = hci_open_dev(dev_id);
    if(dev_sock < 0)
    {
        perror("could not open adapter");
    }

    ii = (inquiry_info *)malloc(sizeof(inquiry_info) * MAX_RSP);
    inquiry_flags = IREQ_CACHE_FLUSH;

    num_rsp = hci_inquiry(dev_id, INQUIRY_LEN, MAX_RSP, NULL, &ii, inquiry_flags);
    if(num_rsp < 0)
    {
        perror("could not inquiry devices");
    }
    else if(num_rsp == 0)
    {
        perror("no devices have been found");
    }
    else
    {
        printf("Found %i bluetooth device(s)!\n", num_rsp);
    }

    for(i = 0; i < num_rsp; i++)
    {
        printf("->Device number %i:\n", i);
        ba2str(&(ii[i].bdaddr), bt_address);
        printf("----->Bt addr: %s\n", bt_address);
        ret =  hci_read_remote_name(dev_sock, &(ii[i].bdaddr), NAME_BUFFER_LEN, bt_name, READ_NAME_TIMEOUT);
        if(ret < 0)
        {
            printf("----->Device name: could not read device name\n");
        }
        else
        {
            printf("----->Device name: %s\n", bt_name);
        }
    }*/

    char dest_addr[18] = "00:16:53:0A:3B:98";

    bt_socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
    sockaddr.rc_family = AF_BLUETOOTH;
    str2ba(dest_addr, &sockaddr.rc_bdaddr);
    sockaddr.rc_channel = 1;

    ret = connect(bt_socket, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
    if(ret < 0)
    {
        perror("error connecting to the device");
    }


    while(1)
    {
        bytes_sent = set_output_state(PORT_A, 100, MOTORON, REGULATION_MODE_IDLE, 100, MOTOR_RUN_STATE_RUNNING, 360);
        printf("sent %i bytes(s)\n", bytes_sent);
        usleep(2 * 1000 * 1000);
    }

    close(bt_socket);
    printf("\nEND\n");
    return 0;
}

//NXT

int play_tone(short frequency, short duration)
{
    uint8_t command[8] = {6, 0, 0x80, 0x03, frequency & 0xFF, (frequency>>8) & 0xFF, duration & 0xFF, (duration>>8) & 0xFF};
    return send(bt_socket, command, 8, 0);
}


int set_output_state(enum NXT_PORT port, int8_t power, enum MOTOR_MODE mode, enum REGULATION_MODE regulation_mode,
                     int8_t turn_ratio, enum RUNSTATE run_state, uint32_t tacho_limit)
{
    uint8_t command[14] = {12, 0, 0x80, 0x04,
                           port, power, mode, regulation_mode,
                           turn_ratio, run_state, tacho_limit & 0xFF,
                           (tacho_limit>>8) & 0xFF, (tacho_limit>>16) & 0xFF,
                           (tacho_limit>>24) & 0xFF
                           };
    return send(bt_socket, command, 14, 0);
}

//
void signal_handler(int s)
{
    close(bt_socket);
    printf("\nsignal: %i\n", s);
    exit(0);
}