o_serial_servos.cpp 6.21 KB
//
//  Created by Arnaud Blanchard on 22/12/15.
//  Copyright ETIS 2015. All rights reserved.
//
#include "serial.h"
#include "blc_core.h" //EXIT_ON..
#include "blc_channel.h"//blc_channel
#include "blc_program.h" //blc_program...
#include <errno.h> //errno
#include <unistd.h>
#include <stdio.h> //getline
#include <stdlib.h> //strtol

/*Scan string input as uchars. Return the number of char read.*/
int fscan_uchars(FILE *file, uchar *uchars, int uchars_nb){
    int i, result;
    int value;
    
    FOR(i, uchars_nb){
        result=fscanf(file, "%d", &value);
        if (result==1){
            if ((value < 0) || ( value > 254)) { //255 is a special value for synchronisation
                PRINT_WARNING("You need number in [0, 254] instead of '%ld' in %dth value", value, i);
                break;
            }
            else uchars[i]=value;
            fscanf(file, "\n");
        }
        else break;
    }
    return i;
}

/**
 We update the name by replacing eventual '<pid>' by the real pid.
 We check the name to be a valid blc_channel name. If yes we retrun 1, 0 otherwise.*/
int check_blc_channel_name(char **name){
    char *pid_pos, *new_name;
    int pos, ret;
    
    pid_pos=strstr(*name, "<pid>");
    if (pid_pos) {
        asprintf(&new_name, "%*s%d",(int)(*name-pid_pos), *name, getpid());
        free(*name);
        *name=new_name;
    }
    
    SYSTEM_ERROR_CHECK(ret=sscanf(*name, "%*[:/.^]%*[^/]%n", &pos), -1, "Checking '%s' for blc_channel name", new_name);
    
    if ((ret==2) && (pos==strlen(*name))) return 1;
    else return 0;
}

int mapping_ports(char const *string, int *ports, int ports_max){
    char const *current_pos=string;
    int port_id, i, read_chars_nb, result;
    
    FOR(i, ports_max){
        result=sscanf(current_pos, "%d%n", &port_id, &read_chars_nb);
        if (read_chars_nb==0) break;
        else if (result==0) EXIT_ON_ERROR("Wrong value '%s' for a port id", current_pos);
        else {
            if (port_id>ports_max) EXIT_ON_ERROR("Your port '%d' must be lower than '%d'", port_id, ports_max);
            ports[i] = port_id;
        }
        if (current_pos[0]==0) break;
        else current_pos+=read_chars_nb;
    }
    return i; //Number of mapped ports
}

void servo_send_commands(int fd, uchar *values, int values_nb, int *ports_map){
    uchar buffer[256];
    int i;

    buffer[0]=255;
    FOR(i, values_nb){
        buffer[1]=ports_map[i];
        buffer[2]=values[i];
        //      SYSTEM_SUCCESS_CHECK(write(fd, buffer, 3), 3,"Writing to the serial port");
        SYSTEM_SUCCESS_CHECK(write(fd, buffer, 3), 3, "Writing to the serial port");
    }
}

void serial_file_loop(int fd, char const *input_name, int motors_nb, int *ports_map, int period){
    blc_mem command_line;
    size_t command_length;
    
    FILE *file;
    blc_array input;
    int read_values_nb, duration;
    struct timeval timer;
    
    if(strcmp(input_name, "-")==0) file=stdin;
    else SYSTEM_ERROR_CHECK(file=fopen(input_name, "r"), NULL, "opening '%s'", input_name);
    input.init('UIN8', 'NDEF', 1, motors_nb);
    
    blc_status=BLC_RUN;
    while(blc_status==BLC_RUN){
        blc_us_time_diff(&timer);
        read_values_nb=fscan_uchars(file, input.uchars, motors_nb);
        if (read_values_nb==motors_nb){
            servo_send_commands(fd, input.uchars, input.total_length, ports_map);
        }else{
            if (read_values_nb==0){
                command_length=getline(&command_line.chars, &command_line.size, file);
                blc_command_interpret_string(command_line.chars, command_length-1); //-1 for last \n
            }
            else {
                PRINT_WARNING("The number of input values '%d' does not fit the number of motors (%d)", read_values_nb, motors_nb);        //Format SSC2
            }
        }
        duration=blc_us_time_diff(&timer);
        if (duration < period) usleep(period*1000-duration);
    }
}

void serial_channel_loop(int fd, char const *input_name,  int motors_nb, int *ports_map, int period){
    blc_channel input;
    
    input.open(input_name, BLC_CHANNEL_READ);
    //Synchronize the loop with the input channel
    blc_loop_try_add_waiting_semaphore(input.sem_new_data);
    blc_loop_try_add_posting_semaphore(input.sem_ack_data);
    
    if (input.type!='UIN8') EXIT_ON_CHANNEL_ERROR(&input, "The input type must be 'UIN8' (unsigned char) but it is not:");
    else if (input.total_length!=motors_nb) EXIT_ON_CHANNEL_ERROR(&input, "The numbers of the motors '%d' does not fit the length of the input", motors_nb);

    BLC_COMMAND_LOOP(period*1000){
        servo_send_commands(fd, input.uchars, input.total_length, ports_map);
    }
}

int main(int argc, char **argv){
    blc_channel input;
    char const *period_str, *device_name, *input_name;
    int period;
    char const *servos, *file_select_name;
    char const  *baudrate_str;
    int  fd;
    int  baudrate;
    int motors_nb=32; //ssc32
    int  ports_map[32];
    
    blc_program_add_option(&baudrate_str, 'b', "baudrate", "integer",  "serial port speed", "115200");
    blc_program_add_option(&file_select_name, 'f', "select-file", "file",  "file among which select lines", NULL);
    blc_program_add_option(&period_str, 'p', "period", "integer", "minimal period between two orders in ms", "0");
    blc_program_add_option(&servos, 's', "servos", "string",  "pins to drive servo motors \"pin1 pin2 pin3 ...\"", "0");
    blc_program_add_option(&device_name, 'D', "device", "string", "device to use for the sound", "/dev/ttyUSB0");
    blc_program_add_parameter(&input_name, "-|file|blc_channel-in", 1, "device name", NULL);
    blc_program_init(&argc, &argv, NULL);
    
    SYSTEM_SUCCESS_CHECK(sscanf(baudrate_str, "%d", &baudrate), 1, "parsing baudrate '%s'", baudrate_str);
    SYSTEM_SUCCESS_CHECK(sscanf(period_str, "%d", &period), 1, "parsing period '%s'", period_str);
    motors_nb=mapping_ports(servos, ports_map, 32);
    fprintf(stderr, "Mode mini SSC-II (compatible with SSC12, SSC32 and pololu)\n");
    
    fd = open_serial_port(device_name, baudrate, 8, 'N', 2);
    
    if (check_blc_channel_name((char**)&input_name)) serial_channel_loop(fd, input_name, motors_nb, ports_map, period);
    else serial_file_loop(fd, input_name, motors_nb, ports_map, period);
    
    close(fd);
    
    return EXIT_SUCCESS;
}