int epos_Receive_PDO_n_Mapping(uint16_t node_id, uint8_t n, uint8_t num_objects, Epos_pdo_mapping* objects) {
int err = 0;
// Set number of objects to zero
SDO_data d;
d.nodeid = node_id;
d.index = 0x1600 + n-1;
d.subindex = 0x00;
d.data.size = 1;
d.data.data = 0;
err = SDO_write(motor_cfg_fd, &d);
if (err != 0) {
return err;
}
// Write objects
d.data.size = 4;
for(int i=0; i<num_objects; i++) {
Epos_pdo_mapping obj = objects[i];
d.subindex = i+1;
d.data.data = ((uint32_t)obj.index << 16) | (obj.subindex<<8) | (obj.length);
err = SDO_write(motor_cfg_fd, &d);
if (err != 0) {
return err;
}
}
// Set Correct number of objects
d.subindex = 0x00;
d.data.size = 1;
d.data.data = num_objects;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Transmit_PDO_n_Parameter(uint16_t node_id, uint8_t n, uint32_t cob) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x1800 + n-1;
d.subindex = 0x01;
d.data.size = 4;
d.data.data = cob;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Max_Acceleration(uint16_t node_id, uint32_t value) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x60C5;
d.subindex = 0x00;
d.data.size = 4;
d.data.data = value;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Motion_Profile_Type(uint16_t node_id, enum Epos_Profile_Type mode) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x6086;
d.subindex = 0x00;
d.data.size = 2;
d.data.data = mode;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Profile_Velocity(uint16_t node_id, uint32_t value) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x6081;
d.subindex = 0x00;
d.data.size = 4;
d.data.data = value;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Target_Position(uint16_t node_id, int32_t enc) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x607A;
d.subindex = 0x00;
d.data.size = 4;
d.data.data = enc;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Position_Window(uint16_t node_id, uint32_t enc) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x6067;
d.subindex = 0x00;
d.data.size = 4;
d.data.data = enc;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Position_Window_Time(uint16_t node_id, uint32_t ms) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x6068;
d.subindex = 0x00;
d.data.size = 4;
d.data.data = ms;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Modes_of_Operation(uint16_t node_id, enum Epos_mode mode) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x6060;
d.subindex = 0x00;
d.data.size = 1;
d.data.data = mode;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Position_Mode_Setting_Value(uint16_t node_id, int32_t value) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x2062;
d.subindex = 0x00;
d.data.size= 4;
d.data.data = value;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Controlword(uint16_t node_id, enum Epos_ctrl ctrl) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x6040;
d.subindex = 0x00;
d.data.size = 2;
d.data.data = ctrl;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Miscellaneous_Configuration(uint16_t node_id, uint16_t value) {
SDO_data d;
d.nodeid = node_id;
d.index = 0x2008;
d.subindex = 0x00;
d.data.size = 2;
d.data.data = value;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Software_Position_Limit(uint16_t node_id, int32_t min, int32_t max) {
int err = 0;
SDO_data d;
d.nodeid = node_id;
d.index = 0x607D;
d.subindex = 0x01;
d.data.size = 4;
d.data.data = min;
err = SDO_write(motor_cfg_fd, &d);
if (err != 0) {
return err;
}
d.subindex = 0x02;
d.data.data = max;
return SDO_write(motor_cfg_fd, &d);
}
int epos_Modes_of_Operation_Display(uint16_t node_id, enum Epos_mode mode) {
int result;
SDO_data d;
d.nodeid = node_id;
d.index = 0x6061;
d.subindex = 0x00;
d.data.size = 1;
d.data.data = mode;
result = SDO_write(motor_cfg_fd, &d);
switch (d.data.data) {
case 7:
printd(LOG_ERROR, "Interpolated_Position_Mode\n");
break;
case 6:
printd(LOG_ERROR, "Homing_Mode\n");
break;
case 3:
printd(LOG_ERROR, "Profile_Velocity_Mode\n");
break;
case 1:
printd(LOG_ERROR, "Profile_Position_Mode\n");
break;
case -1:
printd(LOG_ERROR, "Position_Mode\n");
break;
case -2:
printd(LOG_ERROR, "Velocity_Mode\n");
break;
case -3:
printd(LOG_ERROR, "Current_Mode\n");
break;
case -4:
printd(LOG_ERROR, "Diagnostic_Mode\n");
break;
case -5:
printd(LOG_ERROR, "Master_Encoder_Mode\n");
break;
case -6:
printd(LOG_ERROR, "Step_Direction_Mode\n");
break;
default:
printd(LOG_ERROR, "Unknown Mode\n");
return 1;
}
return 0;
}
int main(int argc, char** argv)
{
extern char *optarg;
char command[200];
char* res;
int ret=0;
int sysret=0;
int i=0;
if (sem_init(&Write_sem, 0, 0) == -1)
handle_error("Writesem_init");
if (sem_init(&Read_sem, 0, 0) == -1)
handle_error("Readsem_init");
/* Defaults */
strcpy(LibraryPath,"/usr/lib/libcanfestival_can_peak_linux.so");
strcpy(BoardBusName,"0");
strcpy(BoardBaudRate,"1M");
/* Init stack timer */
TimerInit();
if (argc > 1){
printf("ok\n");
/* Strip command-line*/
for(i=1 ; i<argc ; i++)
{
if(ProcessCommand(argv[i]) == INIT_ERR) goto init_fail;
}
}
NodeInit(0,1);
RegisterSetODentryCallBack(CANOpenShellOD_Data, 0x2003, 0, &OnStatus3Update);
help_menu();
CurrentNode = 3;
sleep(1);
//setState(CANOpenShellOD_Data, Operational); // Put the master in operational mode
stopSYNC(CANOpenShellOD_Data);
/* Enter in a loop to read stdin command until "quit" is called */
while(ret != QUIT)
{
// wait on stdin for string command
rl_on_new_line ();
res = rl_gets();
//sysret = system(CLEARSCREEN);
if(res[0]=='.'){
ret = ProcessCommand(res+1);
}
else if(res[0]==','){
ret = ProcessFocusedCommand(res+1);
}
else if (res[0]=='\n'){
}
else {
EnterMutex();
SDO_write(CANOpenShellOD_Data,CurrentNode,0x1023,0x01,strlen(res),visible_string, res, 0);
EnterMutex();
SDO_read(CANOpenShellOD_Data,CurrentNode,0x1023,0x03,visible_string,0);
printf("%s\n",SDO_read_data);
}
fflush(stdout);
usleep(500000);
}
printf("Finishing.\n");
// Stop timer thread
StopTimerLoop(&Exit);
/* Close CAN board */
canClose(CANOpenShellOD_Data);
init_fail:
TimerCleanup();
return 0;
}
int ProcessCommand(char* command)
{
int ret = 0;
int sec = 0;
int NodeID;
int NodeType;
UNS32 data = 0;
char buf[50];
EnterMutex();
switch(cst_str4(command[0], command[1], command[2], command[3]))
{
case cst_str4('h', 'e', 'l', 'p') : /* Display Help*/
help_menu();
break;
case cst_str4('c', 'l', 'e', 'a') : /* Display Help*/
system(CLEARSCREEN);
break;
case cst_str4('s', 's', 't', 'a') : /* Slave Start*/
StartNode(ExtractNodeId(command + 5));
break;
case cst_str4('s', 's', 't', 'o') : /* Slave Stop */
StopNode(ExtractNodeId(command + 5));
break;
case cst_str4('s', 'r', 's', 't') : /* Slave Reset */
ResetNode(ExtractNodeId(command + 5));
break;
case cst_str4('i', 'n', 'f', 'o') : /* Retrieve node informations */
GetSlaveNodeInfo(ExtractNodeId(command + 5));
break;
case cst_str4('r', 's', 'd', 'o') : /* Read device entry */
ReadDeviceEntry(command);
break;
case cst_str4('w', 's', 'd', 'o') : /* Write device entry */
WriteDeviceEntry(command);
break;
case cst_str4('n', 'o', 'd', 'e') : /* Write device entry */
ret = sscanf(command, "node %2x", &NodeID);
data = 0;
SDO_write(CANOpenShellOD_Data,NodeID,0x1024,0x00,1, 0, &data, 0);
CurrentNode = NodeID;
break;
case cst_str4('c', 'm', 'd', ' ') : /* Write device entry */
ret = sscanf(command, "cmd %2x,%s", &NodeID, buf );
SDO_write(CANOpenShellOD_Data,NodeID,0x1023,0x01,strlen(buf),visible_string, buf, 0);
SDO_read(CANOpenShellOD_Data,NodeID,0x1023,0x03,visible_string,0);
return 0;
break;
case cst_str4('s', 'y', 'n', '0') : /* Display master node state */
stopSYNC(CANOpenShellOD_Data);
break;
case cst_str4('s', 'y', 'n', '1') : /* Display master node state */
startSYNC(CANOpenShellOD_Data);
break;
case cst_str4('s', 't', 'a', 't') : /* Display master node state */
printf("Status3: %x\n",Status3);
Status3 = 0;
break;
case cst_str4('s', 'c', 'a', 'n') : /* Display master node state */
DiscoverNodes();
break;
case cst_str4('w', 'a', 'i', 't') : /* Display master node state */
ret = sscanf(command, "wait#%d", &sec);
if(ret == 1) {
LeaveMutex();
SleepFunction(sec);
return 0;
}
break;
case cst_str4('g', 'o', 'o', 'o') : /* Quit application */
setState(CANOpenShellOD_Data, Operational);
break;
case cst_str4('q', 'u', 'i', 't') : /* Quit application */
LeaveMutex();
return QUIT;
case cst_str4('l', 'o', 'a', 'd') : /* Library Interface*/
ret = sscanf(command, "load#%100[^,],%30[^,],%4[^,],%d,%d",
LibraryPath,
BoardBusName,
BoardBaudRate,
&NodeID,
&NodeType);
if(ret == 5)
{
LeaveMutex();
ret = NodeInit(NodeID, NodeType);
return ret;
}
else
{
printf("Invalid load parameters\n");
}
break;
default :
help_menu();
}
LeaveMutex();
return 0;
}