Commit 7d649897ddfd946d4b6d8414f4932dca4357bb49

Authored by Arnaud Blanchard
2 parents 58bf184a b1967d15

Fix merge

... ... @@ -18,7 +18,7 @@ find_package(blc_program REQUIRED)
add_definitions(${BL_DEFINITIONS})
include_directories(${BL_INCLUDE_DIRS})
add_executable(i_keyboard src/main.cpp)
add_executable(i_keyboard src/i_keyboard.cpp)
target_link_libraries(i_keyboard ${BL_LIBRARIES})
... ...
... ... @@ -222,11 +222,12 @@ static void loop_float(){
int main(int argc, char** argv){
char *default_output=NULL;
char const *extension;
char const *channel_name, *type_str, *str_quitting_key, *str_neutral_key, *max_str, *min_str, *str_neutral_value, *toggle_mode, *step_size_str, *filename;
char const *channel_name, *type_str, *str_quitting_key, *str_neutral_key, *max_str, *min_str, *str_neutral_value, *toggle_mode, *step_size_str, *filename, *file_select_name;
char const *key_nb_str;
int i;
char *pos;
uchar answer;
blc_array array_select;
asprintf(&default_output, "/%s%d", basename(argv[0]), getpid()); //This will not be free but it is only allocate once
blc_program_set_description("Get keyboard inputs");
... ... @@ -239,6 +240,7 @@ int main(int argc, char** argv){
blc_program_add_option(&str_quitting_key, 'q', "quitting_key", "string", "Define the key used to quit", "q");
blc_program_add_option(&key_nb_str, 's', "size", "integer", "Size of key vector", NULL);
blc_program_add_option(&type_str, 't', "type", "UIN8|FL32", "Define the type of the result", "UIN8");
blc_program_add_option(&file_select_name, 'F', "select", "filename", "File among which vlues will be selected", NULL);
blc_program_add_option(&max_str, 'M', "max", "real", "Define the normed max value", "1.0");
blc_program_add_option(&str_neutral_value, 'N', "neutral_value", "real", "Define the normed neutral value", "0.5");
blc_program_add_option(&step_size_str, 'S', "step", "real", "Set the normed step size", NULL);
... ... @@ -293,11 +295,20 @@ int main(int argc, char** argv){
case 'FL32':
FOR(i, keys_nb) channel.floats[i]=neutral_value;
break;
default:EXIT_ON_ARRAY_ERROR(&channel, "Type not mnaged");
default:EXIT_ON_ARRAY_ERROR(&channel, "Type not managed");
break;
}
}
if (file_select_name){
extension = blc_get_filename_extension(file_select_name);
if (strcmp(extension, "tsv")!=0) EXIT_ON_ERROR("Only .tsv files are managed, however your extension is '%s' in '%s'.", extension, file_select_name);
// array_select.updatewith_tsv_file
// SYSTEM_ERROR_CHECK(file_select=fopen(file_select_name, "r"), NULL, "Impossible to open '%s'", file_select_name);
}
if (display){
if ((channel.type!='UIN8') && (channel.type!='FL32')) EXIT_ON_ARRAY_ERROR(&channel, "This type can not be displayed. Only 'UIN8' or 'FL32' can.");
}
... ...
# Copyright ETIS — ENSEA, Université de Cergy-Pontoise, CNRS (2011 - 2016)
# Author: Arnaud Blanchard
# This software is governed by the CeCILL v2.1 license under French law and abiding by the rules of distribution of free software.
# You can use, modify and/ or redistribute the software under the terms of the CeCILL v2.1 license as circulated by CEA, CNRS and INRIA at the following URL "http://www.cecill.info".
# As a counterpart to the access to the source code and rights to copy, modify and redistribute granted by the license,
# users are provided only with a limited warranty and the software's author, the holder of the economic rights, and the successive licensors have only limited liability.
# In this respect, the user's attention is drawn to the risks associated with loading, using, modifying and/or developing or reproducing the software by the user in light of its specific status of free software,
# that may mean that it is complicated to manipulate, and that also therefore means that it is reserved for developers and experienced professionals having in-depth computer knowledge.
# Users are therefore encouraged to load and test the software's suitability as regards their requirements in conditions enabling the security of their systems and/or data to be ensured  and, more generally, to use and operate it in the same conditions as regards security.
# The fact that you are presently reading this means that you have had knowledge of the CeCILL v2.1 license and that you accept its terms.
cmake_minimum_required(VERSION 2.6)
project(serial)
subdirs(serial_servos)
subdirs(o_serial_servos)
... ...
Programs to communicate with serial devices
===========================================
serial_servos
============
Communicate with a ssc12|32 or pololu board to drive servos in mode Mini-SSC II.
Install
-------
You need to install [blaar](https://blaar.org), it will be included. Then in the blaar directory:
usage
-----
./run.sh serial/serial_servos --baudrate=38400 --servos="1 5 7"
Communicate in **Mini SSC-II** mode (compatible SSC12, SSC32 and pololu) with **baudrate** in parameter waiting for space or tab separated values for **servos** 1, 5 and 7.
You can fill these values with the keyboard or with a **cat** and a file e.g motor_positions ( line starting with # are comments ):
#motor1 motor5 motor7
25 36 89
Then call :
cat motor_positions | ./run.sh serial/serial_servos --baudrate=38400 --servos="1 5 7"
You can also give a file with multiline positions and add a **--period=1000** option. It will successively apply these positions with 1000ms interval.
Use **--help** for all options
arduino
=======
Communicate with an arduino board to acquire analog values, set servos positions, ... (in developmment)
More information about serial communication
===========================================
Good general information for unix programming: [Serial Programming Guide for POSIX Operating Systems](https://www.cmrr.umn.edu/~strupp/serial.html)
... ...
# Copyright ETIS — ENSEA, Université de Cergy-Pontoise, CNRS
# Author: Arnaud Blanchard (November 2016)
# This software is governed by the CeCILL v2.1 license under French law and abiding by the rules of distribution of free software.
# You can use, modify and/ or redistribute the software under the terms of the CeCILL v2.1 license as circulated by CEA, CNRS and INRIA at the following URL "http://www.cecill.info".
# As a counterpart to the access to the source code and rights to copy, modify and redistribute granted by the license,
# users are provided only with a limited warranty and the software's author, the holder of the economic rights, and the successive licensors have only limited liability.
# In this respect, the user's attention is drawn to the risks associated with loading, using, modifying and/or developing or reproducing the software by the user in light of its specific status of free software,
# that may mean that it is complicated to manipulate, and that also therefore means that it is reserved for developers and experienced professionals having in-depth computer knowledge.
# Users are therefore encouraged to load and test the software's suitability as regards their requirements in conditions enabling the security of their systems and/or data to be ensured  and, more generally, to use and operate it in the same conditions as regards security.
# The fact that you are presently reading this means that you have had knowledge of the CeCILL v2.1 license and that you accept its terms.
cmake_minimum_required(VERSION 2.6)
project(arduino)
find_package(blc_channel REQUIRED)
find_package(blc_program REQUIRED)
add_definitions(${BL_DEFINITIONS})
include_directories(${BL_INCLUDE_DIRS} ../include)
add_executable(arduino arduino.cpp ../src/serial.cpp)
target_link_libraries(arduino ${BL_LIBRARIES})
... ...
//
// Created by Arnaud Blanchard on 22/12/15.
// Copyright ETIS 2015. All rights reserved.
//
#include "serial.h"
#include "blc_program.h"
#include "blc_channel.h"
#include <errno.h> //errno
#include <sys/uio.h> //writev
#include <unistd.h>
#define PINS_MAX 256
char const *display;
char const *device_name;
void display_cb(char const*, void*){
if (display) {
blc_eprint_del_end_screen();
display=NULL;
}else display="1";
}
void on_quit(){
if (display) blc_eprint_del_end_screen();
}
int main(int argc, char **argv){
blc_channel channel;
char buff[256];
char const *message;
blc_mem sending_mem, receiving_mem;
char const *analog, *filename_str, *channel_name, *servos;
int pos, tmp_pos;
char ready, answer_size;
int display_height=16;
char const *baudrate_str;
int fd, i;
uchar pins_ids[PINS_MAX];
uint16_t *pins_values;
int pins_nb, tmp_pin_id, rv;
FILE *record_file=NULL;
struct timeval timeout;
fd_set set;
int received_bytes_nb, baudrate;
int timeout_seconds=3;
struct sending_header{
char command;
uchar pins_nb;
}*sending_header;
struct receiving_header{
char answer;
uchar pins_nb;
}*receiving_header;
blc_program_add_option(&analog, 'a', "analog", "integer list", "list of analog port to read", NULL);
blc_program_add_option(&baudrate_str, 'b', "baudrate", "integer", "serial port speed", "115200");
blc_program_add_option(&display, 'd', "display", NULL, "display graph and values", NULL);
blc_program_add_option(&servos, 's', "servos", "string", "pins to drive servo motors \"pin1 pin2 pin3 ...\"", NULL);
blc_program_add_parameter(&device_name, "device", 0, "device name", "/dev/ttyUSB0");
blc_program_init(&argc, &argv, on_quit);
baudrate=strtol(baudrate_str, NULL, 10);
if (analog){
pos=0;
pins_nb=0;
while (sscanf(analog+pos, "%d%n", &tmp_pin_id, &tmp_pos)==1){
if (pins_nb == PINS_MAX) EXIT_ON_ERROR("Too many pins (max %d)", PINS_MAX);
pins_ids[pins_nb]=tmp_pin_id; //Necessary to convert int to uchar
pos+=tmp_pos;
pins_nb++;
}
//We allocate the header and the list of pins to read
sending_mem.allocate(sizeof(struct sending_header));
sending_mem.append((char*)pins_ids, pins_nb*sizeof(uchar));
//We assign the first part of the memory to the header
sending_header=(struct sending_header*)sending_mem.data;
sending_header->command='a';
sending_header->pins_nb=pins_nb;
receiving_mem.allocate(sizeof(struct receiving_header)+pins_nb*sizeof(uint16_t));
receiving_header=(struct receiving_header*)receiving_mem.data;
pins_values=(uint16_t*)(receiving_mem.chars+sizeof(struct receiving_header));
channel_name="/toto";
channel.create_or_open(channel_name, BLC_CHANNEL_WRITE, 'UIN8', 'NDEF', 1, pins_nb);
channel.publish();
}
fd = open_serial_port(device_name, baudrate, 8, 'N', 2);
eprint_serial_port_options(fd);
FD_ZERO(&set); /* clear the set */
FD_SET(fd, &set); /* add our file descriptor to the set */
timeout.tv_sec = timeout_seconds;
timeout.tv_usec = 0;
rv = select(fd + 1, &set, NULL, NULL, &timeout);
if (rv==0) EXIT_ON_ERROR("Timeout after %ds waiting arduino answer to message:\n'%s'\non '%s'.", timeout_seconds, message, device_name);
else if (rv==-1) EXIT_ON_ERROR("Error\n");
BLC_COMMAND_LOOP(100000){
serial_send_buffer(fd, sending_mem.chars, sending_mem.size);
//qreceived_bytes_nb=read(fd, receiving_mem.chars, receiving_mem.size);
received_bytes_nb=serial_receive_buffer(fd, receiving_mem.chars, receiving_mem.size, 3000);
if (received_bytes_nb>0){
if (receiving_header->answer != sending_header->command) {
switch (receiving_header->answer){
case 'e':fprintf(stderr, "error %d\n", receiving_header->pins_nb);
break;
case 'r':fprintf(stderr, "The board has been restarted\n");
break;
default:fprintf(stderr, "receiving answer size '%d': '%c' instead of '%c'\n", received_bytes_nb, receiving_header->answer, sending_header->command);
}
// tcflush(fd, TCIOFLUSH);
}else{
FOR(i, pins_nb) channel.uchars[i]=pins_values[i]/4;
if (display){
blc_eprint_del_end_screen();
channel.fprint_graph_uchars(stderr, "arduino", display_height, 256, 0, "pins ", "volt ");
fprintf(stderr,"values:");
FOR(i, pins_nb) fprintf(stderr, "%d ", pins_values[i]);
blc_eprint_cursor_up(display_height);
}else write(STDERR_FILENO, ".", 1);
if (record_file){
FOR(i, pins_nb) fprintf(record_file, "%d ", pins_values[i]);
fprintf(record_file, "\n");
}
}
}
}
return EXIT_SUCCESS;
}
... ...
//#include <Servo.h>
int main() {
struct struct_header {
char answer_code;
unsigned char pins_nb;
} header;
struct struct_servo {
unsigned char pin_id;
unsigned char position;
} servo;
int low;
// Servo myservo;
unsigned char pin_id;
int i, value;
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Serial.begin(9600); //Maybe it has to be tune for different kinds of arduino
// myservo.attach(9);
Serial.write("r"); //ready
// Serial.flush();
while (1) {
// value=analogRead(0);
// myservo.write(value/4);
// if ( Serial.available() > 0) digitalWrite(13, LOW);
/*
if (Serial.readBytes(&header.answer_code, 1) == 1) {
digitalWrite(13, LOW);
switch (header.answer_code) {
//We read analog values
case 'a':
digitalWrite(13, LOW);
// Serial.readBytes(&header.pins_nb, 1);
Serial.write((uint8_t*)&header, sizeof(header));
for (i = 0; i != header.pins_nb; i++) {
Serial.readBytes(&pin_id, 1);
value = analogRead(pin_id);
Serial.write((uint8_t*)&value, sizeof(value));
}
break;
case 's':
Serial.readBytes(&header.pins_nb, 1); //We read number of requests
Serial.write((uint8_t*)&header, sizeof(header)); // We answer that we have understood
for (i = 0; i != header.pins_nb; i++) {
Serial.readBytes((uint8_t*)&servo, 2);
// myservo.write(servo.position);
}
break;
case 'S':
Serial.readBytes(&header.pins_nb, 1); //We read number of requests
Serial.write((uint8_t*)&header, sizeof(header)); // We answer that we have understood
for (i = 0; i != header.pins_nb; i++) {
Serial.readBytes((uint8_t*)&servo, 2);
// myservo.write(servo.position);
}
break;
default:
Serial.write('e');
Serial.write(1);
break;
}
Serial.write('e');
Serial.write(1);
// Serial.flush();
}
else{ //timeout;
digitalWrite(13, LOW);
delay(1000);
Serial.write('t');
Serial.write(1);
digitalWrite(13, HIGH);
}*/
//Serial.write("u\n");
delay(500);
if (low==1){
digitalWrite(13, HIGH);
low=0;
}
else {
digitalWrite(13, LOW);
low=1;
}
}
}
... ...
# Copyright ETIS — ENSEA, Université de Cergy-Pontoise, CNRS (2011 - 2016)
# Author: Arnaud Blanchard (November 2016)
# This software is governed by the CeCILL v2.1 license under French law and abiding by the rules of distribution of free software.
# You can use, modify and/ or redistribute the software under the terms of the CeCILL v2.1 license as circulated by CEA, CNRS and INRIA at the following URL "http://www.cecill.info".
# As a counterpart to the access to the source code and rights to copy, modify and redistribute granted by the license,
# users are provided only with a limited warranty and the software's author, the holder of the economic rights, and the successive licensors have only limited liability.
# In this respect, the user's attention is drawn to the risks associated with loading, using, modifying and/or developing or reproducing the software by the user in light of its specific status of free software,
# that may mean that it is complicated to manipulate, and that also therefore means that it is reserved for developers and experienced professionals having in-depth computer knowledge.
# Users are therefore encouraged to load and test the software's suitability as regards their requirements in conditions enabling the security of their systems and/or data to be ensured  and, more generally, to use and operate it in the same conditions as regards security.
# The fact that you are presently reading this means that you have had knowledge of the CeCILL v2.1 license and that you accept its terms.
cd `dirname $0`
echo
echo "Checking `basename $PWD`"
echo "===================="
echo
echo "No check for now"
echo
... ...
#ifndef SERIAL_PORT_H
#define SERIAL_PORT_H
#include <stdlib.h>
#define EXIT_ON_SERIAL_PORT_ERROR(fd, ...) serial_port_fatal_error(fd, __FILE__, __FUNCTION__, __LINE__, __VA_ARGS__)
int open_serial_port(char const *device_name, int baudrate, int bits_nb, char parity, int stop_bits);
void eprint_serial_port_infos(int fd);
void serial_send_buffer(int fd, char *buffer, size_t size);
int serial_receive_buffer(int fd, char *buffer, size_t size, int timeout_ms);
void serial_port_fatal_error(int fd, const char *name_of_file, const char* name_of_function, int numero_of_line, const char *message, ...);
#endif
... ...
# Copyright ETIS — ENSEA, Université de Cergy-Pontoise, CNRS
# Author: Arnaud Blanchard (November 2016)
# This software is governed by the CeCILL v2.1 license under French law and abiding by the rules of distribution of free software.
# You can use, modify and/ or redistribute the software under the terms of the CeCILL v2.1 license as circulated by CEA, CNRS and INRIA at the following URL "http://www.cecill.info".
# As a counterpart to the access to the source code and rights to copy, modify and redistribute granted by the license,
# users are provided only with a limited warranty and the software's author, the holder of the economic rights, and the successive licensors have only limited liability.
# In this respect, the user's attention is drawn to the risks associated with loading, using, modifying and/or developing or reproducing the software by the user in light of its specific status of free software,
# that may mean that it is complicated to manipulate, and that also therefore means that it is reserved for developers and experienced professionals having in-depth computer knowledge.
# Users are therefore encouraged to load and test the software's suitability as regards their requirements in conditions enabling the security of their systems and/or data to be ensured  and, more generally, to use and operate it in the same conditions as regards security.
# The fact that you are presently reading this means that you have had knowledge of the CeCILL v2.1 license and that you accept its terms.
cmake_minimum_required(VERSION 2.6)
project(o_serial_servos)
find_package(blc_channel REQUIRED)
find_package(blc_program REQUIRED)
add_definitions(${BL_DEFINITIONS})
include_directories(${BL_INCLUDE_DIRS} ../include)
add_executable(o_serial_servos o_serial_servos.cpp ../src/serial.cpp)
target_link_libraries(o_serial_servos ${BL_LIBRARIES})
... ...
//
// 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;
}
... ...
# Copyright ETIS — ENSEA, Université de Cergy-Pontoise, CNRS
# Author: Arnaud Blanchard (November 2016)
# This software is governed by the CeCILL v2.1 license under French law and abiding by the rules of distribution of free software.
# You can use, modify and/ or redistribute the software under the terms of the CeCILL v2.1 license as circulated by CEA, CNRS and INRIA at the following URL "http://www.cecill.info".
# As a counterpart to the access to the source code and rights to copy, modify and redistribute granted by the license,
# users are provided only with a limited warranty and the software's author, the holder of the economic rights, and the successive licensors have only limited liability.
# In this respect, the user's attention is drawn to the risks associated with loading, using, modifying and/or developing or reproducing the software by the user in light of its specific status of free software,
# that may mean that it is complicated to manipulate, and that also therefore means that it is reserved for developers and experienced professionals having in-depth computer knowledge.
# Users are therefore encouraged to load and test the software's suitability as regards their requirements in conditions enabling the security of their systems and/or data to be ensured  and, more generally, to use and operate it in the same conditions as regards security.
# The fact that you are presently reading this means that you have had knowledge of the CeCILL v2.1 license and that you accept its terms.
cmake_minimum_required(VERSION 2.6)
project(serial_servos)
find_package(blc_program REQUIRED)
add_definitions(${BL_DEFINITIONS})
include_directories(${BL_INCLUDE_DIRS} ../include)
add_executable(serial_servos serial_servos.cpp ../src/serial.cpp)
target_link_libraries(serial_servos ${BL_LIBRARIES})
... ...
//
// Created by Arnaud Blanchard on 22/12/15.
// Copyright ETIS 2015. All rights reserved.
//
#include "serial.h"
#include "blc_program.h"
#include <errno.h> //errno
#include <unistd.h>
#include <stdio.h> //getline
#include <stdlib.h> //strtol
char const *device_name;
int run=1;
void ask_quit(){
run=0;
}
/*Scan string input as uchars. Return the number of char read. String address is updated with the current position*/
int sscan_uchars(char **string, uchar *uchars, int uchars_nb){
int i, result;
int value;
int read_chars_nb;
FOR(i, uchars_nb){
result=sscanf(*string, "%d%n\n", &value, &read_chars_nb); //If there is no "\n" it does not matter
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;
}
else break;
*string +=read_chars_nb;
}
return i;
}
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
}
int main(int argc, char **argv){
blc_array array;
char const *period_str;
int period;
char buffer[256];
char *input_line=NULL;
size_t input_line_capablity=0;
blc_mem sending_mem, receiving_mem;
char const *servos;
char const *baudrate_str;
int fd;
size_t i;
int baudrate;
int motors_nb=32; //ssc32
int ports_map[32];
struct timeval timer;
long int duration;
ssize_t input_string_length;
char* current_pos;
size_t read_numbers_nb;
blc_program_add_option(&baudrate_str, 'b', "baudrate", "integer", "serial port speed", "115200");
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_parameter(&device_name, "device", 0, "device name", "/dev/ttyUSB0");
blc_program_init(&argc, &argv, NULL);
baudrate=strtol(baudrate_str, NULL, 10);
motors_nb=mapping_ports(servos, ports_map, 32);
period=strtol(period_str, NULL, 10);
fprintf(stderr, "Mode mini SSC-II (compatible with SSC12, SSC32 and pololu)\n");
fd = open_serial_port(device_name, baudrate, 8, 'N', 2);
array.init('UIN8', 'NDEF', 1, motors_nb);
i=0;
blc_command_add("q", (type_blc_command_cb)ask_quit, NULL, NULL, NULL);
while(run==1){
blc_us_time_diff(&timer);
input_string_length=getline(&input_line, &input_line_capablity, stdin);
if (input_string_length==-1){
if (feof(stdin)) ask_quit();
else EXIT_ON_SYSTEM_ERROR("Getting input on stdin");
}
else if (input_string_length!=1){
if (input_line[0]=='#') continue; //It is a comment
current_pos=input_line;
read_numbers_nb=sscan_uchars(&current_pos, array.uchars, array.total_length);
//If it was not a number we try to interpret it as a command
if (current_pos==input_line) blc_command_interpret_string(input_line, input_string_length-1);
else if (read_numbers_nb != array.total_length) fprintf(stderr, "Wrong number of values. Waiting '%ld' values but reading '%ld'. Use for exemple option ' --servos=\"1 5 7\"' in order to drive only the motors 1,5,7\n", array.total_length, read_numbers_nb);
else if (current_pos < input_line + input_string_length-2) fprintf(stderr, "There is more text than needed for numbers.'%s' has not been interpreted\n", current_pos);
else {
//Format SSC2
buffer[0]=255;
FOR(i, array.total_length){
buffer[1]=ports_map[i];
buffer[2]=array.uchars[i];
SYSTEM_SUCCESS_CHECK(write(fd, buffer, 3), 3,"Writing to the serial port");
}
}
duration=blc_us_time_diff(&timer);
if (duration < period) usleep(period*1000-duration);
}
}
return EXIT_SUCCESS;
}
... ...
#include "serial.h"
#include <sys/termios.h>
#include <fcntl.h> //open
#include <stdlib.h>
#include <unistd.h> //access
#include <errno.h>//errno
#include <signal.h>//raise
#include "blc_core.h"
static void get_path_with_fd(int fd, char *path, size_t size){
#ifdef __linux__
char fd_name[PATH_MAX];
ssize_t len;
snprintf(fd_name, PATH_MAX, "/proc/self/fd/%d", fd);
SYSTEM_ERROR_CHECK(len=readlink(fd_name, path, size),-1, "Searching for '%s'", fd_name);
if (len < size) path[len]=0;
else EXIT_ON_ERROR("Path name too long");
#else
if (size < PATH_MAX) EXIT_ON_ERROR("Your buffer is too small");
SYSTEM_ERROR_CHECK(fcntl(fd, F_GETPATH, path),-1, "Getting path name of the serial port");
#endif
}
static int baudrates[][2]={
{50, B50},
{75, B75},
{110, B110},
{134, B134},
{150, B150},
{200, B200},
{300, B300},
{600, B600},
{1200, B1200},
{1800, B1800},
{2400, B2400},
{4800, B4800},
{9600, B9600},
{19200, B19200},
{38400, B38400},
{57600, B57600},
{115200, B115200},
{230400, B230400},
#ifdef __linux__
{460800, B460800},
{500000, B500000},
{576000, B576000},
{921600, B921600},
{1000000, B1000000},
{1152000, B1152000},
{1500000, B1500000},
{2000000, B2000000},
{2500000, B2500000},
{3000000, B3000000},
{3500000, B3500000},
{4000000, B4000000},
#endif
};
static int baudrates_nb=sizeof(baudrates)/(2*sizeof(int));
int open_serial_port(char const *device_name, int baudrate, int bits_nb, char parity, int stop_bits){
int fd;
int i, baudrate_code=0;
struct termios toptions;
FOR_INV(i, baudrates_nb) if (baudrates[i][0]==baudrate) break;
if (i==-1) {
fprintf(stderr, "Baudrate '%d' is not possible. The possibilities are:\n", baudrate);
FOR(i, baudrates_nb) fprintf(stderr, "%d, ", baudrates[i][0]);
EXIT_ON_ERROR("Wrong baudrate");
}
else baudrate_code=baudrates[i][1];
/* open serial port maybe we should add O_NDELAY */
SYSTEM_ERROR_CHECK(fd = open(device_name, O_RDWR | O_NOCTTY), -1, "device name:%s", device_name);
/* get current serial port settings */
SYSTEM_ERROR_CHECK(tcgetattr(fd, &toptions), -1, "device '%s'", device_name);
SYSTEM_ERROR_CHECK(cfsetispeed(&toptions, baudrate_code), -1, "Setting input speed of '%d' code '%d.", baudrate, baudrate_code);
SYSTEM_ERROR_CHECK(cfsetospeed(&toptions, baudrate_code), -1, "Setting ouput speed of '%d' code '%d.", baudrate, baudrate_code);
/* 8 bits, no parity, 2 stop bits */
switch (parity){
case 'N':
toptions.c_cflag &= ~PARENB;
break;
case 'E':
toptions.c_cflag |= PARENB;
toptions.c_cflag &= ~PARODD;
break;
case 'O':
toptions.c_cflag |= PARENB | PARODD;
break;
default:
EXIT_ON_ERROR("Parity '%c' is not possible. It can be:\n'N'->NONE\n'E'->EVEN\n'O'->ODD\n", parity);
}
switch (stop_bits){
case 1: toptions.c_cflag &= ~CSTOPB;
break;
case 2: toptions.c_cflag |= CSTOPB;
break;
default: EXIT_ON_ERROR("You can not have '%d' stop bits. Possibilities are 1 or 2.", stop_bits);
}
toptions.c_cflag &= ~CSIZE;
switch (bits_nb){
case 5:
toptions.c_cflag |= CS5;
break;
case 6:
toptions.c_cflag |= CS6;
break;
case 7:
toptions.c_cflag |= CS7;
break;
case 8:
toptions.c_cflag |= CS8;
break;
default: EXIT_ON_ERROR("You can not have size of '%d' bits. Possibilities are 5,6,7 or 8.", bits_nb);
}
/* commit the serial port settings */
SYSTEM_ERROR_CHECK(tcsetattr(fd, TCSANOW, &toptions), -1, "Setting serial port options");
return fd;
}
//Print on stderr the option of the serial port
void eprint_serial_port_infos(int fd){
char path[PATH_MAX];
int baudrate_code, i, input_speed, output_speed;
struct termios toptions;
//may not work on linux use somthing with readlink (/proc/...) ...
get_path_with_fd(fd, path, sizeof(path));
fprintf(stderr, "%s: ", path);
SYSTEM_ERROR_CHECK(tcgetattr(fd, &toptions), -1, "Getting serial port options");
baudrate_code=cfgetispeed(&toptions);
FOR_INV(i, baudrates_nb) if (baudrate_code==baudrates[i][1]) break;
if (i==-1) PRINT_WARNING("Unknown input speed of baudrate code '%d", baudrate_code);
else input_speed= baudrates[i][0];
baudrate_code=cfgetospeed(&toptions);
FOR_INV(i, baudrates_nb) if (baudrate_code==baudrates[i][1]) break;
if (i==-1) PRINT_WARNING("Unknown input speed of baudrate code '%d", baudrate_code);
else output_speed= baudrates[i][0];
if (input_speed==output_speed) fprintf(stderr, "%d bauds, ", input_speed);
else {
fprintf(stderr, "input speed:%d bauds, ", input_speed);
fprintf(stderr, "output speed:%d bauds, ", output_speed);
}
if ((toptions.c_cflag & PARENB)==0) fprintf(stderr, "no parity, ");
else if (toptions.c_cflag & PARODD) fprintf(stderr, "odd parity, ");
else fprintf(stderr, "even parity, ");
if (toptions.c_cflag & CSTOPB) fprintf(stderr, "two stop bits, ");
else fprintf(stderr, "one stop bit, ");
switch (toptions.c_cflag & CSIZE){
case CS5:
fprintf(stderr, "char size 5 bits\n");
break;
case CS6:
fprintf(stderr, "char size 6 bits\n");
break;
case CS7:
fprintf(stderr, "char size 7 bits\n");
break;
case CS8:
fprintf(stderr, "char size 8 bits\n");
break;
}
}
void serial_reconnect(int fd){
}
void serial_send_buffer(int fd, char *buffer, size_t size){
size_t offset=0;
ssize_t ret;
do{
ret=write(fd, buffer, size-offset);
if (ret == -1) {
switch (errno){
case ENXIO:color_fprintf(BLC_RED, stderr, "Device not configured. It may have been disconnected\n");
/* while (access(device_name, W_OK | R_OK)!=0) {
sleep(1);
fprintf(stderr, "waiting for %s\n", device_name);
}
*fd=open_serial_port(device_name, bauds);*/
break;
default:PRINT_SYSTEM_ERROR("failing writing");
}
// tcflush(*fd, TCIOFLUSH);
break;
}
else {
if (ret==0) fprintf(stderr, "write waiting\n");
else offset+=ret;
}
tcdrain(fd);
}
while(offset!=size);
//q tcdrain(fd);
}
int serial_receive_buffer(int fd, char *buffer, size_t size, int timeout_ms){
size_t offset=0;
ssize_t ret;
struct timeval timeout;
fd_set set;
int rv;
div_t division;
do{
FD_ZERO(&set); /* clear the set */
FD_SET(fd, &set); /* add our file descriptor to the set */
division=div(timeout_ms, 1000);
timeout.tv_sec = division.quot;
timeout.tv_usec = division.rem*1000;
rv = select((fd) + 1, &set, NULL, NULL, &timeout);
if (rv==0) {
fprintf(stderr, "timeout\n");
break;
}
else if (rv==-1) fprintf(stderr, "error\n");
else{
ret=read(fd, buffer, size-offset);
if (ret==-1){
switch (errno){
case ENXIO:color_fprintf(BLC_RED, stderr, "Device not configured. It may have been disconnected\n");
/* while (access(device_name, W_OK | R_OK)!=0) {
sleep(1);
fprintf(stderr, "waiting for %s\n", device_name);
}
// *fd=open_serial_port(device_name, bauds);*/
break;
default:PRINT_SYSTEM_ERROR("failing writing");
}
// tcflush(*fd, TCIOFLUSH);
}
else if (ret==0){
fprintf(stderr, "waiting\n");
// tcflush(*fd, TCIOFLUSH);
break;
}
else offset+=ret;
}
}while(offset!=size);
return offset;
}
// Envoie un message d'erreur avec name_of_file, name_of_function, number_of_line et affiche le message formate avec les parametres variables. Puis exit le programme avec le parametre EXIT_FAILURE. To be used with EXIT_ON_ERROR.
void serial_port_fatal_error(int fd, const char *name_of_file, const char* name_of_function, int numero_of_line, const char *message, ...){
va_list arguments;
va_start(arguments, message);
fprintf(stderr, "\n%s: %s \t %s \t %i :\nError: ", blc_program_name, name_of_file, name_of_function, numero_of_line);
color_vfprintf(BLC_BRIGHT_RED, stderr, message, arguments);
va_end(arguments);
fprintf(stderr, "\nSerial port: ");
eprint_serial_port_infos(fd);
fprintf(stderr, "\n\n");
fflush(stderr);
raise(SIGABRT);
exit(EXIT_FAILURE);
}
... ...