diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 0dd14ed1245c9a2d2e65a905469a2234fb3b1557..24c164a4c60208b38af1d95164f57ef41d1150e2 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -10,8 +10,8 @@ extern "C" {
#define DEVICE_ID_OFFSET ((unsigned short int)0x0003)
#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
+#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
-#define MM_PERIOD_OFFSET ((unsigned short int)0x0010)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
@@ -196,8 +196,8 @@ typedef enum {
#define ZIGBEE_BASE_ADDRESS 0x42
#define ZIGBEE_MEM_LENGTH 6
-#define MANAGER_BASE_ADDRESS 0x42
-#define MANAGER_MEM_LENGTH 6
+#define MANAGER_BASE_ADDRESS 0x48
+#define MANAGER_MEM_LENGTH 50
#ifdef __cplusplus
}
diff --git a/include/comm.h b/include/comm.h
deleted file mode 100644
index 41b57cea13369f6093435d0d0354714d6f69c1ca..0000000000000000000000000000000000000000
--- a/include/comm.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef _COMM_H
-#define _COMM_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f4xx.h"
-
-// public functions
-void comm_init(void);
-void comm_start(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index 8e28e2d382d72dca9bb8ec9e6bca3bc64ee88088..4278c010f4b0be431f6dd663fc1f581bbf2652cf 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -147,12 +147,12 @@ void bioloid_dyn_slave_init(void)
DYN_SLAVE_TIMER_ENABLE_CLK;
bioloid_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
bioloid_dyn_slave_tim_handle.Init.Period = 1000;
- bioloid_dyn_slave_tim_handle.Init.Prescaler = 42;
+ bioloid_dyn_slave_tim_handle.Init.Prescaler = 84;
bioloid_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
bioloid_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&bioloid_dyn_slave_tim_handle);
// initialize the timer interrupts
- HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 3, 2);
+ HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 0);
HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
}
diff --git a/src/comm.c b/src/comm.c
deleted file mode 100644
index ddafaa71adfc31979b6b8bc2fa9573378a483b9c..0000000000000000000000000000000000000000
--- a/src/comm.c
+++ /dev/null
@@ -1,263 +0,0 @@
-#include "comm.h"
-#include "gpio.h"
-#include "ram.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "dynamixel_slave_uart_dma.h"
-#include "dynamixel_master_uart_dma.h"
-#include "adc_dma.h"
-
-#define COMM_TIMER TIM7
-#define COMM_TIMER_IRQn TIM7_IRQn
-#define COMM_TIMER_IRQHandler TIM7_IRQHandler
-#define COMM_TIMER_CLK RCC_APB1Periph_TIM7
-
-// private variables
-uint8_t inst_packet[MAX_DATA_LENGTH];
-uint8_t status_packet[MAX_DATA_LENGTH];
-uint8_t data[MAX_DATA_LENGTH];
-// registered write operation variables
-uint8_t reg_data[MAX_DATA_LENGTH];
-uint8_t reg_address,reg_length,reg_op;
-
-// private functions
-uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // pre-read operation
- // read operation
- error=ram_read_table(address,length,data);
- // post-read operation
- if(ram_in_range(BIOLOID_PUSHBUTTON,BIOLOID_PUSHBUTTON,address,length))
- ram_data[BIOLOID_PUSHBUTTON]=0x00;
-
- return error;
-}
-
-uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
-{
- uint8_t error=RAM_SUCCESS,byte_value,i,j,module;
- uint16_t word_value;
-
- // pre-write operation
- if(ram_in_range(BIOLOID_DXL_POWER,BIOLOID_DXL_POWER,address,length))
- {
- if(data[BIOLOID_DXL_POWER-address]&0x01) dyn_master_enable_power();
- else dyn_master_disable_power();
- }
- if(ram_in_range(BIOLOID_LED,BIOLOID_LED,address,length))
- {
- byte_value=data[BIOLOID_LED-address];
- if(byte_value&0x01)
- gpio_set_led(NORTH_LED);
- else
- gpio_clear_led(NORTH_LED);
- if(byte_value&0x02)
- gpio_set_led(SOUTH_LED);
- else
- gpio_clear_led(SOUTH_LED);
- if(byte_value&0x04)
- gpio_set_led(EAST_LED);
- else
- gpio_clear_led(EAST_LED);
- if(byte_value&0x08)
- gpio_set_led(WEST_LED);
- else
- gpio_clear_led(WEST_LED);
- }
- if(ram_in_range(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length))
- {
- word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8);
- manager_set_period(word_value);
- }
- for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
- {
- if(ram_in_range(i,i,address,length))
- {
- byte_value=data[i-address];
- if(byte_value&0x80) manager_enable_servo(j);
- else manager_disable_servo(j);
- module=(byte_value&0x70)>>4;
- manager_set_module(j,module);
- if(byte_value&0x08) manager_enable_servo(j+1);
- else manager_disable_servo(j+1);
- module=byte_value&0x07;
- manager_set_module(j+1,module);
- }
- }
- if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length))
- {
- if(data[BIOLOID_MM_CONTROL-address]&0x01)
- manager_enable();
- else
- manager_disable();
- }
- if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
- {
- if(data[BIOLOID_ACTION_CONTROL-address]&0x01)
- action_start_page();
- if(data[BIOLOID_ACTION_CONTROL-address]&0x02)
- action_stop_page();
- }
- if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier
- action_load_page(data[BIOLOID_ACTION_PAGE-address]);
- // write eeprom
- for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
- EE_WriteVariable(i,data[j]);
- // write operation
- // error=ram_write_table(address,length,data);
- // post-write operation
-
- return error;
-}
-
-// interrupt handlers
-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
- {
- version=dyn_slave_get_inst_packet(inst_packet);// get the received packet
- // check address
- if(version==DYN_VER1)
- {
- 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
- {
- // check the packet checksum
- if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
- {
- // 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)
- {
- 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)
- {
- 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))
- {
- 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;
- }
- }
- 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);
- }
- }
- if(id!=dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to another device
- {
- // 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// version 2
- {
- }
- }
-}
-
-// public functions
-void comm_init(void)
-{
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE);
-
- TIM_TimeBaseStructure.TIM_Period = 1000;
- TIM_TimeBaseStructure.TIM_Prescaler = 42;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure);
-
- NVIC_InitStructure.NVIC_IRQChannel = COMM_TIMER_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- /* initialize variables */
- reg_op=0x00;
-}
-
-void comm_start(void)
-{
- TIM_Cmd(COMM_TIMER, ENABLE);
- TIM_ITConfig(COMM_TIMER, TIM_IT_Update, ENABLE);
-}
-
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 0841cbac0f9cc97f44bf35010e89c9948853e8b5..64118953ac57ef9f130a4b80e1770f486560dc5e 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -2,6 +2,7 @@
#include "motion_manager.h"
#include "dyn_servos.h"
#include "ram.h"
+#include "gpio.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
@@ -126,6 +127,7 @@ void MANAGER_TIMER_IRQHandler(void)
// manager_send_motion_command();
// get the disabled servos position
// manager_get_current_position();
+ gpio_toggle_led(RXD_LED);
}
}
}
@@ -261,10 +263,10 @@ void manager_init(uint16_t period_us)
ENABLE_MANAGER_TIMER_CLK;
MANAGER_TIM_Handle.Instance=MANAGER_TIMER;
MANAGER_TIM_Handle.Init.Period = 0xFFFF;
- MANAGER_TIM_Handle.Init.Prescaler = 42;
+ MANAGER_TIM_Handle.Init.Prescaler = 84;
MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
- HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 0);
+ HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
/* use the internal clock */
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
@@ -291,7 +293,8 @@ uint16_t manager_get_period(void)
void manager_set_period(uint16_t period_us)
{
manager_motion_period=(period_us*1000000)>>16;
- ram_data[BIOLOID_MM_PERIOD_L]=period_us;
+ ram_data[BIOLOID_MM_PERIOD_L]=period_us&0x00FF;
+ ram_data[BIOLOID_MM_PERIOD_H]=(period_us&0xFF00)>>8;
}
inline void manager_enable(void)
diff --git a/src/ram.c b/src/ram.c
index cce0279314fb3f2a000ae4c4a025556e4433469f..7d49db96fccbe5d383208610011af1fb552f5454 100644
--- a/src/ram.c
+++ b/src/ram.c
@@ -22,6 +22,10 @@ void ram_init(void)
ram_data[BAUDRATE_OFFSET]=(uint8_t)eeprom_data;
if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
ram_data[RETURN_DELAY_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
+ ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
+ ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
}