void motor_sync_move(const uint8_t size, const uint8_t * id, const uint16_t * position, const char blocking) { volatile int i, CommStatus; dxl_set_txpacket_id(MOTOR_BROADCAST_ID); //set broadcast id dxl_set_txpacket_instruction(INST_SYNC_WRITE); //set instruction type dxl_set_txpacket_parameter(0, GOAL_POSITION_L); //memory area to write dxl_set_txpacket_parameter(1, 2); //length of the data for(i=0;i<size;i++){ dxl_set_txpacket_parameter(2+(3*i), id[i]); //id dxl_set_txpacket_parameter(2+(3*i)+1, dxl_get_lowbyte(position[i]));//low byte dxl_set_txpacket_parameter(2+(3*i)+2, dxl_get_highbyte(position[i]));//high byte } dxl_set_txpacket_length((2+1)*size+4); //set packet length dxl_txrx_packet(); //transmit packet CommStatus = dxl_get_result(); //get transmission state if( CommStatus == COMM_RXSUCCESS ){ //transmission succeded PrintErrorCode(); //show potentiol motors error (overload, overheat,etc....) if (blocking == MOTOR_MOVE_BLOCKING) //blocking function requested { // Wait for finish of all motors for (i = 0; i < size; i++) motor_wait_finish(id[i], position[i]); } } else //communication failed PrintCommStatus(CommStatus); //show communication error }
bool ControlUtils::syncWriteWord(int8_t addr, const std::vector<int> &joints, const std::vector<int16_t> &val) { if (joints.size() != val.size()) return false; if (joints.empty()) return true; // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, addr); dxl_set_txpacket_parameter(1, 2); for (size_t i = 0; i < joints.size(); i++) { dxl_set_txpacket_parameter(2+3*i, _id[joints[i]]); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(val[i])); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(val[i])); } dxl_set_txpacket_length((2+1)*joints.size()+4); dxl_txrx_packet(); if(dxl_get_result() == COMM_RXSUCCESS) return true; else return false; }
//------------------------------------------------------------------------------ // // Data Format for SYNC write: // ID 0xFE // Length ((L+1)*N )+4 L: Data Length per RX-28, N: Number of motors // Instruction 0x83 // Start address to write data parameter 1 // Length of data to write parameter 2 = Number of motors // // First ID of RX-28 parameter 3 // First data of the 1st RX-28 // .. // Lth data of the 1st RX-28 // // #define ID (2) // #define LENGTH (3) // #define INSTRUCTION (4) // #define ERRBIT (4) // #define PARAMETER (5) // #define DEFAULT_BAUDNUMBER (1) // int16_t MOTOR_SendPositionCommandsToMotors(void) { int16_t i; int16_t numJointsSent = 0; dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 4); // Write 4 bytes for( i=0; i<MOTOR_NJOINTS; i++ ) { if ( !joint[i].bPositionSet ) { // Don't send a joint value if we haven't got a command continue; } dxl_set_txpacket_parameter(2+(5*numJointsSent), i); dxl_set_txpacket_parameter(2+(5*numJointsSent)+1, dxl_get_lowbyte(joint[i].pos)); dxl_set_txpacket_parameter(2+(5*numJointsSent)+2, dxl_get_highbyte(joint[i].pos)); dxl_set_txpacket_parameter(2+(5*numJointsSent)+3, dxl_get_lowbyte(joint[i].speed)); dxl_set_txpacket_parameter(2+(5*numJointsSent)+4, dxl_get_highbyte(joint[i].speed)); numJointsSent++; } dxl_set_txpacket_length((5*numJointsSent)+4); dxl_txrx_packet(); return MOTOR_SUCCESS; }
void dxl_sync_write_send() { while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; dxl_txrx_packet(); }
void dxl_ping( int id ) { while(giBusUsing); gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_PING; gbInstructionPacket[LENGTH] = 2; dxl_txrx_packet(); }
void dxl_write_byte( int id, int address, int value ) { while(giBusUsing); gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_WRITE; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = (unsigned char)value; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); }
void dxl_write_word( int id, int address, int value ) { while(giBusUsing); gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_WRITE; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); gbInstructionPacket[LENGTH] = 5; dxl_txrx_packet(); }
//############################################################################## uint8_t dxl_recover(uint8_t id, HaViMo2_Region_Buffer_t* hvm2rb) { if (hvm2rb==0) return 0xFF; while (giBusUsing); hvm2rb->valid=0; uint8_t i; for (i=0; i<15; i++) { gbInstructionPacket[ID] = id; gbInstructionPacket[INSTRUCTION] = INST_READ; gbInstructionPacket[PARAMETER] = ((i+1)*16); gbInstructionPacket[PARAMETER+1] = 16; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); if (gbStatusPacket[LENGTH]==(16+2)) { hvm2rb->valid++; } else { return hvm2rb->valid; } hvm2rb->rb[i].Index=gbStatusPacket[PARAMETER]; hvm2rb->rb[i].Color=gbStatusPacket[PARAMETER+1]; hvm2rb->rb[i].NumPix=( (uint16_t)gbStatusPacket[PARAMETER+2]+ ((uint16_t)gbStatusPacket[PARAMETER+3]<<8)); hvm2rb->rb[i].SumX= ( ((uint32_t)gbStatusPacket[PARAMETER+4]+ ((uint32_t)gbStatusPacket[PARAMETER+5]<<8)+ ((uint32_t)gbStatusPacket[PARAMETER+6]<<16)) ); hvm2rb->rb[i].SumY= ( ((uint32_t)gbStatusPacket[PARAMETER+8]+ ((uint32_t)gbStatusPacket[PARAMETER+9]<<8)+ ((uint32_t)gbStatusPacket[PARAMETER+10]<<16)) ); hvm2rb->rb[i].MaxX=gbStatusPacket[PARAMETER+12]; hvm2rb->rb[i].MinX=gbStatusPacket[PARAMETER+13]; hvm2rb->rb[i].MaxY=gbStatusPacket[PARAMETER+14]; hvm2rb->rb[i].MinY=gbStatusPacket[PARAMETER+15]; } return hvm2rb->valid; }
int dxl_read_word( int id, int address ) { while(giBusUsing); gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_READ; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = 2; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); }
void SendServoTargetPos(uint8_t ServoNo, int16_t targetPos,uint16_t targetSpeed) { //printf("setting servo %d pos to %d @ %d\n", ServoNo, targetPos, targetSpeed); dxl_set_txpacket_id(ServoNo); dxl_set_txpacket_instruction(INST_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, LO_BYTE(targetPos)); dxl_set_txpacket_parameter(2, HI_BYTE(targetPos)); dxl_set_txpacket_parameter(3, LO_BYTE(targetSpeed)); dxl_set_txpacket_parameter(4, HI_BYTE(targetSpeed)); dxl_set_txpacket_length(7); dxl_txrx_packet(); }
int dxl_read_byte( int id, int address ) { while(giBusUsing); // TxDString("E"); gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_READ; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = 1; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); return (int)gbStatusPacket[PARAMETER]; }
//-------------------------------------------------------------------- //[SetRegOnAllServos] Function that is called to set the state of one // register in all of the servos, like Torque on... //-------------------------------------------------------------------- void SetRegOnAllServos(uint8_t bReg, uint8_t bVal) { dxl_set_txpacket_id(AX_ID_BROADCAST); dxl_set_txpacket_instruction(AX_CMD_SYNC_WRITE); dxl_set_txpacket_parameter(0, bReg); // which register dxl_set_txpacket_parameter(1, 1); // 1 byte dxl_set_txpacket_length(2*NUMSERVOS+3); for (byte i = 0; i < NUMSERVOS; i++) { dxl_set_txpacket_parameter(2+i*2, (cPinTable[i])); // 1 byte dxl_set_txpacket_parameter(3+i*2, bVal); // 1 byte } dxl_txrx_packet(); //return dxl_get_result(); // don't care for now }
// Function to write data into the control table of the Dynamixel actuator // Length N+3 (N is the number of data to be written) // Instruction 0x03 // Parameter1 Starting address of the location where the data is to be written // Parameter2 1st data to be written // Parameter3 2nd data to be written, etc. // In this case we only have a 1-byte parameter int dxl_write_byte( int id, int address, int value ) { // wait for the bus to be free while(giBusUsing); // create a WRITE instruction packet and send gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_WRITE; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = (unsigned char)value; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); return gbCommStatus; }
// Read data from the control table of a Dynamixel device // Length 0x04, Instruction 0x02 // Parameter1 Starting address of the location where the data is to be read // Parameter2 Length of the data to be read (one byte in this case) int dxl_read_byte( int id, int address ) { // wait for the bus to be free while(giBusUsing); // create a READ instruction packet and send gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_READ; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = 1; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); return (int)gbStatusPacket[PARAMETER]; }
// Read data from the control table of a Dynamixel device // Length 0x04, Instruction 0x02 // Parameter1 Starting address of the location where the data is to be read // Parameter2 Length of the data to be read (2 bytes in this case) int dxl_read_word( int id, int address ) { // wait for the bus to be free while(giBusUsing); // create a READ instruction packet and send gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_READ; gbInstructionPacket[PARAMETER] = (unsigned char)address; gbInstructionPacket[PARAMETER+1] = 2; gbInstructionPacket[LENGTH] = 4; dxl_txrx_packet(); // combine the 2 bytes into a word and return return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); }
void dxl_SetPosition(int ServoID, int Position, int Speed) // Sets the target position and speed of the specified servo ////Made by Martin S. Mason(Professor @Mt. San Antonio College) { while(giBusUsing); gbInstructionPacket[ID] = (unsigned char)ServoID; gbInstructionPacket[INSTRUCTION] = INST_WRITE; gbInstructionPacket[PARAMETER] = (unsigned char)30; gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position); gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position); gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed); gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed); gbInstructionPacket[LENGTH] = 7; dxl_txrx_packet(); }
// Function for controlling several Dynamixel actuators at the same time. // The communication time decreases by using the Sync Write instruction // since many instructions can be transmitted by a single instruction. // However, you can use this instruction only when the lengths and addresses // of the control table to be written to are the same. // The broadcast ID (0xFE) needs to be used for transmitting. // ID: 0xFE // Length: (L + 1) * N + 4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel actuators) // Instruction: 0x83 // Parameter1 Starting address of the location where the data is to be written // Parameter2 The length of the data to be written (L) // Parameter3 The ID of the 1st Dynamixel actuator // Parameter4 The 1st data for the 1st Dynamixel actuator // Parameter5 The 2nd data for the 1st Dynamixel actuator // ParameterL+4 The ID of the 2nd Dynamixel actuator // ParameterL+5 The 1st data for the 2nd Dynamixel actuator // ParameterL+6 The 2nd data for the 2nd Dynamixel actuator // … // NOTE: this function only allows 2 bytes of data per actuator int dxl_sync_write_word( int NUM_ACTUATOR, int address, const uint8 ids[], int16 values[] ) { int i = 0; // wait for the bus to be free while(giBusUsing); // check how many actuators are to be broadcast to if (NUM_ACTUATOR == 0) { // nothing to do, return return 0; } else if (NUM_ACTUATOR == 1) { // easy, we can use dxl_write_word for a single actuator dxl_write_word( ids[0], address, values[0] ); return 0; } // Multiple values, create sync write packet // ID is broadcast id dxl_set_txpacket_id(BROADCAST_ID); // Instruction is sync write dxl_set_txpacket_instruction(INST_SYNC_WRITE); // Starting address where to write to dxl_set_txpacket_parameter(0, address); // Length of data to be written (each word = 2 bytes) dxl_set_txpacket_parameter(1, 2); // Loop over the active Dynamixel id's for( i=0; i<NUM_ACTUATOR; i++ ) { // retrieve the id and value for each actuator and add to packet dxl_set_txpacket_parameter(2+3*i, ids[i]); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(values[i])); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(values[i])); } // total length is as per formula above with L=2 dxl_set_txpacket_length((2+1)*NUM_ACTUATOR + 4); // all done, send the packet dxl_txrx_packet(); // there is no status packet return, so return the CommStatus return gbCommStatus; }
// Function setting goal and speed for all Dynamixel actuators at the same time // Uses the Sync Write instruction (also see dxl_sync_write_word) // Inputs: NUM_ACTUATOR - number of Dynamixel servos // ids - array of Dynamixel ids to write to // goal - array of goal positions // speed - array of moving speeds //Returns: commStatus int dxl_set_goal_speed( int NUM_ACTUATOR, const uint8 ids[], uint16 goal[], uint16 speed[] ) { int i = 0; // wait for the bus to be free while(giBusUsing); // check how many actuators are to be broadcast to if (NUM_ACTUATOR == 0) { // nothing to do, return return 0; } // Multiple values, create sync write packet // ID is broadcast id dxl_set_txpacket_id(BROADCAST_ID); // Instruction is sync write dxl_set_txpacket_instruction(INST_SYNC_WRITE); // Starting address where to write to dxl_set_txpacket_parameter(0, DXL_GOAL_POSITION_L); // Length of data to be written (2 words = 4 bytes) dxl_set_txpacket_parameter(1, 4); // Loop over the active Dynamixel id's for( i=0; i<NUM_ACTUATOR; i++ ) { // retrieve the id and value for each actuator and add to packet dxl_set_txpacket_parameter(2+5*i, ids[i]); dxl_set_txpacket_parameter(2+5*i+1, dxl_get_lowbyte(goal[i])); dxl_set_txpacket_parameter(2+5*i+2, dxl_get_highbyte(goal[i])); dxl_set_txpacket_parameter(2+5*i+3, dxl_get_lowbyte(speed[i])); dxl_set_txpacket_parameter(2+5*i+4, dxl_get_highbyte(speed[i])); } // total length is as per formula above with L=4 dxl_set_txpacket_length((4+1)*NUM_ACTUATOR + 4); // all done, send the packet dxl_txrx_packet(); // there is no status packet return, so return the CommStatus return gbCommStatus; }
void MotorControl( int id, int power ){ int CommStatus; // printf( "%d %d\n", id, power ); // dxl_write_word( id, P_GOAL_SPEED_L, power ); if(1){ dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_SPEED_L); dxl_set_txpacket_parameter(1, 2); dxl_set_txpacket_parameter(2, id); dxl_set_txpacket_parameter(3, dxl_get_lowbyte(power)); dxl_set_txpacket_parameter(4, dxl_get_highbyte(power)); dxl_set_txpacket_length(4+3*1); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); } }
// Ping a Dynamixel device int dxl_ping( int id ) { // wait for the bus to be free while(giBusUsing); // create a PING instruction packet and send gbInstructionPacket[ID] = (unsigned char)id; gbInstructionPacket[INSTRUCTION] = INST_PING; gbInstructionPacket[LENGTH] = 2; dxl_txrx_packet(); if (gbCommStatus == COMM_RXSUCCESS) { // return the error code return (int)gbStatusPacket[ERRBIT]; // check if servo exists (via timeout) } else if( gbCommStatus == COMM_RXTIMEOUT ) { return -1; } else { return 0; } }
int main(void) { int id[NUM_ACTUATOR]; float phase[NUM_ACTUATOR]; float theta = 0; int AmpPos = 512; //int AmpPos = 2048; // for EX series int GoalPos; int i; int CommStatus; serial_initialize(57600); dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index sei(); // Interrupt Enable printf( "\n\nSyncWrite example for CM-700\n\n" ); for( i=0; i<NUM_ACTUATOR; i++ ) { id[i] = i+1; phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR; } // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); // Set goal position dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos ); _delay_ms(1000); while(1) { // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<NUM_ACTUATOR; i++ ) { dxl_set_txpacket_parameter(2+3*i, id[i]); GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos); printf( "%d ", GoalPos ); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos)); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos)); } dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4); printf( "\n" ); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); theta += STEP_THETA; if( theta > 2*PI ) theta -= 2*PI; _delay_ms(CONTROL_PERIOD); } return 0; }
int main(void) { #if 0 int id[NUM_ACTUATOR]; float phase[NUM_ACTUATOR]; float theta = 0; int AmpPos = 512; //int AmpPos = 2048; // for EX series int GoalPos; int i; int CommStatus; int isPress = 0; int isOn = 0; unsigned char ReceivedData; int Value; mServoList[0] = (stServo *)malloc(sizeof(stServo)); memset((void *)mServoList[0], 0x00, sizeof(stServo) ); mServoList[0]->id = 4; serial_initialize(57600); dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index sei(); // Interrupt Enable printf( "\n\nSyncWrite example for CM-700\n\n" ); #ifdef MODE_SYNC for( i=0; i<NUM_ACTUATOR; i++ ) { id[i] = i+2; phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR; } #else int wPresentPos = 512; #endif //Set EEP Lock dxl_write_word( BROADCAST_ID, P_EEP_LOCK, 1 ); // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); // Set goal position dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos ); dxl_write_word( 4, P_TORQUE_LIMIT_L, 0); _delay_ms(1000); while(1) { if(~PIND & SW_START){ isPress = 1; }else{ if( isPress == 1 ){ if( isOn == 0 ){ isOn = 1; }else{ isOn = 0; } } isPress = 0; } // while( ReceivedData = getchar() != NULL ){ if(ReceivedData == 'u') Value++; else if(ReceivedData == 'd') Value--; printf("%d, %d\r\n", Value, ReceivedData); // } if( isOn ){ #ifdef MODE_SYNC // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<NUM_ACTUATOR; i++ ) { dxl_set_txpacket_parameter(2+3*i, id[i]); GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos); printf( "%d ", GoalPos ); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos)); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos)); } dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4); printf( "\n" ); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); theta += STEP_THETA; if( theta > 2*PI ) theta -= 2*PI; #else wPresentPos = dxl_read_word( 4, P_PRESENT_POSITION_L ); printf( "%d\n", wPresentPos ); dxl_write_word( 2, P_GOAL_POSITION_L, wPresentPos ); dxl_write_word( 3, P_GOAL_POSITION_L, wPresentPos ); PrintErrorCode(); #endif } getServoStatus(); _delay_ms(CONTROL_PERIOD); } return 0; #endif #if 0 DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; while (1) { if(~PIND & SW_START) PORTC = ~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY); else PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; } return 1; #endif while(isFinish == 0){ _delay_ms(500); getSerialData(); // ReceivedData = getchar(); //if(ReceivedData == 'u'){ //printf("%d\r\n", Value); //Value++; //}else if(ReceivedData == 'd'){ //printf("%d\r\n", Value); //Value--; //}else if(ReceivedData == 10 || ReceivedData == 13 ){ //printf("%s\r\n", "end"); //break; //} printf("%s\r\n", "Loop"); } printf("%s\r\n", "finish"); return 0; }
void ServoControl( int act ){ int i; int CommStatus = 0; if( act >= ACT_MAX ){ // printf( "act error: %d / %d\n", act, SERVO_MAX ); return; } //GetAngle int angle = 0; int diffMax = 0; int angleDiff[SERVO_MAX] = {0}; for(int i=0; i<SERVO_MAX; i++ ){ // if( motionFirst < 0 ){ angle = dxl_read_word( servoId[i], P_PRESENT_POSITION_L ); // }else{ // angle = angleList[motionFirst][i]; // } angleDiff[i] = angleList[act][i] - angle; if( angleDiff[i] < 0 ){ angleDiff[i] = angleDiff[i] * -1; } if( diffMax < angleDiff[i] ){ diffMax = angleDiff[i]; } } // motionFirst = act; int speed[SERVO_MAX] = {100}; for(int i=0; i<SERVO_MAX; i++ ){ speed[i] = (int)((float)(angleList[act][SERVO_MAX]) * ((float)angleDiff[i] / diffMax)); if( speed[i] == 0 ){ speed[i] = 1; } } // diffmaxTest[motionCount-1] = diffMax; // movingTime = ((float)CYCLE_TIME/VALUE_MAX) * ((float)VALUE_MAX / angleList[act][SERVO_MAX]) * diffMax; movingTime = diffMax * (float)(((VALUE_MAX*10)/angleList[act][SERVO_MAX])/2); if( movingTime < MAIN_DELAY ){ movingTime = MAIN_DELAY; } //Speed dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_SPEED_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<SERVO_MAX; i++ ){ dxl_set_txpacket_parameter(2+(3*i), servoId[i]); dxl_set_txpacket_parameter(3+(3*i), dxl_get_lowbyte(speed[i])); dxl_set_txpacket_parameter(4+(3*i), dxl_get_highbyte(speed[i])); } dxl_set_txpacket_length(4+3*SERVO_MAX); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ){ PrintErrorCode(); //Angle dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<SERVO_MAX; i++ ){ dxl_set_txpacket_parameter(2+(3*i), servoId[i]); dxl_set_txpacket_parameter(3+(3*i), dxl_get_lowbyte(angleList[act][i])); dxl_set_txpacket_parameter(4+(3*i), dxl_get_highbyte(angleList[act][i])); } dxl_set_txpacket_length(4+3*SERVO_MAX); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ){ PrintErrorCode(); }else{ PrintCommStatus(CommStatus); } }else{ PrintCommStatus(CommStatus); } }
void DXLJointInterface::process() { switch(controlType()) { case CT_POSITION: { int goalTicks = m_goalPosition / TICKS_TO_RAD; dxl_write_word(id(), REG_GOAL_POSITION, goalTicks + m_tickOffset); } break; case CT_VELOCITY: { int velTicks = m_goalSpeed / VEL_TO_RAD_PER_S; if(velTicks < 0) { velTicks = (1 << 10) | (-velTicks); } dxl_write_word(id(), REG_MOVING_SPEED, velTicks); } break; case CT_TORQUE: { int torqueTicks = m_goalTorque / TORQUE_TO_NM; if(torqueTicks < 0) { torqueTicks = (1 << 10) | (-torqueTicks); } if(torqueTicks == 0) torqueTicks = 1; dxl_write_word(id(), REG_GOAL_TORQUE, torqueTicks); } break; } // int torqueTicks = 1023.0 * (m_torqueLimit / 4.4); // at 12V // if(torqueTicks > 1023) // torqueTicks = 1023; // printf("torque: %4d\n", torqueTicks); // // dxl_write_word(id(), 34, torqueTicks); dxl_set_txpacket_id(id()); dxl_set_txpacket_instruction(INST_READ); dxl_set_txpacket_length(2 + 2); dxl_set_txpacket_parameter(0, 36); // start address: present pos dxl_set_txpacket_parameter(1, 6); // #bytes dxl_txrx_packet(); if(dxl_get_rxpacket_length() != 6+2) { fprintf(stderr, "DXL: Got malformed read answer\n"); return; } int currentTicks = (dxl_get_rxpacket_parameter(1) << 8) | dxl_get_rxpacket_parameter(0); m_currentPosition = TICKS_TO_RAD * (currentTicks - m_tickOffset); double lastVel = m_currentVelocity; int velTicks = (dxl_get_rxpacket_parameter(3) << 8) | dxl_get_rxpacket_parameter(2); if(velTicks & (1 << 10)) velTicks = -(velTicks & 1023); m_currentVelocity = VEL_TO_RAD_PER_S * velTicks; int loadTicks = (dxl_get_rxpacket_parameter(5) << 8) | dxl_get_rxpacket_parameter(4); if(loadTicks & (1 << 10)) loadTicks = -(loadTicks & 1023); m_currentLoad = (1.0 / 1023.0) * loadTicks; int current = dxl_read_word(id(), 68) - 2048; m_currentCurrent = 0.0045 * current; m_currentAcc = (m_currentVelocity - lastVel) / 0.02; }
void Dynamixel::txrxPacket(void){ dxl_txrx_packet(); }
int main() { int id[NUM_ACTUATOR]; int baudnum = 1; int deviceIndex = 0; float phase[NUM_ACTUATOR]; float theta = 0; int AmpPos = 512; //int AmpPos = 2048; // for EX series int GoalPos; int i; int CommStatus; printf( "\n\nSyncWrite example for Linux\n\n" ); // Initialize id and phase for( i=0; i<NUM_ACTUATOR; i++ ) { id[i] = i+1; phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR; } ///////// Open USB2Dynamixel //////////// if( dxl_initialize(deviceIndex, baudnum) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; } else printf( "Succeed to open USB2Dynamixel!\n" ); // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); // Set goal position dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos ); while(1) { printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" ); if(getchar() == 0x1b) break; theta = 0; do { // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<NUM_ACTUATOR; i++ ) { dxl_set_txpacket_parameter(2+3*i, id[i]); GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (double)AmpPos); printf( "%d:%d ", id[i], GoalPos ); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos)); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos)); } dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4); printf( "\n" ); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { PrintErrorCode(); } else { PrintCommStatus(CommStatus); break; } theta += STEP_THETA; usleep(CONTROL_PERIOD); }while(theta < 2*PI); } dxl_terminate(); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; }