o_serial_servos.cpp
6.21 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
//
// 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;
}