void do_expmt()
{
Graph g;
g.init_graph();
int row_num = 4;
int column_num = 5;
Node nodes[MAX_VERTEX_NUM]; //Set of Nodes;
init_nodes(nodes, g, row_num, column_num);
Node target; // The Target
int tracking_id = init_target(target, nodes);
int target_from = nodes[tracking_id].get_start();
double total_target_dist = 0;
while (total_target_dist < Target_Moving_Dist_Threshold) {
int target_to = get_target_next_step(target, g);
//printf("(%d -> %d)\n", target_from, target_to); //test
//vector<int> shortest_paths[MAX_VERTEX_NUM];
//vector<int> vertexes_selected;
//double shortest_dists[MAX_VERTEX_NUM];
//shortest_path_dijkstra(g, target_from, target_to,\
// shortest_paths, vertexes_selected,\
// shortest_dists, Search_Threshold);
//int is_enclosed = 0;
//if (is_target_enclosed(nodes, \
// shortest_paths, vertexes_selected)) {
// is_enclosed = 1;
//}
vector<Expect_Saving> expects_set;
//if (!is_enclosed) {
//get_all_expects(expects_set, nodes, g, shortest_paths,\
// vertexes_selected, shortest_dists, tracking_id);
get_all_expects(expects_set, nodes, g, target_from, target_to,\
tracking_id);
//}
//printf("get_all_expects passed.\n"); //test
mobile_node_schedule(expects_set, nodes, target_from, target_to,\
g, tracking_id);
//printf("mobile_node_schedule passed.\n"); //test
target.set_start(target_to);
target.set_end(target_to);
target.set_x_(g.get_vertex(target_to).get_x_());
target.set_y_(g.get_vertex(target_to).get_y_());
double target_moved_dist = g.get_arcs_length(target_from, target_to);
total_target_dist += target_moved_dist;
target_from = target_to;
}
//double total_node_moving_dist = total_target_dist - Total_Distance_Saved;
//printf("Total_Moving_Distance = %f (/%f)\n", \
// total_node_moving_dist, total_target_dist);
printf("Total_Node_Moving_Dist = %f (/%f)\n", \
Total_Node_Moving_Dist, total_target_dist);
FILE *result_file = fopen(Result_File, "a");
//printf("Result_File = %s\n", Result_File);
if (result_file) {
fprintf(result_file, "%d %f\n", \
Mobile_Node_Num, Total_Node_Moving_Dist/total_target_dist);
fclose(result_file);
} else {
printf("Error: cannot create file \"%s\"\n", Result_File);
}
}
int main(int argc, char *argv[]) {
if (argc == 1) {
printf("Usage: %s name1 size1 ... nameN sizeN\n\n"
"Creates device files for dataports in /dev with given names\n"
"and sizes (in bytes).\n"
"Dataport ids will be allocated in increasing order from left\n"
"to right, starting from 1.\n"
"Sizes must match those given in the corresponding camkes spec.\n"
"This should only be run once per boot.\n", argv[0]);
return 1;
}
dataport_t dataports[MAX_NUM_DATAPORTS];
int num_dataports = process_dataport_args(argc - 1, argv + 1,
dataports,
MAX_NUM_DATAPORTS);
unsigned major = device_get_major(DEVICE_NAME);
assert(major != NO_MAJOR);
printf("Device \"%s\" found with major number %d\n\n", DEVICE_NAME, major);
make_nodes(dataports, num_dataports, major);
init_nodes(dataports, num_dataports);
return 0;
}
/*
initialize network
*/
bool setup_network(void) {
now = time(NULL);
init_events();
init_connections();
init_subnets();
init_nodes();
init_edges();
init_requests();
if(get_config_int(lookup_config(config_tree, "PingInterval"), &pinginterval)) {
if(pinginterval < 1) {
pinginterval = 86400;
}
} else
pinginterval = 60;
if(!get_config_int(lookup_config(config_tree, "PingTimeout"), &pingtimeout))
pingtimeout = 5;
if(pingtimeout < 1 || pingtimeout > pinginterval)
pingtimeout = pinginterval;
if(!get_config_int(lookup_config(config_tree, "MaxOutputBufferSize"), &maxoutbufsize))
maxoutbufsize = 10 * MTU;
if(!setup_myself())
return false;
return true;
}
void atheme_setup(void)
{
#if HAVE_UMASK
/* file creation mask */
umask(077);
#endif
base_eventloop = mowgli_eventloop_create();
hooks_init();
db_init();
init_resolver();
translation_init();
#ifdef ENABLE_NLS
language_init();
#endif
init_nodes();
init_confprocess();
init_newconf();
servtree_init();
hook_add_event("email_canonicalize");
hook_add_email_canonicalize(canonicalize_email_case);
/* No need for canonicalize_emails(), no accounts exist yet. */
modules_init();
pcommand_init();
authcookie_init();
common_ctcp_init();
}
void atheme_setup(void)
{
#if HAVE_UMASK
/* file creation mask */
umask(077);
#endif
base_eventloop = mowgli_eventloop_create();
hooks_init();
db_init();
init_resolver();
translation_init();
#ifdef ENABLE_NLS
language_init();
#endif
init_nodes();
init_confprocess();
init_newconf();
servtree_init();
register_email_canonicalizer(canonicalize_email_case, NULL);
modules_init();
pcommand_init();
authcookie_init();
common_ctcp_init();
}
/*
initialize network
*/
bool setup_network(meshlink_handle_t *mesh) {
init_connections(mesh);
init_nodes(mesh);
init_edges(mesh);
init_requests(mesh);
mesh->pinginterval = 60;
mesh->pingtimeout = 5;
maxoutbufsize = 10 * MTU;
if(!setup_myself(mesh))
return false;
return true;
}
static void
compile(char *base_name)
{
/* symbol table */
state.global = push_symbol_table(state.global, 1);
init_nodes();
state.root = NULL;
/* open and parse the source public file */
state.src = NULL;
open_src(state.basefilename, source_with);
if (state.src)
yyparse();
/* open input file */
open_src(state.srcfilename, source);
/* parse the input file */
yyparse();
/* optimize the intermediate code */
/* optimize(); */
/* open output filename */
strcpy(state.asmfilename, base_name);
strcat(state.asmfilename, ".asm");
state.output.f = fopen(state.asmfilename, "w");
if (state.output.f == NULL) {
perror(state.asmfilename);
exit(1);
}
/* write the assembly output */
write_asm();
fclose(state.output.f);
/* destory symbol table for the current module */
state.global = pop_symbol_table(state.global);
/* free all the memory */
free_nodes();
return;
}
struct BTree *init_tree5(void)
{
struct BTreeNode *nodes[5];
init_nodes(&nodes[0], 5);
set_children(nodes[0], nodes[1], nodes[2]);
set_children(nodes[1], nodes[3], nodes[4]);
set_children(nodes[2], NULL, NULL);
set_children(nodes[3], NULL, NULL);
set_children(nodes[4], NULL, NULL);
return build_tree("Binary Tree 5",
" ┌────── 0 ──────┐\n"
"┌────── 1 ──────┐ 2\n"
"3 4",
"5", "3", nodes[0]);
}
struct BTree *init_tree7(void)
{
struct BTreeNode *nodes[6];
init_nodes(&nodes[0], 6ul);
set_children(nodes[0], nodes[2], nodes[1]);
set_children(nodes[1], nodes[4], nodes[3]);
set_children(nodes[2], NULL, NULL);
set_children(nodes[3], NULL, NULL);
set_children(nodes[4], NULL, nodes[5]);
set_children(nodes[5], NULL, NULL);
return build_tree("Binary Tree 7",
" ┌────── 0 ──────┐\n"
" 2 ┌────── 1 ──────┐\n"
" 4 ──────┐ 3\n"
" 5",
"6", "4", nodes[0]);
}
struct BTree *init_tree2(void)
{
struct BTreeNode *nodes[6];
init_nodes(&nodes[0], 6ul);
set_children(nodes[0], nodes[2], nodes[1]);
set_children(nodes[1], NULL, NULL);
set_children(nodes[2], nodes[3], nodes[4]);
set_children(nodes[3], NULL, NULL);
set_children(nodes[4], nodes[5], NULL);
set_children(nodes[5], NULL, NULL);
return build_tree("Binary Tree 2",
" ┌────── 0 ──────┐\n"
"┌────── 2 ──────┐ 1\n"
"3 ┌────── 4\n"
" 5",
"6", "4", nodes[0]);
}
int plugin_init(int type)
{
switch (type)
{
case PROC_MAIN:
break;
case PROC_CONN:
break;
case PROC_WORK:
setup_timer();
init_connect_to_db();
init_connect_to_alarm();
init_nodes();
init_update_notice();
init_log_archive(config_get_strval("log_dir", "./log/"), 2 * 3600);
break;
default:
break;
}
return 0;
}
//-----------------------------------------------------------------------------
// Main... process command line parameters, and then setup our listening
// sockets and event loop.
int main(int argc, char **argv)
{
system_data_t sysdata;
///============================================================================
/// Initialization.
///============================================================================
init_sysdata(&sysdata);
init_settings(&sysdata);
get_options(sysdata.settings, argc, argv);
init_maxconns(&sysdata);
init_daemon(&sysdata);
init_events(&sysdata);
init_logging(&sysdata);
logger(sysdata.logging, 1, "System starting up");
init_signals(&sysdata);
init_buffers(&sysdata);
init_servers(&sysdata);
init_stats(&sysdata);
init_risp(&sysdata);
init_nodes(&sysdata);
init_msglist(&sysdata);
init_queues(&sysdata);
init_controllers(&sysdata);
///============================================================================
/// Main Event Loop.
///============================================================================
// enter the event loop.
logger(sysdata.logging, 1, "Starting Event Loop");
assert(sysdata.evbase);
event_base_loop(sysdata.evbase, 0);
logger(sysdata.logging, 1, "Shutdown preparations complete. Shutting down now.");
///============================================================================
/// Shutdown
///============================================================================
cleanup_events(&sysdata);
cleanup_controllers(&sysdata);
cleanup_queues(&sysdata);
cleanup_msglist(&sysdata);
cleanup_nodes(&sysdata);
cleanup_risp(&sysdata);
cleanup_stats(&sysdata);
cleanup_servers(&sysdata);
cleanup_buffers(&sysdata);
cleanup_signals(&sysdata);
logger(sysdata.logging, 1, "Shutdown complete.\n");
cleanup_logging(&sysdata);
cleanup_daemon(&sysdata);
cleanup_maxconns(&sysdata);
cleanup_settings(&sysdata);
cleanup_sysdata(&sysdata);
// good-bye.
return 0;
}
int main()
{
float target_torque = -25.0; // mNm
float actual_torque = 0; // mNm
float tolerance = 0.76; // mNm
int actual_position = 0; // ticks
int actual_velocity = 0; // rpm
int ack = 0;
int sdo_update = 1; // 1- yes / 0 - no
int slave_number = 0;
/* Initialize Ethercat Master */
init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize all connected nodes with Mandatory Motor Configurations (specified under config/motor/)*/
init_nodes(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize torque parameters */
initialize_torque(slave_number, slv_handles);
/* Initialize the node specified with slave_number with Profile Torque(TQ) configurations (specified under config/motor/)*/
set_operation_mode(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Enable operation of node in TQ mode */
enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
while(1)
{
/* Update the process data (EtherCat packets) sent/received from the node */
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
if(master_setup.op_flag) /*Check if the master is active*/
{
/* Send target torque for the node specified by slave_number */
set_torque_mNm(target_torque, slave_number, slv_handles);
/* Check if target torque is reached with specified tolerance */
ack = target_torque_reached(slave_number, target_torque, tolerance, slv_handles);
/* Read actual node sensor values */
actual_torque= get_torque_actual_mNm(slave_number, slv_handles);
actual_position = get_position_actual_ticks(slave_number, slv_handles);
actual_velocity = get_velocity_actual_rpm(slave_number, slv_handles);
printf("target_torque %f \n",target_torque);
printf("actual_torque %f position %d velocity %d ack %d\n", actual_torque, actual_position, actual_velocity, ack);
}
if(ack == 1)
{
break;
}
}
printf("reached \n");
ack = 0;
while(!ack)
{
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
if(master_setup.op_flag) /*Check if the master is active*/
{
actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
actual_position = get_position_actual_ticks(slave_number, slv_handles);
actual_velocity = get_velocity_actual_rpm(slave_number, slv_handles);
if(actual_torque > tolerance || actual_torque < -tolerance)
{
/* Quick stop Profile Torque mode (for emergency) */
quick_stop_torque(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
ack = 1;
}
printf("actual_torque %f position %d velocity %d ack %d\n", actual_torque, actual_position, actual_velocity, ack);
}
}
printf("reached \n");
/* Regain control of node to continue after quick stop */
renable_ctrl_quick_stop(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
set_operation_mode(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/*target_torque = 15.0; // mNm
while(1)
{
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
if(master_setup.op_flag) // Check if the master is active
{
set_torque_mNm(target_torque, slave_number, slv_handles);
ack = target_torque_reached(slave_number, target_torque, tolerance, slv_handles);
actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
printf("target_torque %f \n",target_torque);
printf("actual_torque %f ack %d\n", actual_torque, ack);
}
if(ack == 1)
{
break;
}
}
*/
/*printf("reached \n");
while(1)
{
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
if(master_setup.op_flag)//Check if we are up
{
actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
printf("actual_torque %f \n",actual_torque);
}
}*/
/* Shutdown node operations */
shutdown_operation(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
return 0;
}
int main(int argc, char *argv[])
{
bool have_conf = false;
bool have_log = false;
char buf[32];
int i, pid, r;
FILE *pid_file;
const char *pidfilename = RUNDIR "/atheme.pid";
#ifdef HAVE_GETRLIMIT
struct rlimit rlim;
#endif
curr_uplink = NULL;
mowgli_init();
/* Prepare gettext */
#ifdef ENABLE_NLS
setlocale(LC_ALL, "");
bindtextdomain(PACKAGE_NAME, LOCALEDIR);
textdomain(PACKAGE_NAME);
#endif
/* change to our local directory */
if (chdir(PREFIX) < 0)
{
perror(PREFIX);
return 20;
}
#ifdef HAVE_GETRLIMIT
/* it appears certian systems *ahem*linux*ahem*
* don't dump cores by default, so we do this here.
*/
if (!getrlimit(RLIMIT_CORE, &rlim))
{
rlim.rlim_cur = rlim.rlim_max;
setrlimit(RLIMIT_CORE, &rlim);
}
#endif
/* do command-line options */
while ((r = getopt(argc, argv, "c:dhrl:np:v")) != -1)
{
switch (r)
{
case 'c':
config_file = sstrdup(optarg);
have_conf = true;
break;
case 'd':
log_force = true;
break;
case 'h':
print_help();
exit(EXIT_SUCCESS);
break;
case 'r':
readonly = true;
break;
case 'l':
log_path = sstrdup(optarg);
have_log = true;
break;
case 'n':
runflags |= RF_LIVE;
break;
case 'p':
pidfilename = optarg;
break;
case 'v':
print_version();
exit(EXIT_SUCCESS);
break;
default:
printf("usage: atheme [-dhnvr] [-c conf] [-l logfile] [-p pidfile]\n");
exit(EXIT_SUCCESS);
break;
}
}
if (!have_conf)
config_file = sstrdup(SYSCONFDIR "/atheme.conf");
if (!have_log)
log_path = sstrdup(LOGDIR "/atheme.log");
cold_start = true;
runflags |= RF_STARTING;
me.kline_id = 0;
me.start = time(NULL);
CURRTIME = me.start;
srand(arc4random());
me.execname = argv[0];
/* set signal handlers */
init_signal_handlers();
/* initialize strshare */
strshare_init();
/* open log */
log_open();
mowgli_log_set_cb(process_mowgli_log);
slog(LG_INFO, "%s is starting up...", PACKAGE_STRING);
/* check for pid file */
if ((pid_file = fopen(pidfilename, "r")))
{
if (fgets(buf, 32, pid_file))
{
pid = atoi(buf);
if (!kill(pid, 0))
{
fprintf(stderr, "atheme: daemon is already running\n");
exit(EXIT_FAILURE);
}
}
fclose(pid_file);
}
#if HAVE_UMASK
/* file creation mask */
umask(077);
#endif
event_init();
hooks_init();
init_netio();
init_socket_queues();
db_init();
translation_init();
#ifdef ENABLE_NLS
language_init();
#endif
init_nodes();
init_confprocess();
init_newconf();
servtree_init();
modules_init();
pcommand_init();
conf_init();
if (!conf_parse(config_file))
{
slog(LG_ERROR, "Error loading config file %s, aborting",
config_file);
exit(EXIT_FAILURE);
}
if (config_options.languagefile)
{
slog(LG_DEBUG, "Using language: %s", config_options.languagefile);
if (!conf_parse(config_options.languagefile))
slog(LG_INFO, "Error loading language file %s, continuing",
config_options.languagefile);
}
authcookie_init();
common_ctcp_init();
if (!backend_loaded && authservice_loaded)
{
slog(LG_ERROR, "atheme: no backend modules loaded, see your configuration file.");
exit(EXIT_FAILURE);
}
/* we've done the critical startup steps now */
cold_start = false;
/* load our db */
if (db_load)
db_load();
else if (backend_loaded)
{
slog(LG_ERROR, "atheme: backend module does not provide db_load()!");
exit(EXIT_FAILURE);
}
db_check();
#ifdef HAVE_FORK
/* fork into the background */
if (!(runflags & RF_LIVE))
{
close(0);
if (open("/dev/null", O_RDWR) != 0)
{
slog(LG_ERROR, "unable to open /dev/null??");
exit(EXIT_FAILURE);
}
if ((i = fork()) < 0)
{
slog(LG_ERROR, "can't fork into the background");
exit(EXIT_FAILURE);
}
/* parent */
else if (i != 0)
{
slog(LG_INFO, "pid %d", i);
slog(LG_INFO, "running in background mode from %s", PREFIX);
exit(EXIT_SUCCESS);
}
/* parent is gone, just us now */
if (setsid() < 0)
{
slog(LG_ERROR, "unable to create new session: %s", strerror(errno));
exit(EXIT_FAILURE);
}
dup2(0, 1);
dup2(0, 2);
}
else
{
slog(LG_INFO, "pid %d", getpid());
slog(LG_INFO, "running in foreground mode from %s", PREFIX);
}
#else
slog(LG_INFO, "running in foreground mode from %s", PREFIX);
#endif
#ifdef HAVE_GETPID
/* write pid */
if ((pid_file = fopen(pidfilename, "w")))
{
fprintf(pid_file, "%d\n", getpid());
fclose(pid_file);
}
else
{
fprintf(stderr, "atheme: unable to write pid file\n");
exit(EXIT_FAILURE);
}
#endif
/* no longer starting */
runflags &= ~RF_STARTING;
/* we probably have a few open already... */
me.maxfd = 3;
/* DB commit interval is configurable */
if (db_save && !readonly)
event_add("db_save", db_save, NULL, config_options.commit_interval);
/* check expires every hour */
event_add("expire_check", expire_check, NULL, 3600);
/* check k/x/q line expires every minute */
event_add("kline_expire", kline_expire, NULL, 60);
event_add("xline_expire", xline_expire, NULL, 60);
event_add("qline_expire", qline_expire, NULL, 60);
/* check authcookie expires every ten minutes */
event_add("authcookie_expire", authcookie_expire, NULL, 600);
/* reseed rng a little every five minutes */
event_add("rng_reseed", rng_reseed, NULL, 293);
me.connected = false;
uplink_connect();
/* main loop */
io_loop();
/* we're shutting down */
hook_call_shutdown();
if (db_save && !readonly)
db_save(NULL);
remove(pidfilename);
errno = 0;
if (curr_uplink != NULL && curr_uplink->conn != NULL)
sendq_flush(curr_uplink->conn);
connection_close_all();
me.connected = false;
/* should we restart? */
if (runflags & RF_RESTART)
{
slog(LG_INFO, "main(): restarting");
#ifdef HAVE_EXECVE
execv(BINDIR "/atheme-services", argv);
#endif
}
slog(LG_INFO, "main(): shutting down");
log_shutdown();
return 0;
}
int main() {
int target_velocity = 2000; //rpm
int acceleration = 1000; //rpm/s
int deceleration = 1000; //rpm/s
int actual_velocity = 0; // rpm
int actual_position; // ticks
float actual_torque; // mNm
int steps = 0;
int velocity_ramp = 0; // rpm
/* Initialize Ethercat Master */
init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize all connected nodes with Mandatory Motor Configurations (specified in config/motor/)*/
init_nodes(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize the node specified with ECAT_SLAVE_0 with CSV configurations (specified in config/motor/)*/
set_operation_mode(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
/* Enable operation of node in CSV mode */
enable_operation(ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
/* Initialize velocity profile parameters */
steps = init_velocity_profile_params(target_velocity,
actual_velocity, acceleration, deceleration, ECAT_SLAVE_0,
slv_handles);
/* catch interrupt signal */
signal(SIGINT, INThandler);
/* Just for better printing result */
printf("\n");
system("setterm -cursor off");
while(1)
{
if (master_setup.op_flag && actual_velocity == 0) /*Check if the master is active and we haven't started moving yet*/
{
for (int step = 1; step < steps + 1; step++) {
/* Update the process data (EtherCat packets) sent/received from the node */
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Generate target velocity steps */
velocity_ramp = generate_profile_velocity(step, ECAT_SLAVE_0,
slv_handles);
/* Send target velocity for the node specified by ECAT_SLAVE_0 */
set_velocity_rpm(velocity_ramp, ECAT_SLAVE_0, slv_handles);
/* Read actual node sensor values */
actual_velocity
= get_velocity_actual_rpm(ECAT_SLAVE_0, slv_handles);
actual_position = get_position_actual_ticks(ECAT_SLAVE_0,
slv_handles);
actual_torque = get_torque_actual_mNm(ECAT_SLAVE_0, slv_handles);
printf("\r Velocity: %d Position: %d Torque: %f ",
actual_velocity, actual_position, actual_torque);
}
}
else if (break_loop){
break;
}
else {
/* Update the process data (EtherCat packets) sent/received from the node */
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Read actual node sensor values */
actual_velocity
= get_velocity_actual_rpm(ECAT_SLAVE_0, slv_handles);
actual_position = get_position_actual_ticks(ECAT_SLAVE_0,
slv_handles);
actual_torque = get_torque_actual_mNm(ECAT_SLAVE_0, slv_handles);
printf("\r Velocity: %d Position: %d Torque: %f ",
actual_velocity, actual_position, actual_torque);
}
}
printf("\n");
/* Quick stop velocity mode (for emergency) */
quick_stop_velocity(ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
/* Regain control of node to continue after quick stop */
renable_ctrl_quick_stop(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
set_operation_mode(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
enable_operation(ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
/* Shutdown node operations */
shutdown_operation(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
TOTAL_NUM_OF_SLAVES);
/* Just for better printing result */
system("setterm -cursor on");
return 0;
}
int setup_problem(Exo_DB *exo, /* ptr to the finite element mesh database */
Dpi *dpi) /* distributed processing information */
/********************************************************************
*
* setup_problem():
*
* Setup_problem() determines the degrees of freedom at each
* node and formulates the solution vector. It then determines the
* communications pattern for exchanging that solution vector between
* processors.
* Lastly, it sets up structures that help to carry out the
* boundary condition integrals on side sets.
*
* NOTE:
* This function was formed by taking common parts out of
**********************************************************************/
{
int i;
static char *yo = "setup_problem:";
/*
* Initialize nodal based structures pertaining to properties
* and boundary conditions
*/
init_nodes(exo, dpi);
/*
* Determine the side set or node set index that matches all of
* those boundary conditions. Store it in the boundary condition
* structure.
*/
bc_set_index(exo);
/*
* Determine what boundary conditions are on side sets that
* are internal-boundary side sets
*/
bc_internal_boundary(exo);
/*
* Determine the neighboring material indecises on all boundary
* conditions whether or not they be specified in the input deck
*/
bc_matrl_index(exo);
/*
* Determine from the boundary conditions and problem description
* structures what degrees of freedom are shared between materials
* and which degrees of freedom generate multiple degrees of freedom
* at each node
*/
coordinate_discontinuous_variables(exo, dpi);
/*
* Reconcile the boundary conditions with material properties
* entered into the databases.
*/
reconcile_bc_to_matrl();
/*
* Determine the values of the DV_Indexing_Type field for boundary
* conditions
*/
determine_dvi_index();
/*
* Enumerate my own degrees of freedom on this processor.
*/
setup_local_nodal_vars(exo, dpi);
#ifdef DEBUG_HKM
printf("%s: Proc %d after setup_local_nodal_vars\n", yo, ProcID); fflush(stdout);
#endif
/*
* setup communications patterns between ghost and owned
* nodes.
*/
setup_nodal_comm_map(exo, dpi, cx);
#ifdef DEBUG_HKM
printf("%s: Proc %d after setup_nodal_comm_map\n", yo, ProcID); fflush(stdout);
#endif
/*
* Find the global maximum number of unknowns located at any one
* node on any processor
*/
MaxVarPerNode = find_MaxUnknownNode();
#ifdef DEBUG_HKM
printf("%s: Proc %d after find_MaxUnknownNode\n", yo, ProcID); fflush(stdout);
#endif
/*
* Exchange my idea of what materials I have at each node with
* my surrounding processors. Make sure we are all in sync
*/
setup_external_nodal_matrls(exo, dpi, cx);
/*
* Exchange my idea of what degrees of freedom I have with my
* surrounding processors. Make sure we are all in sync.
* Owned nodes tell ghost nodes what variables are active
* at that node.
*/
setup_external_nodal_vars(exo, dpi, cx);
#ifdef DEBUG_HKM
printf("%s: Proc %d after setup_external_nodal_vars\n", yo, ProcID); fflush(stdout);
#endif
/*
* Finish setting the unknown map on this processor
*/
set_unknown_map(exo, dpi);
/*
* Now determine the communications pattern that is necessary to
* exchange the solution vector in as efficient a manner as
* possible
*/
#ifdef DEBUG_HKM
printf("P_%d: setup_dof_comm_map() begins...\n", ProcID); fflush(stdout);
#endif
log_msg("setup_dof_comm_map...");
setup_dof_comm_map(exo, dpi, cx);
/*
* Output some statistics concerning the communications pattern
*/
if (Num_Proc > 1) output_comm_stats(dpi, cx);
#ifdef COUPLED_FILL
/*
* I extracted this from setup_fill_comm_map because some of the
* renormalization routines make use of them.
*/
num_fill_unknowns = count_vardofs(FILL, dpi->num_universe_nodes);
internal_fill_unknowns = count_vardofs(FILL, dpi->num_internal_nodes);
owned_fill_unknowns = count_vardofs(FILL,
(dpi->num_internal_nodes + dpi->num_boundary_nodes));
boundary_fill_unknowns = owned_fill_unknowns - internal_fill_unknowns;
external_fill_unknowns = num_fill_unknowns - owned_fill_unknowns;
#else /* COUPLED_FILL */
/*
* Set up the communications pattern for doing a solution
* of the FILL variable type equations alone, if needed
*/
if (Explicit_Fill) {
#ifdef DEBUG
fprintf(stderr, "P_%d: setup_fill_comm_map() begins...\n", ProcID);
#endif /* DEBUG */
log_msg("setup_fill_comm_map...");
setup_fill_comm_map(exo, dpi, cx);
}
#endif /* COUPLED_FILL */
/*
* Possibly increase the number of variable descriptions to include
* those that are not part of the solution vector
*/
vdesc_augment();
/*
* Setup Boundary condition inter-connectivity
* -> Stefan flow bc's need to know what bc's contain the reaction
* info.
* -> YFLUX bc's need to be connected -> future implementation
*/
set_up_BC_connectivity();
/*
* Set up the structures necessary to carry out
* surface integrals
*/
#ifdef DEBUG
fprintf(stderr, "P_%d set_up_Surf_BC() begins...\n",ProcID);
#endif
log_msg("set_up_Surf_BC...");
set_up_Surf_BC(First_Elem_Side_BC_Array, exo, dpi);
/*
* Set up the Edge boundary condition structures
*/
#ifdef DEBUG
fprintf(stderr, "P_%d: set_up_Edge_BC() begins...\n",ProcID);
#endif
log_msg("set_up_Edge_BC...");
set_up_Edge_BC(First_Elem_Edge_BC_Array, exo, dpi);
/*
* Set up "boundary" conditions on level set surfaces
*/
set_up_Embedded_BC();
/*
* Print out the edge boudary condition structures
* if necessary
*/
if (Debug_Flag) {
print_setup_Surf_BC(First_Elem_Side_BC_Array);
}
/*
* Malloc structures of size Num_Var_Info_Records
*/
ei->VDindex_to_Lvdesc = alloc_int_1(Num_Var_Info_Records, -1);
for (i = 0; i < MAX_ELEMENT_INDICES_RELATED; i++) {
eiRelated[i]->VDindex_to_Lvdesc = alloc_int_1(Num_Var_Info_Records, -1);
}
/*
* Setup the storage for temporary quantities of interest that
* are storred on a "per processor element" basis. These include
* temporary storage of volumetric quadrature information
*/
setup_element_storage();
/*
* Setup some structures for solving problems with shell elements.
*/
init_shell_element_blocks(exo);
/* Communicate non-shared but needed BC information */
exchange_bc_info();
return 0;
}
int main (int argc, char ** argv)
{
pwr_tStatus sts;
qcom_sQid my_q = qcom_cNQid;
qcom_sGet get;
errh_Init("pwr_linksup", errh_eAnix_linksup);
errh_SetStatus( PWR__SRVSTARTUP);
if (!qcom_Init(&sts, NULL, "pwr_linksup")) {
errh_Error("qcom_Init, %m", sts);
errh_SetStatus( PWR__SRVTERM);
exit(sts);
}
sts = gdh_Init("pwr_linksup");
if (EVEN(sts)) {
errh_Fatal("gdh_Init, %m", sts);
errh_SetStatus( PWR__SRVTERM);
exit(sts);
}
if (!qcom_CreateQ(&sts, &my_q, NULL, "events")) {
errh_Fatal("qcom_CreateQ, %m", sts);
errh_SetStatus( PWR__SRVTERM);
exit(sts);
}
if (!qcom_Bind(&sts, &my_q, &qcom_cQini)) {
errh_Fatal("qcom_Bind(Qini), %m", sts);
errh_SetStatus( PWR__SRVTERM);
exit(-1);
}
/* Wait for local nethandler to start */
while (EVEN(gdh_NethandlerRunning()))
sleep(1);
plc_UtlWaitForPlc();
LstIni(&node_l);
LstIni(&timer_l);
init_nodes();
if (!LstEmp(&node_l)) {
list_state = eListState_Scan;
} else {
errh_Info("No nodes to supervise, exiting");
errh_SetStatus( pwr_cNStatus);
exit(0);
}
errh_SetStatus( PWR__SRUN);
for (;;) {
scan_timers();
scan_nodes();
get.data = NULL;
if (qcom_Get(&sts, &my_q, &get, cTimerTimeScan) != NULL) {
if (get.type.b == qcom_eBtype_event) {
event(&get);
} else {
errh_Info("unexpected message type, type: %d", get.type.b);
}
qcom_Free(&sts, get.data);
}
aproc_TimeStamp(((float)cTimerTimeScan)/1000, 5);
}
}
int
main(int argc, char *argv[])
{
struct hostent *hp;
struct in_addr in;
#ifdef _WIN32
WSADATA WsaData;
#endif /* _WIN32 */
GtkWidget *main_window;
#ifdef _WIN32
if (WSAStartup(0x0202, &WsaData)) {
fprintf(stderr, "Could not initialize WinSock.\n");
exit(EXIT_FAILURE);
}
#endif /* _WIN32 */
/* Get IP */
if ((hp = gethostbyname(argc > 1 ? argv[1] : "localhost")) == 0) {
fprintf(stderr, "Not a valid host \"%s\"\n", argv[1]);
exit(EXIT_FAILURE);
}
in.s_addr = ((struct in_addr *)(void *)(hp->h_addr))->s_addr;
printf("Address: %s\n", inet_ntoa(in));
/* fill in the socket structure with host information */
memset(&pin, 0, sizeof(pin));
pin.sin_family = AF_INET;
pin.sin_addr.s_addr = ((struct in_addr *)(void *)(hp->h_addr))->s_addr;
pin.sin_port = htons(IPC_PORT);
gtk_init(&argc, &argv);
init_nodes();
freeze_packets = 1;
display_dec = 1;
/* "Failsafe" values */
ipversion = AF_INET;
ipsize = sizeof(struct in_addr);
main_window = create_main_window();
gtk_widget_show(main_window);
printf("Done building GUI\n");
memset(&main_addr, 0, sizeof(union olsr_ip_addr));
memset(&null_addr, 0, sizeof(union olsr_ip_addr));
/* Terminate signal */
signal(SIGINT, shutdown_);
/* Init node timeout */
nodes_timeout = NEIGHB_HOLD_TIME_NW;
init_timer((olsr_u32_t) (nodes_timeout * 1000), &hold_time_nodes);
ipc_connect(&pin);
add_timeouts();
gtk_main();
return 0;
}
int main()
{
int flag = 0;
int acceleration = 350; // rpm/s
int deceleration = 350; // rpm/s
int velocity = 350; // rpm
int actual_position = 0; // ticks
int target_position = 0; // ticks
int actual_velocity; // rpm
float actual_torque; // mNm
int steps = 0;
int i = 1;
int position_ramp = 0;
int ack;
int home_velocity = 250; // rpm
int home_acceleration = 250; // rpm/s
int sdo_update = 1; // 1- yes / 0 - no
int slave_number = 0;
/* Initialize Ethercat Master */
init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize torque parameters */
initialize_torque(slave_number, slv_handles);
/* Initialize all connected nodes with Mandatory Motor Configurations (specified under config/motor/)*/
init_nodes(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize the node specified with slave_number with Homing configurations (specified under config/motor/)*/
set_operation_mode(HM, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Enable operation of node in Homing mode */
enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
start_homing(&master_setup, slv_handles, home_velocity, home_acceleration, slave_number, TOTAL_NUM_OF_SLAVES);
/* Shutdown Homing Operation */
shutdown_operation(HM, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/*
* Acquire actual position from the node a few times after homing and
* set it as target position. (wait for controller to settle)
*/
i = 0;
int difference = 1500;
int previous=0;
while(1)
{
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
if(master_setup.op_flag) //Check if the master is active
{
actual_position = get_position_actual_ticks(slave_number, slv_handles);
target_position = actual_position;
set_profile_position_ticks(target_position, slave_number, slv_handles);
i = i+1;
difference = actual_position - previous;
if(difference <1 && difference >-1)
{
//printf(" difference %d act %d\n",difference, actual_position);
break;
}
previous = actual_position;
//printf(" difference %d act %d\n",difference, actual_position);
}
}
/* Now initialize the node specified with slave_number with CSP configurations (specified under config/motor/)*/
set_operation_mode(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Enable operation of node in CSP mode */
enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Initialize position profile parameters */
initialize_position_profile_limits(slave_number, slv_handles);//*/
/* Now setting a new target position after homing */
target_position = get_position_actual_ticks(slave_number, slv_handles) + 5000;
if(target_position > 35000)
target_position = 35000;
printf(" target_position %d\n", target_position);
/*calculate the no. of steps for the profile*/
steps = init_position_profile_params(target_position, actual_position, velocity, acceleration, \
deceleration, slave_number, slv_handles);
/* Execute the position profile steps in a loop */
i = 1;
while(1)
{
/* Update the process data (EtherCat packets) sent/received from the node */
pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
if(master_setup.op_flag) /*Check if the master is active*/
{
if(i<steps)
{
/* Generate target position steps */
position_ramp = generate_profile_position(i, slave_number, slv_handles);
/* Send target position for the node specified by slave_number */
set_position_ticks(position_ramp, slave_number, slv_handles);
i = i+1;
}
if(i >= steps)
{
printf("ack received");
break;
}
//printf("actual position %d \n", get_position_actual_ticks(slave_number, slv_handles));
/* Read actual node sensor values */
actual_position = get_position_actual_ticks(slave_number, slv_handles);
actual_velocity = get_velocity_actual_rpm(slave_number, slv_handles);
actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
printf("actual position %d actual velocity %d actual_torque %f\n", actual_position, actual_velocity, actual_torque);
}
}
/* Quick stop position mode (for emergency) */
quick_stop_position(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
/* Regain control of node to continue after quick stop */
renable_ctrl_quick_stop(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
set_operation_mode(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES); // set operation mode to CSP
enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES); // enable operation
/* Shutdown node operations */
shutdown_operation(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES); // stop the node operation. or can continue with new position profile
return 0;
}
