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
Hide whitespace changes
Inline Side-by-side
Showing
with 94 additions and 118 deletions
src/comm.c
+ 94
− 118
View file @ b5d6ada2
......@@ -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 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;
// pre-write operation
......@@ -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)
{
do_op=0x01;
if(ram_in_range(i,i,address,length))
{
byte_value=data[i-address];
......@@ -87,11 +86,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
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(data[BIOLOID_MM_CONTROL-address]&0x01)
......@@ -99,22 +93,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
else
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(data[BIOLOID_ACTION_CONTROL-address]&0x01)
......@@ -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
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
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
......@@ -147,110 +116,117 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
void COMM_TIMER_IRQHandler(void)
{
uint8_t error,length,prev_id,id;
TDynVersion version;
TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update);
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
id=dyn_get_id(inst_packet);
if(id==dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
if(version==DYN_VER1)
{
// check the packet checksum
if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
id=dyn_get_id(inst_packet);
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
switch(dyn_get_instruction(inst_packet))
// check the packet checksum
if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
{
case DYN_PING: if(id!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID)
{
if(error==RAM_SUCCESS)
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;
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)
// process the packet
switch(dyn_get_instruction(inst_packet))
{
case DYN_PING: if(id!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID)
{
error=write_operation(reg_address,reg_length,reg_data);
reg_op=0x00;
if(error==RAM_SUCCESS)
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;
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))
{
length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address());
error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data);
if(prev_id==0xFF)// first device in the bulk read sequence
{
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);
}
else// wait for the previous device in the sequence to send its data
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);
reg_op=0x00;
}
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{
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);
length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address());
error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data);
if(prev_id==0xFF)// first device in the bulk read sequence
{
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);
}
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;
case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
break;
default:
break;
break;
case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
break;
default:
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
if(dyn_get_id(inst_packet)!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
// 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);
}
}
}
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)
RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE);
TIM_TimeBaseStructure.TIM_Period = 1000;
TIM_TimeBaseStructure.TIM_Prescaler = 84;
TIM_TimeBaseStructure.TIM_Prescaler = 42;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure);
......
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