Commit 6d3ac527 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files
parents 66ac77f4 d81925c6
......@@ -214,7 +214,7 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
* \brief
*
*/
unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt);
unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt);
#ifdef __cplusplus
}
......
......@@ -51,10 +51,21 @@ unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *d
destination[DYN_HEADER_OFF]=0xFF;
destination[DYN_HEADER_OFF+1]=0xFF;
destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
destination[DYN_LENGTH_OFF]=2;//source[DYN2_LENGTH_OFF];
destination[DYN_INST_OFF]=source[DYN2_INST_OFF];
for(i=0;i<destination[DYN_LENGTH_OFF];i++)
destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
switch(source[DYN2_INST_OFF])
{
case DYN_PING: break;
case DYN_READ: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
destination[DYN_DATA_OFF+1]=source[DYN2_DATA_OFF+2];
destination[DYN_LENGTH_OFF]+=2;
break;
case DYN_WRITE: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
for(i=0;i<source[DYN2_DATA_OFF]-5;i++)
destination[DYN_DATA_OFF+1+i]=source[DYN2_DATA_OFF+2+i];
destination[DYN_LENGTH_OFF]+=source[DYN2_DATA_OFF]-4;
break;
}
dyn_set_checksum(destination);
return 0x01;
}
......@@ -247,10 +258,10 @@ unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char
destination[DYN_HEADER_OFF]=0xFF;
destination[DYN_HEADER_OFF+1]=0xFF;
destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF]-2;
destination[DYN_ERROR_OFF]=source[DYN2_ERROR_OFF];
for(i=0;i<destination[DYN_LENGTH_OFF];i++)
destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
for(i=0;i<destination[DYN_LENGTH_OFF]-2;i++)
destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+1+i];
dyn_set_checksum(destination);
return 0x01;
}
......
......@@ -88,11 +88,25 @@ unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *
destination[DYN2_HEADER_OFF+2]=0xFD;
destination[DYN2_RESERVED]=0x00;
destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
destination[DYN2_LENGTH_OFF]=3;//source[DYN_LENGTH_OFF];
destination[DYN2_LENGTH_OFF+1]=0x00;
destination[DYN2_INST_OFF]=source[DYN_INST_OFF];
for(i=0;i<source[DYN_LENGTH_OFF];i++)
destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
switch(source[DYN_INST_OFF])
{
case DYN_PING: break;
case DYN_READ: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
destination[DYN2_DATA_OFF+1]=0x00;
destination[DYN2_DATA_OFF+2]=source[DYN_DATA_OFF+1];
destination[DYN2_DATA_OFF+3]=0x00;
(*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=4;
break;
case DYN_WRITE: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
destination[DYN2_DATA_OFF+1]=0x00;
for(i=0;i<source[DYN_LENGTH_OFF]-3;i++)
destination[DYN2_DATA_OFF+2+i]=source[DYN_DATA_OFF+1+i];
(*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=source[DYN_LENGTH_OFF]-1;
break;
}
dyn2_set_checksum(destination);
return 0x01;
......@@ -417,12 +431,12 @@ unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *
destination[DYN2_HEADER_OFF+2]=0xFD;
destination[DYN2_RESERVED]=0x00;
destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF]+2;
destination[DYN2_LENGTH_OFF+1]=0x00;
destination[DYN2_INST_OFF]=inst;
destination[DYN2_ERROR_OFF]=source[DYN_ERROR_OFF];
for(i=0;i<source[DYN_LENGTH_OFF];i++)
destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
for(i=0;i<destination[DYN2_LENGTH_OFF]-4;i++)
destination[DYN2_DATA_OFF+1+i]=source[DYN_DATA_OFF+i];
dyn2_set_checksum(destination);
return 0x01;
......
......@@ -1297,13 +1297,26 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
return error;
}
unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt)
unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt)
{
unsigned char error=DYN_COMM_ERROR;
unsigned char error=DYN_COMM_ERROR,i;
if(master!=0x00000000)
{
dyn2_copy_packet(inst_pkt,master->tx_buffer);
if(inst_ver==DYN_VER1)
{
if(master->version==DYN_VER1)
dyn_copy_packet(inst_pkt,master->tx_buffer);
else
dyn2_convert_v1_inst_packet(inst_pkt,master->tx_buffer);
}
else
{
if(master->version==DYN_VER1)
dyn_convert_v2_inst_packet(inst_pkt,master->tx_buffer);
else
dyn2_copy_packet(inst_pkt,master->tx_buffer);
}
master->rx_num_packets=0x01;
master->rx_no_answer=0x00;
// enable transmission
......@@ -1322,7 +1335,20 @@ unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,
}
// wait for the replay within the given timeout
error=dyn_master_wait_reception(master);
dyn2_copy_packet(master->rx_buffer,status_pkt);
if(inst_ver==DYN_VER1)
{
if(master->version==DYN_VER1)
dyn_copy_packet(master->rx_buffer,status_pkt);
else
dyn_convert_v2_status_packet(master->rx_buffer,status_pkt);
}
else
{
if(master->version==DYN_VER1)
dyn2_convert_v1_status_packet((TDynInstruction)inst_pkt[DYN2_INST_OFF],master->rx_buffer,status_pkt);
else
dyn2_copy_packet(master->rx_buffer,status_pkt);
}
}
return error;
......
......@@ -169,13 +169,19 @@ void dyn_slave_loop(TDynamixelSlave *slave)
if(send_status==0x01)
dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
}
if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
{
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
}
if(send_status==0xFF)// packet has not been processed
{
if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
{
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
if(slave->version==DYN_VER1)
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
else
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
}
}
}
else
{
......
......@@ -98,6 +98,8 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
(*error)=DYN_INST_ERROR;
}
}
else if(id==DYN_BROADCAST_ID)
send_status=0xFF;
break;
case DYN_WRITE: (*length)=dyn_get_write_data(rx_buffer,data);
(*error)=device->on_write(dyn_get_write_address(rx_buffer),*length,data);
......@@ -108,6 +110,8 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
if((*error)!=DYN_NO_ERROR)
(*error)=DYN_INST_ERROR;
}
else if(id==DYN_BROADCAST_ID)
send_status=0xFF;
break;
case DYN_REG_WRITE: device->reg_length=dyn_get_reg_write_data(rx_buffer,device->reg_buffer);
device->reg_address=dyn_get_reg_write_address(rx_buffer);
......@@ -199,6 +203,8 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
}
}
}
else
send_status=0xFF;
}
}
......@@ -242,6 +248,8 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
(*error)=DYN_INST_ERROR;
}
}
else if(id==DYN_BROADCAST_ID)
send_status=0xFF;
break;
case DYN_WRITE: (*length)=dyn2_get_write_data(rx_buffer,data);
(*error)=device->on_write(dyn2_get_write_address(rx_buffer),*length,data);
......@@ -252,6 +260,8 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
if((*error)!=DYN_NO_ERROR)
(*error)=DYN_INST_ERROR;
}
else if(id==DYN_BROADCAST_ID)
send_status=0xFF;
break;
case DYN_REG_WRITE: device->reg_length=dyn2_get_reg_write_data(rx_buffer,device->reg_buffer);
device->reg_address=dyn2_get_reg_write_address(rx_buffer);
......@@ -376,6 +386,8 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
}
}
}
else
send_status=0xFF;
}
}
......
......@@ -35,7 +35,7 @@ typedef struct // Header Structure (total 64unsigned char)
typedef struct // Step Structure (total 64unsigned char)
{
short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
unsigned short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
unsigned char pause; // Pause time 62
unsigned char time; // Time 63
}TStep;
......
......@@ -120,7 +120,7 @@ void action_load_next_step(TActionMModule *action)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
angle=action->current_page.steps[action->current_step_index].position[i];
angle=(signed short int)action->current_page.steps[action->current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
target_angles=action->mmodule.manager->servo_values[i].target_angle;
else
......@@ -131,9 +131,9 @@ void action_load_next_step(TActionMModule *action)
else
{
if(action->current_step_index==action->current_page.header.stepnum-1)
next_angle=action->next_page.steps[0].position[i];
next_angle=(signed short int)action->next_page.steps[0].position[i];
else
next_angle=action->current_page.steps[action->current_step_index+1].position[i];
next_angle=(signed short int)action->current_page.steps[action->current_step_index+1].position[i];
if(next_angle==0x5A00)
next_angles=target_angles;
else
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment