diff --git a/src/comm.c b/src/comm.c
index f96d8c08768ae007437a032ade8d9f9f8a8d0247..ddafaa71adfc31979b6b8bc2fa9573378a483b9c 100644
--- a/src/comm.c
+++ b/src/comm.c
@@ -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);