dxl-all.cpp
Go to the documentation of this file.
00001 /*
00002  * dnx-all.cpp - FTDI serial communication version
00003  *
00004  *  Created on: Nov 17, 2010
00005  *      Author: Erik Schuitema
00006  */
00007 
00008 #include <sys/mman.h>
00009 #include <signal.h>
00010 #include <threemxl/platform/hardware/dynamixel/dynamixel/Dynamixel.h>
00011 
00012 #include <stdio.h>
00013 #include <stdlib.h>
00014 #include <string.h>
00015 
00016 LxSerial serialPort;
00017 
00018 #define MAX_DEVICE_NAME_LEN 20
00019 char devicename[MAX_DEVICE_NAME_LEN];
00020 
00021 int gNumDynamixels      = 0;
00022 int gDxlMinID           = 0;
00023 int gDxlMaxID           = DXL_BROADCAST_ID-1;
00024 //int gDynamixelIDs[gNumDynamixels]     = {100, 101, 102, 103, 104, 105}; // hard-coded IDs; legs only
00025 CDynamixel gDynamixels[MAX_NUM_DYNAMIXELS];
00026 
00027 bool gDxlTaskProcDone=false;
00028 bool gMotorsInitialized=false;
00029 
00030 bool gQuit=false;
00031 
00032 bool dxl_init_all_motors()
00033 {
00034         if (!gMotorsInitialized)
00035         {
00036                 printf("Detecting dynamixels in the ID range %d-%d ", gDxlMinID, gDxlMaxID);
00037                 gNumDynamixels=0;
00038                 // Find all dynamixels and configure and init them
00039                 CDxlConfig dxlConfig;
00040                 for (int iID=gDxlMinID; iID<=gDxlMaxID; iID++)
00041                 {
00042                         gDynamixels[gNumDynamixels].setSerialPort(&serialPort);
00043                         gDynamixels[gNumDynamixels].setConfig(dxlConfig.setID(iID));
00044                         if (gDynamixels[gNumDynamixels].init(false) == DXL_SUCCESS)     // false means: do not send (the default) config to motor
00045                         {
00046                                 // Dynamixel with ID = iID responded!
00047                                 gNumDynamixels++;
00048                                 printf("\n * Dynamixel found with ID %d ", iID);
00049                         }
00050                         else
00051                         {
00052                                 printf(".");
00053                                 fflush(stdout);
00054                         }
00055                 }
00056                 gMotorsInitialized=true;
00057                 printf("\nDetection done. Found %d Dynamixels. %s\n", gNumDynamixels, gNumDynamixels>0?"":"Quitting.");
00058         }
00059         return gNumDynamixels > 0;
00060 }
00061 
00062 void dxl_task_on_proc(void *arg)
00063 {
00064         if (dxl_init_all_motors())
00065         {
00066                 for (int iDx=0; iDx<gNumDynamixels; iDx++)
00067                 {
00068                         gDynamixels[iDx].enableTorque(DXL_ON);
00069                 }
00070         }
00071         // Do not automatically terminate; do not use gDxlTaskProcdone
00072 }
00073 
00074 void dxl_task_off_proc(void *arg)
00075 {
00076         if (dxl_init_all_motors())
00077         {
00078                 for (int iDx=0; iDx<gNumDynamixels; iDx++)
00079                 {
00080                         gDynamixels[iDx].enableTorque(DXL_OFF);
00081                 }
00082         }
00083         gDxlTaskProcDone = true;
00084 }
00085 
00086 void dxl_task_volt_proc(void *arg)
00087 {
00088         if (dxl_init_all_motors())
00089         {
00090                 printf("Putting all motors in voltage control mode (at 0 volt).\n");
00091                 for (int iDx=0; iDx<gNumDynamixels; iDx++)
00092                 {
00093                         gDynamixels[iDx].setEndlessTurnMode(true);
00094                         gDynamixels[iDx].enableTorque(DXL_OFF); // This turns off the torque after setting volt mode
00095                 }
00096         }
00097         gDxlTaskProcDone = true;
00098 }
00099 
00100 void dxl_task_pos_proc(void *arg)
00101 {
00102         if (dxl_init_all_motors())
00103         {
00104                 printf("Putting all motors in position control mode.\n");
00105                 for (int iDx=0; iDx<gNumDynamixels; iDx++)
00106                 {
00107                         gDynamixels[iDx].enableTorque(DXL_OFF); // This ensures that enabling position mode doesn't make the motor go to the last set position with maximum speed.
00108                         gDynamixels[iDx].setEndlessTurnMode(false);
00109                         gDynamixels[iDx].enableTorque(DXL_OFF); // This turns off the torque again after setting position mode
00110                 }
00111         }
00112         gDxlTaskProcDone = true;
00113 }
00114 
00115 void dxl_task_ping_proc(void *arg)
00116 {
00117         if (dxl_init_all_motors())
00118         {
00119                 for (int iDx=0; iDx<gNumDynamixels; iDx++)
00120                 {
00121                         int error = gDynamixels[iDx].ping();
00122                         if (error != DXL_SUCCESS)
00123                                 printf("Pinging dynamixel with ID=%d returned an error: %d\n", gDynamixels[iDx].getID(), error);
00124                         else
00125                                 printf("Dynamixel (ID=%d) responded to PING!\n", gDynamixels[iDx].getID());
00126                 }
00127         }
00128         gDxlTaskProcDone = true;
00129 }
00130 
00131 void dxl_task_report_proc(void *arg)
00132 {
00133         if (dxl_init_all_motors())
00134         {
00135                 printf("Now reporting:\n");
00136                 for (int iDx=0; iDx<gNumDynamixels; iDx++)
00137                 {
00138                         printf("Report of Dynamixel with ID=%d:\n", gDynamixels[iDx].getID());
00139                         int error = gDynamixels[iDx].printReport(stdout);
00140                         if (error != DXL_SUCCESS)
00141                                 printf("Reporting dynamixel with ID=%d returned an error: %d\n", gDynamixels[iDx].getID(), error);
00142                         printf("\n");
00143                 }
00144         }
00145         gDxlTaskProcDone = true;
00146 }
00147 
00148 
00149 void catch_signal(int sig)
00150 {
00151         printf("Break signal received.\n");
00152         gQuit = true;
00153 }
00154 
00155 int main(int argc, char** argv)
00156 {
00157         if (argc < 6)
00158         {
00159                 printf("Usage: dxl-all [serial device] [baud rate] [min-dynamixel-ID] [max-dynamixel-ID] [report|ping|on|off|volt|pos]\n");
00160                 printf(" * report\t// shows information on all motors\n * on\t\t// enables the torque of all motors\n * off\t\t// disables the torque of all motors\n * volt\t\t// puts all motors in voltage control mode (at 0 volt)\n * pos\t\t// puts all motors in position control mode\n * ping\t\t// pings all motors\n");
00161                 return -1;
00162         }
00163 
00164         // Set serial device name
00165         if (strlen(argv[1]) < MAX_DEVICE_NAME_LEN)
00166                 strcpy(devicename, argv[1]);
00167         else
00168         {
00169                 printf("[ERROR] Device name too long (probably not /dev/ttyUSB0 ..)!\n");
00170                 return -1;
00171         }
00172 
00173         // Open serial port
00174         if (!serialPort.port_open(devicename, LxSerial::RS485_FTDI))
00175         {
00176                 printf("[ERROR] Failed to open serial port!\n");
00177                 return -1;
00178         }
00179 
00180         // Set correct baud rate
00181         int baudrate = atoi(argv[2]);
00182         serialPort.set_speed_int(baudrate);
00183         // Set correct baud rate
00184 //      serialPort.set_speed(LxSerial::S500000);
00185 
00186         // Create dynamixel task
00187                 // nothing to do here
00188         int ret=0;
00189 
00190         // Detect minimum ID
00191         gDxlMinID = atoi(argv[3]);
00192 
00193         // Detect maximum ID
00194         gDxlMaxID = atoi(argv[4]);
00195 
00196         // Detect task type and start the appropriate task
00197         if (strcmp(argv[5], "ping") == 0)
00198         {
00199                 dxl_task_ping_proc(NULL);
00200 
00201         }
00202         else if ((strcmp(argv[5], "on") == 0) || (strcmp(argv[5], "off") == 0)) // if 'on' or 'off'
00203         {
00204                 if (strcmp(argv[5], "on") == 0)
00205                 {
00206                         printf("Press Ctrl-C to terminate\n");
00207                         // This job can be canceled using Ctrl-C
00208                         signal(SIGTERM, catch_signal);
00209                         signal(SIGINT, catch_signal);
00210 
00211                         dxl_task_on_proc(NULL);
00212 
00213                         while (!gQuit)
00214                                 pause();
00215                 }
00216                 // Always turn off at the end
00217                 dxl_task_off_proc(NULL);
00218         }
00219         else if (strcmp(argv[5], "report") == 0)
00220         {
00221                 dxl_task_report_proc(NULL);
00222         }
00223         else if (strcmp(argv[5], "volt") == 0)
00224         {
00225                 dxl_task_volt_proc(NULL);
00226         }
00227         else if (strcmp(argv[5], "pos") == 0)
00228         {
00229                 dxl_task_pos_proc(NULL);
00230         }
00231 
00232         if (ret)
00233         {
00234                 printf("[ERROR] Failed to start dynamixel_test_task, code %d\n",ret);
00235         }
00236 
00237         // Now wait for the main proc to finish
00238         while (!gDxlTaskProcDone)
00239                 usleep((__useconds_t)1E5);      // 100ms
00240 
00241         // Clean up
00242         serialPort.port_close();
00243         printf("End of dxl-all.\n");
00244         return 0;
00245 }


threemxl
Author(s):
autogenerated on Thu Jun 6 2019 21:10:52