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


threemxl
Author(s):
autogenerated on Fri Aug 28 2015 13:21:08