bool qb_class::open(const char* port, const int br) {
// Check connection state
if (qb_comm_ != NULL)
return false;
qb_comm_ = new comm_settings;
// Call open communication function
if (br >= 2000000)
openRS485(qb_comm_, port, B2000000);
else
openRS485(qb_comm_, port, B460800);
if (qb_comm_->file_handle == INVALID_HANDLE_VALUE)
return false;
// Set all cubes communication
for (int i = cube_chain_.size(); i--;)
cube_chain_[i]->cube_comm_ = qb_comm_;
// Set all hands communication
for (int i = hand_chain_.size(); i--;)
hand_chain_[i]->cube_comm_ = qb_comm_;
return true;
}
static void mdlStart(SimStruct *S)
{
char string_aux[255]; // auxiliar string
char serial_port_path[255]; // auxiliar string
char my_port[255];
int baud_rate;
comm_settings comm_settings_t;
ssPrintf("qbmove simulink library version: %s\n", QBMOVE_SIMULINK_VERSION);
//====================================================== opening serial port
for (int i = 0; param_com_port(i); ++i)
serial_port_path[i] = (char) param_com_port(i);
switch(param_com_baudrate){
case 1:
baud_rate = BAUD_RATE_2000000;
break;
case 2:
baud_rate = BAUD_RATE_460800;
break;
case 3:
baud_rate = BAUD_RATE_115200;
break;
case 4:
baud_rate = BAUD_RATE_57600;
break;
}
#if defined(_WIN32) || defined(_WIN64)
sprintf(my_port, "\\\\.\\%s", serial_port_path);
#else
strcpy(my_port, serial_port_path);
#endif
openRS485(&comm_settings_t, my_port, baud_rate);
pwork_handle = comm_settings_t.file_handle;
#if defined(_WIN32) || defined(_WIN64)
if(pwork_handle == INVALID_HANDLE_VALUE)
#else
if(pwork_handle == -1)
#endif
{
ssPrintf("Check your COM port. \nCould not connect to %s\n", serial_port_path);
out_handle = &pwork_handle;
// Stop simulation
mexEvalString("set_param(bdroot, 'SimulationCommand', 'stop')");
return;
}
out_handle = &pwork_handle;
}
int open_port(char* port_s) {
printf("Opening serial port...");
fflush(stdout);
openRS485(&comm_settings_t, port_s);
if(comm_settings_t.file_handle == INVALID_HANDLE_VALUE)
{
puts("Couldn't connect to the serial port.");
return 0;
}
usleep(500000);
printf("Done.\n");
return 1;
}
bool qbInterface::open(const char* port) {
// Check Connection State
if(cube_comm_ != NULL) {
cerr << "WARNING: Port already opened" << endl;
return false;
}
cube_comm_ = new comm_settings;
// Establish serial connection
openRS485(cube_comm_, port);
if (cube_comm_->file_handle == INVALID_HANDLE_VALUE) {
cerr << "ERROR: Unable to open port" << endl;
return false;
}
return true;
}
int open_port() {
FILE *file;
char port[255];
file = fopen(QBMOVE_FILE, "r");
fscanf(file, "serialport %s\n", port);
fclose(file);
openRS485(&comm_settings_t, port);
if(comm_settings_t.file_handle == INVALID_HANDLE_VALUE)
{
puts("Couldn't connect to the serial port.");
return 0;
}
usleep(500000);
return 1;
}
/** main loop
*/
int main (int argc, char **argv)
{
int option;
int longIndex = 0;
FILE *file;
comm_settings comm_settings_t;
int num_ports = 0;
char ports[10][255];
char list_of_devices[255];
char port_s[255];
int i = 0;
int aux_int;
unsigned char aux_uchar;
char aux_string[10000];
// unsigned int aux_uint;
global_args.device_id = 0;
global_args.flag_id = 0;
global_args.flag_pid_control = 0;
global_args.flag_activation = 0;
global_args.flag_deactivation = 0;
global_args.flag_input_mode = 0;
global_args.flag_pos_resolution = 0;
global_args.flag_meas_offset = 0;
global_args.flag_meas_multiplier = 0;
global_args.flag_list_devices = 0;
global_args.flag_serial_port = 0;
global_args.flag_ping = 0;
global_args.flag_restore = 0;
global_args.flag_verbose = 0;
global_args.flag_max_step = 0;
global_args.flag_max_current = 0;
//======================================================= processing options
while ((option = getopt_long( argc, argv, optString, longOpts, &longIndex )) != -1)
{
switch (option)
{
case 'i':
sscanf(optarg,"%d", &aux_int);
global_args.new_id = aux_int;
global_args.flag_id = 1;
break;
case 'k':
printf("Set the new values for the PID controller:\nK_p: ");
scanf("%f", global_args.pid_control);
printf("K_i: ");
scanf("%f", global_args.pid_control + 1);
printf("K_d: ");
scanf("%f", global_args.pid_control + 2);
printf("PID parameters: %f, %f, %f\n", global_args.pid_control[0], global_args.pid_control[1], global_args.pid_control[2]);
global_args.flag_pid_control = 1;
break;
case 'a':
global_args.flag_activation = 1;
break;
case 'd':
global_args.flag_deactivation = 1;
break;
case 'm':
puts("\nChoose Input Mode:\n");
puts("[0] - Default: Motors are commanded using the communication interface.");
puts("[1] - Encoder 3 works as a 'joystick' for the motors.");
puts("");
puts("--- EMG PART ---");
puts("[2] - EMG signal is used to proportionally control hand closure.");
puts("[3] - EMG signals are used to control opening and closing of the hand.");
puts("[4] - First EMG signal passing the threshold is used to control the hand.");
puts("");
scanf("%d",&aux_int);
if (aux_int < 1 || aux_int > 4) {
aux_int = 0;
}
global_args.input_mode = aux_int;
global_args.flag_input_mode = 1;
break;
case 's':
puts("[0] - 360 degrees (1 turn).");
puts("[1] - 720 degrees (2 turn).");
puts("[2] - 1440 degrees (4 turn).");
puts("[3] - 2880 degrees (8 turn).");
puts("[4] - 5760 degrees (16 turn).");
puts("[5] - 11520 degrees (32 turn).");
puts("[6] - 23040 degrees (64 turn).");
puts("[7] - 46080 degrees (128 turn).");
puts("[8] - 92160 degrees (256 turn).");
puts("");
for (i = 0; i < NUM_OF_SENSORS; i++) {
printf("\nChoose angle resolution for sensor #%d:\n", (i + 1));
puts("");
scanf("%d", &aux_int);
global_args.pos_resolution[i] = aux_int;
}
global_args.flag_pos_resolution = 1;
break;
case 'u':
#if NUM_OF_SENSORS == 4
sscanf( optarg,"%f,%f,%f,%f",
global_args.measurement_multiplier,
global_args.measurement_multiplier + 1,
global_args.measurement_multiplier + 2,
global_args.measurement_multiplier + 3 );
#else
sscanf( optarg,"%f,%f,%f",
global_args.measurement_multiplier,
global_args.measurement_multiplier + 1,
global_args.measurement_multiplier + 2 );
#endif
global_args.flag_meas_multiplier = 1;
break;
case 'o':
#if NUM_OF_SENSORS == 4
sscanf( optarg,"%hd,%hd,%hd,%hd",
global_args.measurement_offset,
global_args.measurement_offset + 1,
global_args.measurement_offset + 2,
global_args.measurement_offset + 3);
#else
sscanf( optarg,"%hd,%hd,%hd",
global_args.measurement_offset,
global_args.measurement_offset + 1,
global_args.measurement_offset + 2);
#endif
global_args.flag_meas_offset = 1;
break;
case 'p':
global_args.flag_ping = 1;
break;
case 'r':
global_args.flag_restore = 1;
break;
case 't':
global_args.flag_serial_port = 1;
break;
case 'l':
global_args.flag_list_devices = 1;
break;
case 'v':
global_args.flag_verbose = 1;
break;
case 'q':
global_args.flag_set_limit = 1;
break;
case 'w':
sscanf(optarg, "%d", &aux_int);
global_args.activate_limit = aux_int;
global_args.flag_activate_limit = 1;
break;
case 'b':
printf("Set the new values for max step:\nmax_step_pos: ");
scanf("%d", &global_args.max_step_pos);
printf("max_step_neg: ");
scanf("%d", &global_args.max_step_neg);
global_args.flag_max_step = 1;
break;
case 'c':
sscanf(optarg, "%d", &aux_int);
global_args.max_current = (int16_t)aux_int;
global_args.flag_max_current = 1;
break;
case 'h':
case '?':
default:
display_usage();
return 0;
break;
}
}
if((optind == 1) | (global_args.flag_verbose & (optind == 2)))
{
display_usage();
return 0;
}
//====================================================== setting serial port
if(global_args.flag_serial_port)
{
num_ports = RS485listPorts(ports);
if(num_ports)
{
puts("\nChoose serial port:\n");
for(i = 0; i < num_ports; ++i)
{
printf("[%d] - %s\n\nSerial port: ", i+1, ports[i]);
scanf("%d", &aux_int);
if( aux_int && (aux_int <= num_ports) )
{
file = fopen(HANDMOVE_FILE, "w");
fprintf(file,"serialport %s",ports[aux_int-1]);
fclose(file);
}
else puts("Choice not available");
}
}
else
puts("No serial port available.");
return 0;
}
//=============================================== reading configuration file
if(global_args.flag_verbose)
puts("Reading configuration file.");
file = fopen(HANDMOVE_FILE, "r");
if (file == NULL) {
printf("Error opening %s\n", HANDMOVE_FILE);
return 0;
}
fscanf(file, "serialport %s", port_s);
fclose(file);
if(global_args.flag_verbose)
printf("Serial port: %s.\n",port_s);
//====================================================== opening serial port
if(global_args.flag_verbose)
puts("Connecting to serial port.");
openRS485(&comm_settings_t, port_s);
if(comm_settings_t.file_handle == INVALID_HANDLE_VALUE)
{
puts("Couldn't connect to the serial port.");
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
if(global_args.flag_verbose)
puts("Serial port connected.");
//============================================================= list devices
if(global_args.flag_list_devices)
{
aux_int = RS485ListDevices(&comm_settings_t, list_of_devices);
printf("\nNumber of devices: %d \n\n", aux_int);
puts("List of devices:");
for(i = 0; i < aux_int; ++i)
{
printf("%d\n",list_of_devices[i]);
}
return 0;
}
//======================================================== gettind device id
if (argc - optind == 1)
{
sscanf(argv[optind++],"%d",&global_args.device_id);
if(global_args.flag_verbose)
printf("Device ID:%d\n", global_args.device_id);
}
//===================================================================== ping
// If ping... then DOESN'T PROCESS OTHER COMMANDS
if(global_args.flag_ping)
{
if(global_args.flag_verbose)
puts("Pinging serial port.");
if(global_args.device_id){
commGetInfo(&comm_settings_t, global_args.device_id, INFO_ALL, aux_string);
}
else
RS485GetInfo(&comm_settings_t, aux_string);
puts(aux_string);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//======================================================== ...
if ((!(argc - optind == 1)) & (global_args.flag_verbose))
puts("No device ID was chosen. Running in broadcasting mode.");
//===================================================== setting parameter ID
// If id is changed... then DOESN'T PROCESS OTHER COMMANDS
if(global_args.flag_id)
{
if(global_args.flag_verbose)
printf("Changing device's id %d to %d.\n", global_args.device_id ,global_args.new_id);
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_ID, &global_args.new_id, 1);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//================================================== setting PID parameter
if(global_args.flag_pid_control)
{
if(global_args.flag_verbose) {
printf("Changing PID control constant to:\n");
printf("P: %f\n", global_args.pid_control[0]);
printf("I: %f\n", global_args.pid_control[1]);
printf("D: %f\n", global_args.pid_control[2]);
}
//da correggere
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_PID_CONTROL, global_args.pid_control, 3);
//usleep(100000); //100 ms
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//===================================== setting parameter startup activation
if(global_args.flag_activation)
{
if(global_args.flag_verbose)
puts("Changing startup activation to on.");
aux_uchar = 3;
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_STARTUP_ACTIVATION, &aux_uchar, 1);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//===================================== setting parameter startup activation
if(global_args.flag_deactivation)
{
if(global_args.flag_verbose)
puts("Changing startup activation to off.");
aux_uchar = 0;
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_STARTUP_ACTIVATION, &aux_uchar, 1);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//============================================== setting parameter input mode
if(global_args.flag_input_mode)
{
if(global_args.flag_verbose)
puts("Changing input mode.");
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_INPUT_MODE, &global_args.input_mode, 1);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//==================================== setting parameter position resolution
if(global_args.flag_pos_resolution)
{
if(global_args.flag_verbose)
puts("Changing angle resolution.");
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_POS_RESOLUTION, global_args.pos_resolution, NUM_OF_SENSORS);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//==================================== setting parameter position multiplier
if(global_args.flag_meas_multiplier)
{
if(global_args.flag_verbose)
puts("Changing measurement position multipliers.");
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_MEASUREMENT_MULTIPLIER, global_args.measurement_multiplier, NUM_OF_SENSORS);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//======================================== setting parameter position offset
if(global_args.flag_meas_offset)
{
if(global_args.flag_verbose)
puts("Changing position offset.");
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_MEASUREMENT_OFFSET, global_args.measurement_offset, NUM_OF_SENSORS);
commStoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//======================================= restore factory default parameters
if(global_args.flag_restore)
{
if(global_args.flag_verbose)
puts("Restoring factory default parameters.");
commRestoreParams(&comm_settings_t, global_args.device_id);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
//====================================================== set position limits
if (global_args.flag_set_limit) {
short int limits[4];
printf("\nInf limit 1: ");
scanf("%hd", limits);
printf("\nSup limit 1: ");
scanf("%hd", limits + 1);
printf("\nInf limit 2: ");
scanf("%hd", limits + 2);
printf("\nSup limit 2: ");
scanf("%hd", limits + 3);
printf("limits: %d, %d, %d, %d\n", limits[0], limits[1], limits[2], limits[3]);
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_POS_LIMIT, limits, 4);
usleep(100000);
commStoreParams(&comm_settings_t, global_args.device_id);
}
//================================================== activate position limit
if (global_args.flag_activate_limit) {
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_POS_LIMIT_FLAG, &(global_args.activate_limit), 1);
commStoreParams(&comm_settings_t, global_args.device_id);
}
//============================================================= set max step
if (global_args.flag_max_step) {
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_MAX_STEP_POS, &(global_args.max_step_pos), 1);
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_MAX_STEP_NEG, &(global_args.max_step_neg), 1);
usleep(100000);
commStoreParams(&comm_settings_t, global_args.device_id);
}
if (global_args.flag_max_current) {
commSetParam(&comm_settings_t, global_args.device_id,
PARAM_CURRENT_LIMIT, &(global_args.max_current), 1);
commStoreParams(&comm_settings_t, global_args.device_id);
}
//========================== closing serial port and closing the application
closeRS485(&comm_settings_t);
if(global_args.flag_verbose)
puts("Closing the application.");
return 0;
}
