void Read(int id, int addr, int len)
{
int result = COMM_TXFAIL, error = 0;
int ivalue = 0;
long lvalue = 0;
if(len == 1)
result = DXL.ReadByte(id, addr, &ivalue, &error);
else if(len == 2)
result = DXL.ReadWord(id, addr, &ivalue, &error);
else if(len == 4)
result = DXL.ReadDWord(id, addr, &lvalue, &error);
if(result != COMM_RXSUCCESS)
{
fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
return;
}
if(error != 0)
PrintErrorCode(error);
if(len == 1)
fprintf(stderr, "\n READ VALUE : %d \n\n", ivalue);
else if(len == 2)
fprintf(stderr, "\n READ VALUE : (UNSIGNED) %u , (SIGNED) %d \n\n", ivalue, ivalue);
else
fprintf(stderr, "\n READ VALUE : (UNSIGNED) %lu , (SIGNED) %d \n\n", lvalue, lvalue);
}
void xlMotor::jointMove(int position, int speed)
{
Dxl.jointMode(myID);
delayMicroseconds(100);
Dxl.setPosition(myID,position,speed);
delayMicroseconds(1);
}
void xlMotor::jointMove(int position)
{
Dxl.jointMode(myID);
delayMicroseconds(1);
Dxl.setPosition(myID,position,512);
delayMicroseconds(1);
}
void xlMotor::wheelMove(char direction,int speed)
{
if (direction=='F') {
Dxl.goalSpeed(myID, speed);
}
else if(direction=='R')
{
Dxl.goalSpeed(myID, speed| 0x400);
}
}
void xlMotor::wheelMove(char direction)
{
Dxl.wheelMode(myID);
if (direction=='F')
{
Dxl.goalSpeed(myID, 512);
}
else if(direction=='R')
{
Dxl.goalSpeed(myID, 512| 0x400);
}
else if(direction=='S')
{
Dxl.goalSpeed(myID, 0);
}
}
void xlMotor::resetSettings(void){
xlMotor::setBaud(3);
xlMotor::setReturnDelay(250);
Dxl.jointMode(myID);
xlMotor::setTempLimit(70);
xlMotor::setStatusReturn(2);
}
int main()
{
Dynamixel test;
double feed;
if(!test.Initialize("/dev/ttyUSB0",false,10.0))
{
return -1;
}
//do a sine wave
double t=0;
double val;
while(1)
{
double top=(564-512)/512.0*100.0;
double bot=(440-512)/512.0*100.0;
//val=(top-bot)/2.0*sin(t/(2*M_PI))+(top+bot)/2.0;
//test.SetServo(1,val);
//feed=test.GetServo(1);
//printf("%lf %lf\n",val,feed);
//t+=0.05;
for(double val=bot;val<=top;val+=1.0)
{
printf("%lf\n",val);
test.SetServo(1,val);
CxUtils::SleepMs(50);
}
for(double val=top;val>=bot;val-=1.0)
{
printf("%lf\n",val);
test.SetServo(1,val);
CxUtils::SleepMs(50);
}
}
return 0;
}
int main(int argc,char** argv)
{
ros::init(argc, argv, "c8_dynamixel");
Dynamixel motors;
float counter = -180;
ros::Rate loop_rate(100);
while(ros::ok())
{
if(counter < 180)
{
motors.moveMotor(counter*3.14/180);
counter++;
}else{
counter = -180;
}
loop_rate.sleep();
}
}
void Refresh(int id, int addr, int len)
{
int result = COMM_TXFAIL, error = 0;
int ivalue = 0;
long lvalue = 0;
fprintf(stderr, "\n [ESC] : Quit monitoring \n\n");
while(1)
{
if(len == 1)
result = DXL.ReadByte(id, addr, &ivalue, &error);
else if(len == 2)
result = DXL.ReadWord(id, addr, &ivalue, &error);
else if(len == 4)
result = DXL.ReadDWord(id, addr, &lvalue, &error);
if(result != COMM_RXSUCCESS)
{
//fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
continue;
}
if(error != 0)
PrintErrorCode(error);
if(len == 1)
fprintf(stderr, " READ VALUE : %.3d [0x%.2X] \r", ivalue, ivalue);
else if(len == 2)
fprintf(stderr, " READ VALUE : %.5d [0x%.2X 0x%.2X] \r", ivalue, DXL_LOBYTE(ivalue), DXL_HIBYTE(ivalue));
else
fprintf(stderr, " READ VALUE : %.10ld [0x%.2X 0x%.2X 0x%.2X 0x%.2X] \r",
lvalue, DXL_LOBYTE(DXL_LOWORD(lvalue)), DXL_HIBYTE(DXL_LOWORD(lvalue)), DXL_LOBYTE(DXL_HIWORD(lvalue)), DXL_HIBYTE(DXL_HIWORD(lvalue)));
if(kbhit())
{
char c = _getch();
if(c == 0x1b)
break;
}
}
fprintf(stderr, "\n\n");
}
int Instruction::moveWithSpeed(std::vector<instruction_set> instruction_list){
SerialPortF comPort;
Dynamixel communication;
hr = comPort.Open(L"COM1", baudRate);
if(FAILED(hr))
{
printf("Cannot open COM1 port\n");
return -1;
}else{
//printf("Starting communication\n");
}
communication.SendTossModeCommand(&comPort);
for(int indx = 0; indx < instruction_list.size(); indx++){
communication.SetDynamixelMovingSpeed(&comPort, instruction_list[indx].id_motor, instruction_list[indx].value, instruction_list[indx].speed);
}
comPort.Close();
return 0;
}
void Broadcast()
{
unsigned char param[Amount*(1+4)];
for(int i = 0; i < Amount; i++)
{
DXL.WriteByte(Id[i], P_TORQUE_ENABLE, 1, 0);
}
int goal=0;
int GoalPos;
while(1)
{
printf( "Press any key to continue!(press ESC to quit)\n" );
if(_getch() == 0x1b)
break;
do
{
// Make syncwrite packet
for(int i = 0; i < Amount; i++)
{
printf("for id=%d,input goalpos\n",Id[i]);
scanf("%d",&goal);
GoalPos = (int)(goal);
param[i*(1+4)+0] = (unsigned char)Id[i];
param[i*(1+4)+1] = DXL_LOBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+2] = DXL_HIBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+3] = DXL_LOBYTE(DXL_HIWORD(GoalPos));
param[i*(1+4)+4] = DXL_HIBYTE(DXL_HIWORD(GoalPos));
}
printf("%d\n", GoalPos);
int result = DXL.SyncWrite(P_GOAL_POSITION_LL, 4, param, Amount*(1+4));
if( result != COMM_RXSUCCESS )
{
printf("fail to open serial port!\n");//PrintCommStatus(result);
break;
}
usleep(CONTROL_PERIOD*1000);
}while(1);
}
}
void Write(int id, int addr, long value, int len)
{
int result = COMM_TXFAIL, error = 0;
if(len == 1)
result = DXL.WriteByte(id, addr, (int)value, &error);
else if(len == 2)
result = DXL.WriteWord(id, addr, (int)value, &error);
else if(len == 4)
result = DXL.WriteDWord(id, addr, value, &error);
if(result != COMM_RXSUCCESS)
{
fprintf(stderr, "\n Fail to write! \n\n");
return;
}
if(error != 0)
PrintErrorCode(error);
fprintf(stderr, "\n Success to write! \n\n");
}
void BeltTest()
{
int result;
long lvalue1 , lvalue2;
while(true)
{
if(kbhit())
break;
result = DXL.ReadDWord(17, 611, &lvalue1, 0);
result = DXL.ReadDWord(18, 611, &lvalue2, 0);
fprintf(stderr, "\n READ VALUE : %d , %d \n\n", lvalue1, lvalue2);
}
}
int main()
{
int i;
Dynamixel DXL("/dev/ttyUSB0");
//BulkRead bulkread(&DXL);
// Open device
if( DXL.Connect() == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
//Close port of USB2DXL
DXL.Disconnect();
return 0;
}
void BroadcastPing()
{
std::vector<PingInfo> vec_info = std::vector<PingInfo>();
DXL.BroadcastPing(vec_info);
if(vec_info.size() > 0)
{
Amount=vec_info.size();
fprintf(stderr, "\n");
for(int i = 0; i < (int)vec_info.size(); i++)
{
fprintf(stderr, " ... SUCCESS \r");
fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n",
vec_info.at(i).ID, vec_info.at(i).ModelNumber, DXL_LOBYTE(vec_info.at(i).ModelNumber), DXL_HIBYTE(vec_info.at(i).ModelNumber));
Id[i]=vec_info.at(i).ID;
}
fprintf(stderr, "\n");
}
}
void Dump(int id, int addr, int len)
{
int result = COMM_TXFAIL, error = 0;
unsigned char* data = (unsigned char*)calloc(len, sizeof(unsigned char));
result = DXL.Read(id, addr, len, data, &error);
if(result != COMM_RXSUCCESS)
{
fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
return;
}
if(error != 0)
PrintErrorCode(error);
if(id != BROADCAST_ID)
{
fprintf(stderr, "\n");
for(int i = addr; i < addr+len; i++)
fprintf(stderr, "ADDR %.3d [0x%.4X] : %.3d [0x%.2X] \n", i, i, data[i-addr], data[i-addr]);
fprintf(stderr, "\n");
}
}
void Scan()
{
fprintf(stderr, "\n");
PingInfo *data = new PingInfo();
for(int i = 1; i < 253; i++)
{
if(DXL.Ping(i, data, 0) == COMM_RXSUCCESS)
{
fprintf(stderr, "\n ... SUCCESS \r");
fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n",
data->ID, data->ModelNumber, DXL_LOBYTE(data->ModelNumber), DXL_HIBYTE(data->ModelNumber));
}
else
fprintf(stderr, ".");
if(kbhit())
{
char c = _getch();
if(c == 0x1b)
break;
}
}
fprintf(stderr, "\n\n");
}
int main(int argc, char *argv[])
{
fprintf(stderr, "\n***********************************************************************\n");
fprintf(stderr, "* DXL Protocol 2.0 Monitor *\n");
fprintf(stderr, "***********************************************************************\n\n");
char *dev = (char*)"/dev/ttyUSB0";
/* parameter parsing */
while(1)
{
int option_index = 0, c = 0;
static struct option long_options[] = {
{"h", no_argument, 0, 0},
{"help", no_argument, 0, 0},
{"d", required_argument, 0, 0},
{"device", required_argument, 0, 0},
{0, 0, 0, 0}
};
/* parsing all parameters according to the list above is sufficent */
c = getopt_long_only(argc, argv, "", long_options, &option_index);
/* no more options to parse */
if(c == -1) break;
/* unrecognized option */
if(c == '?') {
Usage(argv[0]);
return 0;
}
/* dispatch the given options */
switch(option_index) {
/* h, help */
case 0:
case 1:
Usage(argv[0]);
return 0;
break;
/* d, device */
case 2:
case 3:
Usage(argv[0]);
dev = strdup(optarg);
DXL.ComPort->SetPortName(dev);
break;
default:
Usage(argv[0]);
return 0;
}
}
if(DXL.Connect() == false)
{
fprintf(stderr, " Fail to open USB2Dyanmixel! [%s] \n\n", dev);
return 0;
}
char input[128];
char cmd[80];
char param[20][30];
int num_param;
char* token;
while(1)
{
printf("[CMD] ");
gets(input);
fflush(stdin);
if(strlen(input) == 0)
continue;
token = strtok(input, " ");
if(token == 0)
continue;
strcpy(cmd, token);
token = strtok(0, " ");
num_param = 0;
while(token != 0)
{
strcpy(param[num_param++], token);
token = strtok(0, " ");
}
if(strcmp(cmd, "help") == 0 || strcmp(cmd, "h") == 0 || strcmp(cmd, "?") == 0)
{
Help();
}
else if(strcmp(cmd, "ping") == 0)
{
if(num_param == 0)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
fprintf(stderr, "\n");
PingInfo *data = new PingInfo();
for(int i = 0; i < num_param; i++)
{
if(DXL.Ping(atoi(param[i]), data, 0) == COMM_RXSUCCESS)
{
fprintf(stderr, " ... SUCCESS \r");
fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n",
data->ID, data->ModelNumber, DXL_LOBYTE(data->ModelNumber), DXL_HIBYTE(data->ModelNumber));
}
else
{
fprintf(stderr, " ... FAIL \r");
fprintf(stderr, " [ID:%.3d] \n", atoi(param[i]));
}
}
fprintf(stderr, "\n");
}
else if(strcmp(cmd, "scan") == 0)
{
Scan();
}
else if(strcmp(cmd, "broadcast") == 0)
{
Broadcast();
}
else if(strcmp(cmd, "baud") == 0)
{
if(num_param != 1)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
if(DXL.SetBaudrate(atoi(param[0])) == false)
fprintf(stderr, " Failed to change baudrate! \n");
else
fprintf(stderr, " Success to change baudrate! [ BAUD NUM: %d ]\n", atoi(param[0]));
}
else if(strcmp(cmd, "wrb") == 0 || strcmp(cmd, "w") == 0)
{
if(num_param != 3)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Write(atoi(param[0]), atoi(param[1]), atoi(param[2]), 1);
}
else if(strcmp(cmd, "wrw") == 0)
{
if(num_param != 3)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Write(atoi(param[0]), atoi(param[1]), atoi(param[2]), 2);
}
else if(strcmp(cmd, "wrd") == 0)
{
if(num_param != 3)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Write(atoi(param[0]), atoi(param[1]), atol(param[2]), 4);
}
else if(strcmp(cmd, "rdb") == 0)
{
if(num_param != 2)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Read(atoi(param[0]), atoi(param[1]), 1);
}
else if(strcmp(cmd, "rdw") == 0)
{
if(num_param != 2)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Read(atoi(param[0]), atoi(param[1]), 2);
}
else if(strcmp(cmd, "rdd") == 0)
{
if(num_param != 2)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Read(atoi(param[0]), atoi(param[1]), 4);
}
else if(strcmp(cmd, "r") == 0)
{
if(num_param != 3)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Dump(atoi(param[0]), atoi(param[1]), atoi(param[2]));
}
else if(strcmp(cmd, "mon") == 0)
{
int len = 0;
if(num_param > 2 && strcmp(param[2],"b") == 0) len = 1;
else if(num_param > 2 && strcmp(param[2],"w") == 0) len = 2;
else if(num_param > 2 && strcmp(param[2],"d") == 0) len = 4;
if(num_param != 3 || len == 0)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
Refresh(atoi(param[0]), atoi(param[1]), len);
}
else if(strcmp(cmd, "bp") == 0)
{
if(num_param != 0)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
BroadcastPing();
}
else if(strcmp(cmd, "reboot") == 0)
{
if(num_param != 1)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
int result = DXL.Reboot(atoi(param[0]), 0);
if(result != COMM_RXSUCCESS)
fprintf(stderr, "\n Fail to reboot! \n\n");
else
fprintf(stderr, "\n Success to reboot! \n\n");
}
else if(strcmp(cmd, "reset") == 0)
{
if(num_param != 2)
{
fprintf(stderr, " Invalid parameters! \n");
continue;
}
int result = DXL.FactoryReset(atoi(param[0]), atoi(param[1]), 0);
if(result != COMM_RXSUCCESS)
fprintf(stderr, "\n Fail to reset! \n\n");
else
fprintf(stderr, "\n Success to reset! \n\n");
}
else if(strcmp(cmd, "exit") == 0)
{
DXL.Disconnect();
return 0;
}
}
}
int xlMotor::readVoltage(void){
return Dxl.readByte(myID, 45);
}
void xlMotor::unlock(void){
Dxl.writeByte(myID, 47, 0);
}
void xlMotor::lock(void){
Dxl.writeByte(myID, 47, 1);
}
void xlMotor::setPunch(int newPunch){
Dxl.writeWord(myID, 48, newPunch);
}
int xlMotor::isError(void){
return Dxl.readByte(myID, 18);
}
int main()
{
int PresentPos = 0;
int PresentVel = 0;
int index = 0, result = COMM_TXFAIL, error = 0, Moving = 1;
int GoalPos[2] = {-125700, 125700};
int GoalVel[2] = {-8000, 8000};
Dynamixel DXL("/dev/ttyUSB0");
// Open device
if( DXL.Connect() == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
if(DXL.SetBaudrate(DEFAULT_BAUDNUM) == true)
{
printf( "Succeed to change the baudrate!\n" );
}
else
{
printf( "Failed to change the baudrate!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
result = DXL.WriteByte(DEFAULT_ID, P_TORQUE_ENABLE, 1, &error);
if(result == COMM_RXSUCCESS)
{
PrintErrorCode(error);
}
else
PrintCommStatus(result);
while(1)
{
printf("Definir Velocidad (-8000,8000) \r\n");
cin>>GoalVel[0];
printf( "Presione Enter para continuar!(presione ESC y Enter para salir)\n" );
if(_getch() == 0x1b)
break;
// Write goal position
//DXL.WriteDWord( DEFAULT_ID, P_GOAL_POSITION_LL, GoalPos[index], &error);
// Write goal Speed
result = DXL.WriteDWord( DEFAULT_ID, P_GOAL_VELOCITY_LL, GoalVel[index], &error);
if(result == COMM_RXSUCCESS)
{
PrintErrorCode(error);
}
else
PrintCommStatus(result);
// Change goal position
/* for(int i=0;i<1;i++)
{
// Read present position
//result = DXL.ReadDWord(DEFAULT_ID, P_PRESENT_POSITION_LL, (long*) &PresentPos, &error);
// Read present Velocity
result = DXL.ReadDWord(DEFAULT_ID, P_PRESENT_VELOCITY_LL, (long*) &PresentVel, &error);
if( result == COMM_RXSUCCESS )
{
printf( "%d %d\n", GoalVel[index], PresentVel );
PrintErrorCode(error);
}
else
{
PrintCommStatus(result);
break;
}
}//while(Moving == 1);*/
// Change goal position
}
// Close device
DXL.Disconnect();
printf( "Press Enter key to terminate...\n" );
_getch();
return 0;
}
int xlMotor::isItMoving(void){
return Dxl.readByte(myID, 49);
}
int xlMotor::readTemperature(void){
return Dxl.readByte(myID, 46);
}
int xlMotor::isItWorking(void){
return Dxl.readByte(myID, 47);
}
int main()
{
int id[NUM_ACTUATOR];
float phase[NUM_ACTUATOR];
float theta = 0;
int AmpPos = 150000;
int GoalPos;
int i;
int result;
// Initialize id and phase
for( i=0; i < NUM_ACTUATOR; i++ )
{
id[i] = i+1;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
Dynamixel DXL("/dev/ttyUSB0");
// Open device
if( DXL.Connect() == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
if(DXL.SetBaudrate(DEFAULT_BAUDNUM) == true)
{
printf( "Succeed to change the baudrate!\n" );
}
else
{
printf( "Failed to change the baudrate!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
unsigned char param[NUM_ACTUATOR*(1+4)];
DXL.WriteByte(1, P_TORQUE_ENABLE, 1, 0);
DXL.WriteByte(2, P_TORQUE_ENABLE, 1, 0);
while(1)
{
printf( "Press any key to continue!(press ESC to quit)\n" );
if(_getch() == 0x1b)
break;
theta = 0;
do
{
// Make syncwrite packet
for(i = 0; i < NUM_ACTUATOR; i++)
{
GoalPos = (int)((sin(theta+phase[i])) * (double)AmpPos);
param[i*(1+4)+0] = (unsigned char)id[i];
param[i*(1+4)+1] = DXL_LOBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+2] = DXL_HIBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+3] = DXL_LOBYTE(DXL_HIWORD(GoalPos));
param[i*(1+4)+4] = DXL_HIBYTE(DXL_HIWORD(GoalPos));
}
printf("%d\n", GoalPos);
result = DXL.SyncWrite(P_GOAL_POSITION_LL, 4, param, NUM_ACTUATOR*(1+4));
if( result != COMM_RXSUCCESS )
{
PrintCommStatus(result);
break;
}
theta += STEP_THETA;
usleep(CONTROL_PERIOD*1000);
}while(theta < 2*PI);
}
DXL.Disconnect();
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
void setup() {
pinMode(LED_PIN, OUTPUT);
Serial.begin(9600);
dyn.begin(config::dynamixel::baud_rate);
wrist_left.init();
wrist_right.init();
}
void loop() {
static long p_time = 0;
while (Serial.available()) {
Serial.print((char) Serial.read());
Serial.flush();
}
long c_time = millis();
if (c_time - p_time > 500) {
p_time = c_time;
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
Serial.print("Dynamixel 1: ");
Serial.print(wrist_left.get_voltage());
Serial.print(" Dynamixel 2: ");
Serial.print(wrist_right.get_voltage());
Serial.println();
Serial.flush();
}
}
