dxl-goto.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 void 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. Setting joint mode.", iID);
00049                                 gDynamixels[gNumDynamixels].setEndlessTurnMode(false);
00050                         }
00051                         else
00052                         {
00053                                 printf(".");
00054                                 fflush(stdout);
00055                         }
00056                 }
00057                 gMotorsInitialized=true;
00058                 printf("\nDetection done. Found %d Dynamixels. %s\n", gNumDynamixels, gNumDynamixels>0?"Now reporting:\n":"Quitting.");
00059         }
00060 }
00061 
00062 void dxl_task_off_proc(void *arg)
00063 {
00064         for (int iDx=0; iDx<gNumDynamixels; iDx++)
00065         {
00066                 gDynamixels[iDx].enableTorque(DXL_OFF);
00067         }
00068 }
00069 
00070 void catch_signal(int sig)
00071 {
00072         printf("Break signal received.\n");
00073         gQuit = true;
00074 }
00075 
00076 int main(int argc, char** argv)
00077 {
00078         if (argc < 6)
00079         {
00080                 printf("Usage: dxl-goto [serial device] [baud rate] [ID] [pos] [speed]\n");
00081                 return -1;
00082         }
00083 
00084         // Set serial device name
00085         if (strlen(argv[1]) < MAX_DEVICE_NAME_LEN)
00086                 strcpy(devicename, argv[1]);
00087         else
00088         {
00089                 printf("[ERROR] Device name too long (probably not /dev/ttyUSB0 ..)!\n");
00090                 return -1;
00091         }
00092 
00093         // Open serial port
00094         if (!serialPort.port_open(devicename, LxSerial::RS485_FTDI))
00095         {
00096                 printf("[ERROR] Failed to open serial port!\n");
00097                 return -1;
00098         }
00099 
00100         // Set correct baud rate
00101         int baudrate = atoi(argv[2]);
00102         serialPort.set_speed_int(baudrate);
00103 
00104         int ret=0;
00105 
00106         // Find dynamixel and configure and init
00107         int ID = atoi(argv[3]);
00108         CDxlConfig dxlConfig;
00109         gDynamixels[0].setSerialPort(&serialPort);
00110         gDynamixels[0].setConfig(dxlConfig.setID(ID));
00111         if (gDynamixels[0].init(false) == DXL_SUCCESS)  // false means: do not send (the default) config to motor
00112         {
00113                 // Dynamixel with ID = iID responded!
00114                 gNumDynamixels++;
00115                 printf("\n * Dynamixel found with ID %d. Setting joint mode.\n", ID);
00116                 gDynamixels[0].setEndlessTurnMode(false);
00117         }
00118         else
00119         {
00120                 printf("\n Dynamixel not found. Quitting.\n");
00121                 fflush(stdout);
00122                 return -1;
00123         }
00124 
00125         // Goal position
00126         int pos = atoi(argv[4]);
00127         if ((pos < 0) || (pos > 1023))
00128         {
00129                 printf("\n Error: position must be between 0 and 1023. Quitting.\n");
00130                 return -1;
00131         }
00132         // Speed
00133         int speed = atoi(argv[5]);
00134         if ((speed < 0) || (speed > 1023))
00135         {
00136                 printf("\n Error: speed must be between 0 and 1023. Quitting.\n");
00137                 return -1;
00138         }
00139 
00140         printf("\n Press Ctrl-C to terminate\n");
00141         // This job can be canceled using Ctrl-C
00142         signal(SIGTERM, catch_signal);
00143         signal(SIGINT, catch_signal);
00144 
00145         //gDynamixels[0].setRawPos(pos, speed);
00146 
00147         gDynamixels[0].setRawPos(500, 100);
00148         gDynamixels[0].setCompliance(0, 32);
00149         usleep(1000000);
00150         int stepper=0;
00151         double samplefreq = 75.0;
00152         double sinefreq = 1.0;
00153         double amplitude = 8.0;
00154         while (!gQuit)
00155         {
00156                 /*
00157                 int sleeptime=500000;
00158                 printf("Pos 400\n");
00159                 gDynamixels[0].setRawPos(300, 0);
00160                 gDynamixels[0].setRawPos(300, speed);
00161                 usleep(sleeptime);
00162                 printf("Pos 600\n");
00163                 gDynamixels[0].setRawPos(700, 0);
00164                 gDynamixels[0].setRawPos(700, speed);
00165                 usleep(sleeptime);
00166                 */
00167                 double speed = amplitude*sin((sinefreq*stepper/samplefreq)*6.28);
00168                 double posdiff = speed*(1.0/samplefreq);
00169                 printf("posdiff: %.4f(rad) (%d)\n", posdiff, (int)(posdiff/DXL_STEPS_TO_RAD));
00170                 gDynamixels[0].getPos();
00171                 gDynamixels[0].setPos(gDynamixels[0].presentPos() + posdiff, -1);
00172                 //gDynamixels[0].setSpeed(speed);
00173                 usleep((1.0/samplefreq)*1E6);
00174                 stepper++;
00175         }
00176         gDynamixels[0].setRawPos(500, speed);
00177         //gDynamixels[0].setCompliance(0, 32);
00178 
00179         // ===== PREMATURE ENDING ====
00180         printf("Quitting while keeping motor operational...\n"); serialPort.port_close(); return 0;
00181         // ===========================
00182 
00183         while (!gQuit)
00184                 pause();
00185 
00186         // Always turn off at the end
00187         dxl_task_off_proc(NULL);
00188 
00189         // Clean up
00190         serialPort.port_close();
00191         printf("End of dxl-all.\n");
00192         return 0;
00193 }


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