Skip to content
Snippets Groups Projects
Commit b5d6ada2 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Fully implemented the communication protocol.

parent c1171ae0
No related branches found
No related tags found
No related merge requests found
...@@ -37,7 +37,7 @@ uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data) ...@@ -37,7 +37,7 @@ uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
{ {
uint8_t error=RAM_SUCCESS,byte_value,i,j,do_op=0x00,module; uint8_t error=RAM_SUCCESS,byte_value,i,j,module;
uint16_t word_value; uint16_t word_value;
// pre-write operation // pre-write operation
...@@ -73,7 +73,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) ...@@ -73,7 +73,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
} }
for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2) for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
{ {
do_op=0x01;
if(ram_in_range(i,i,address,length)) if(ram_in_range(i,i,address,length))
{ {
byte_value=data[i-address]; byte_value=data[i-address];
...@@ -87,11 +86,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) ...@@ -87,11 +86,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
manager_set_module(j+1,module); manager_set_module(j+1,module);
} }
} }
if(do_op)
{
do_op=0x00;
manager_disable_servos();
}
if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length)) if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length))
{ {
if(data[BIOLOID_MM_CONTROL-address]&0x01) if(data[BIOLOID_MM_CONTROL-address]&0x01)
...@@ -99,22 +93,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) ...@@ -99,22 +93,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
else else
manager_disable(); manager_disable();
} }
/* if(ram_in_range(BIOLOID_MM_KNEE_GAIN_L,BIOLOID_MM_KNEE_GAIN_L,address,length))
manager_set_knee_gain(data[BIOLOID_MM_KNEE_GAIN_L-address]);
if(ram_in_range(BIOLOID_MM_KNEE_GAIN_R,BIOLOID_MM_KNEE_GAIN_R,address,length))
manager_set_knee_gain(data[BIOLOID_MM_KNEE_GAIN_R-address]);
if(ram_in_range(BIOLOID_MM_ANKLE_PITCH_GAIN_L,BIOLOID_MM_ANKLE_PITCH_GAIN_L,address,length))
manager_set_ankle_pitch_gain(data[BIOLOID_MM_ANKLE_PITCH_GAIN_L,address]);
if(ram_in_range(BIOLOID_MM_ANKLE_PITCH_GAIN_R,BIOLOID_MM_ANKLE_PITCH_GAIN_R,address,length))
manager_set_ankle_pitch_gain(data[BIOLOID_MM_ANKLE_PITCH_GAIN_R,address]);
if(ram_in_range(BIOLOID_MM_HIP_ROLL_GAIN_L,BIOLOID_MM_HIP_ROLL_GAIN_L,address,length))
manager_set_hip_roll_gain(data[BIOLOID_MM_ROLL_GAIN_L-address]);
if(ram_in_range(BIOLOID_MM_HIP_ROLL_GAIN_R,BIOLOID_MM_HIP_ROLL_GAIN_R,address,length))
manager_set_hip_roll_gain(data[BIOLOID_MM_ROLL_GAIN_R-address]);
if(ram_in_range(BIOLOID_MM_ANKLE_ROLL_GAIN_L,BIOLOID_MM_ANKLE_ROLL_GAIN_L,address,length))
manager_set_ankle_roll_gain(data[BIOLOID_MM_CONTROL_L-address]);
if(ram_in_range(BIOLOID_MM_ANKLE_ROLL_GAIN_R,BIOLOID_MM_ANKLE_ROLL_GAIN_R,address,length))
manager_set_ankle_roll_gain(data[BIOLOID_MM_CONTROL_R-address]);*/
if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length)) if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
{ {
if(data[BIOLOID_ACTION_CONTROL-address]&0x01) if(data[BIOLOID_ACTION_CONTROL-address]&0x01)
...@@ -124,15 +102,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) ...@@ -124,15 +102,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
} }
if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier
action_load_page(data[BIOLOID_ACTION_PAGE-address]); action_load_page(data[BIOLOID_ACTION_PAGE-address]);
/* if(ram_in_range(BIOLOID_IMU_CONTROL,BIOLOID_IMU_CONTROL,address,length))
{
if(data[BIOLOID_IMU_CONTROL-address]&0x01)
imu_start();
else
imu_stop();
if(data[BIOLOID_IMU_CONTROL-address]&0x02)
imu_start_gyro_cal();
}*/
// write eeprom // write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++) for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]); EE_WriteVariable(i,data[j]);
...@@ -147,110 +116,117 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) ...@@ -147,110 +116,117 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
void COMM_TIMER_IRQHandler(void) void COMM_TIMER_IRQHandler(void)
{ {
uint8_t error,length,prev_id,id; uint8_t error,length,prev_id,id;
TDynVersion version;
TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update); TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update);
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
{ {
dyn_slave_get_inst_packet(inst_packet);// get the received packet version=dyn_slave_get_inst_packet(inst_packet);// get the received packet
// check address // check address
id=dyn_get_id(inst_packet); if(version==DYN_VER1)
if(id==dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
{ {
// check the packet checksum id=dyn_get_id(inst_packet);
if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay if(id==dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
{ {
// process the packet // check the packet checksum
switch(dyn_get_instruction(inst_packet)) if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
{ {
case DYN_PING: if(id!=DYN_BROADCAST_ID) // process the packet
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); switch(dyn_get_instruction(inst_packet))
break; {
case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data); case DYN_PING: if(id!=DYN_BROADCAST_ID)
if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID) dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
{ break;
if(error==RAM_SUCCESS) case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data); if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID)
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
break;
case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
error=write_operation(dyn_get_write_address(inst_packet),length,data);
if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
{
if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
break;
case DYN_REG_WRITE: reg_length=dyn_get_reg_write_data(inst_packet,reg_data);
reg_address=dyn_get_reg_write_address(inst_packet);
reg_op=0x01;// signal there is a registered operation pending
if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_ACTION: if(reg_op)
{ {
error=write_operation(reg_address,reg_length,reg_data); if(error==RAM_SUCCESS)
reg_op=0x00; dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
} }
break; break;
case DYN_RESET: case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
break; error=write_operation(dyn_get_write_address(inst_packet),length,data);
case DYN_SYNC_READ: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
break; {
case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(inst_packet,dyn_slave_get_address()))// the device is addressed if(error==RAM_SUCCESS)
{ dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
length=dyn_get_sync_write_data(inst_packet,dyn_slave_get_address(),data); else
error=write_operation(dyn_get_sync_write_address(inst_packet),length,data); dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
} }
break; break;
case DYN_BULK_READ: if(dyn_bulk_read_id_present(inst_packet,dyn_slave_get_address(),&prev_id)) case DYN_REG_WRITE: reg_length=dyn_get_reg_write_data(inst_packet,reg_data);
{ reg_address=dyn_get_reg_write_address(inst_packet);
length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address()); reg_op=0x01;// signal there is a registered operation pending
error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data); if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
if(prev_id==0xFF)// first device in the bulk read sequence dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
{ break;
if(error==RAM_SUCCESS) case DYN_ACTION: if(reg_op)
dyn_slave_send_status_packet(DYN_NO_ERROR,length,data); {
else error=write_operation(reg_address,reg_length,reg_data);
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); reg_op=0x00;
} }
else// wait for the previous device in the sequence to send its data break;
case DYN_RESET:
break;
case DYN_SYNC_READ: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
break;
case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(inst_packet,dyn_slave_get_address()))// the device is addressed
{
length=dyn_get_sync_write_data(inst_packet,dyn_slave_get_address(),data);
error=write_operation(dyn_get_sync_write_address(inst_packet),length,data);
}
break;
case DYN_BULK_READ: if(dyn_bulk_read_id_present(inst_packet,dyn_slave_get_address(),&prev_id))
{ {
do{ length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address());
while(!dyn_slave_is_packet_ready());// wait until a new packet is received error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data);
dyn_slave_get_inst_packet(inst_packet); if(prev_id==0xFF)// first device in the bulk read sequence
}while(dyn_get_id(inst_packet)!=prev_id); {
if(error==RAM_SUCCESS) if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,length,data); dyn_slave_send_status_packet(DYN_NO_ERROR,length,data);
else else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
else// wait for the previous device in the sequence to send its data
{
do{
while(!dyn_slave_is_packet_ready());// wait until a new packet is received
dyn_slave_get_inst_packet(inst_packet);
}while(dyn_get_id(inst_packet)!=prev_id);
if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,length,data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
} }
} break;
break; case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); break;
break; default:
default: break;
break; }
}
else
{
// send a checksum error answer
if(dyn_get_id(inst_packet)!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
} }
} }
else if(id!=dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to another device
{ {
// send a checksum error answer // send the incomming packet to the dynamixel bus
if(dyn_get_id(inst_packet)!=DYN_BROADCAST_ID) if(dyn_master_resend_inst_packet(inst_packet,status_packet)!=DYN_TIMEOUT)
dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00); {
// send the answer back to the computer
dyn_slave_resend_status_packet(status_packet);
}
} }
} }
else// the packet is addressed to another device else// version 2
{ {
// send the incomming packet to the dynamixel bus
if(dyn_master_resend_inst_packet(inst_packet,status_packet)!=DYN_TIMEOUT)
{
// send the answer back to the computer
dyn_slave_resend_status_packet(status_packet);
}
} }
} }
} }
...@@ -264,7 +240,7 @@ void comm_init(void) ...@@ -264,7 +240,7 @@ void comm_init(void)
RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE); RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE);
TIM_TimeBaseStructure.TIM_Period = 1000; TIM_TimeBaseStructure.TIM_Period = 1000;
TIM_TimeBaseStructure.TIM_Prescaler = 84; TIM_TimeBaseStructure.TIM_Prescaler = 42;
TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure); TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment