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; }