diff --git a/include/dynamixel_master_uart_dma.h b/include/dynamixel_master_uart_dma.h
index 68bb562482d655470b4ca2582154098a59274bbd..c96f7b1c97dd0661f0a9da1ae5cbcb33b0e138d8 100755
--- a/include/dynamixel_master_uart_dma.h
+++ b/include/dynamixel_master_uart_dma.h
@@ -19,7 +19,6 @@ uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint
uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
void dyn_master_action(void);
void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data);
-uint8_t dyn_master_reset(uint8_t id);
// repeater functions
uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
diff --git a/include/dynamixel_slave_uart_dma.h b/include/dynamixel_slave_uart_dma.h
index 65454ddbdd1f018750ea88a81cd191267d5f3d2c..4652a1be75fcc2981b5ad398d701aa61874e3e4b 100644
--- a/include/dynamixel_slave_uart_dma.h
+++ b/include/dynamixel_slave_uart_dma.h
@@ -6,8 +6,11 @@
// public functions
void dyn_slave_init(void);
-void dyn_slave_set_address(uint8_t id);
-uint8_t dyn_slave_get_address(void);
+inline void dyn_slave_set_address(uint8_t id);
+inline uint8_t dyn_slave_get_address(void);
+inline void dyn_slave_set_return_delay(uint8_t delay);
+inline void dyn_slave_set_return_level(uint8_t level);
+inline uint8_t dyn_slave_get_return_level(void);
uint8_t dyn_slave_is_packet_ready(void);
void dyn_slave_get_inst_packet(uint8_t *packet);
void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data);
diff --git a/include/motion_pages.h b/include/motion_pages.h
index 66b0c5fff81b8e31cbb42ed0cbe7f46e298459c5..ef6f772f0437fa494d32c5b0f632ef749d7672a3 100644
--- a/include/motion_pages.h
+++ b/include/motion_pages.h
@@ -3,7 +3,7 @@
#include "stm32f4xx.h"
-#define MAX_PAGES 256
+#define MAX_PAGES 1
#define PAGE_MAX_NUM_SERVOS 31
#define POSE_NUMBER_OF_POSES_PER_PAGE 7
diff --git a/include/ram.h b/include/ram.h
index 126d156b410ce24a43dcafe7250b3f3ced400afb..b8d8efe6bb1454b0509bb8f1cf5424c5409c3d83 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -2,9 +2,40 @@
#define _RAM_H
#include "stm32f4xx.h"
+#include "eeprom.h"
-extern uint8_t ram_data[256];
+#define RAM_SUCCESS 0
+#define RAM_BAD_ADDRESS -1
+#define RAM_WRITE_ERROR -2
+#define RAM_BAD_BIT -3
+
+typedef enum {
+ BIOLOID_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
+ BIOLOID_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
+ BIOLOID_VERSION = FIRMWARE_VERSION_OFFSET,
+ BIOLOID_ID = DEVICE_ID_OFFSET,
+ BIOLOID_BAUD_RATE = BAUDRATE_OFFSET,
+ BIOLOID_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
+ BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
+ BIOLOID_DXL_POWER = 0x18,
+ BIOLOID_LED = 0x19,
+ BIOLOID_PUSHBUTTON = 0x1A,
+ BIOLOID_MM_NUM_SERVOS = 0x1B
+} bioloid_registers;
+
+#define RAM_SIZE 256
+
+extern uint8_t ram_data[RAM_SIZE];
void ram_init(void);
+inline void ram_read_byte(uint8_t address, uint8_t *data);
+inline void ram_read_word(uint8_t address, uint16_t *data);
+uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data);
+uint8_t ram_set_bit(uint8_t address, uint8_t bit);
+uint8_t ram_clear_bit(uint8_t address, uint8_t bit);
+uint8_t ram_write_byte(uint8_t address, uint8_t data);
+uint8_t ram_write_word(uint8_t address, uint16_t data);
+uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data);
+inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length);
#endif
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 84ab749109bf0cae40d07a2466a802f4ec2118f7..0734158e0069bc93123c06e1d4fca96e8a58f55a 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -15,34 +15,56 @@
uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
{
- uint8_t i;
- uint16_t eeprom_data;
+ uint8_t error;
- if(address >= DEVICE_MODEL_OFFSET && address <= RETURN_LEVEL_OFFSET ) // EEPROM region
- {
- for(i=0;i<length;i++)
- {
- if(EE_ReadVariable(address+i,&eeprom_data)==1)
- return DYN_INST_ERROR;
- else
- data[i]=(uint8_t)eeprom_data;
- }
- return DYN_NO_ERROR;
- }
- else// RAM region
+ // pre-read operation
+ // read operation
+ error=ram_read_table(address,length,data);
+ // post-read operation
+ if(ram_in_range(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,value;
+
+ // pre-write operation
+ if(ram_in_range(BIOLOID_LED,address,length))
{
- for(i=0;i<length;i++)
- data[i]=ram_data[address+i];
- return DYN_NO_ERROR;
- }
+ value=data[address-BIOLOID_LED];
+ if(value&0x01)
+ gpio_set_led(NORTH_LED);
+ else
+ gpio_clear_led(NORTH_LED);
+ if(value&0x02)
+ gpio_set_led(SOUTH_LED);
+ else
+ gpio_clear_led(SOUTH_LED);
+ if(value&0x04)
+ gpio_set_led(EAST_LED);
+ else
+ gpio_clear_led(EAST_LED);
+ if(value&0x08)
+ gpio_set_led(WEST_LED);
+ else
+ gpio_clear_led(WEST_LED);
+ }
+ // write operation
+ error=ram_write_table(address,length,data);
+ // post-write operation
+
+ return error;
}
int32_t main(void)
{
uint8_t inst_packet[1024];
uint8_t status_packet[1024];
- uint8_t data[1024],error;
- uint16_t address;
+ uint8_t data[1024],error,length;
+ uint16_t eeprom_data;
/* initialize EEPROM */
EE_Init();
@@ -50,19 +72,23 @@ int32_t main(void)
ram_init();
/* initialize the 1ms system timer */
time_init();
+ /* initialize the gpio */
+ gpio_init();
/* initialize the dynamixel master interface */
dyn_master_init();
dyn_master_set_timeout(20);
/* initialize the dynamixel slave interface*/
dyn_slave_init();
- EE_ReadVariable(DEVICE_ID_OFFSET,&address);
- dyn_slave_set_address((uint8_t)address);
+ EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
+ dyn_slave_set_address((uint8_t)eeprom_data);
+ EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data);
+ dyn_slave_set_return_delay((uint8_t)eeprom_data);
+ EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
+ dyn_slave_set_return_level((uint8_t)eeprom_data);
/* initialize the IMU */
- imu_init();
- /* initialize the gpio */
- gpio_init();
+// imu_init();
// initialize the Analog to digital converter
- adc_init();
+// adc_init();
// initialize motion manager
manager_init(7800);
@@ -83,12 +109,25 @@ int32_t main(void)
case DYN_PING: 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(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(error,dyn_get_read_length(inst_packet),data);
- else
- dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ if(dyn_slave_get_return_level()!=0)
+ {
+ 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:
+ case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
+ if(length!=DYN_BAD_FORMAT)
+ error=write_operation(dyn_get_write_address(inst_packet),length,data);
+ if(dyn_slave_get_return_level()==2)
+ {
+ if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+
break;
default:
break;
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
index d00178c78760f4c943e02e2d92fad74b6cd1a3ce..8abdfcaa9ecbd800671ea7086fe4fd1ca1014a1d 100755
--- a/src/dynamixel_master_uart_dma.c
+++ b/src/dynamixel_master_uart_dma.c
@@ -1,5 +1,6 @@
#include "dynamixel_master_uart_dma.h"
#include "time.h"
+#include "gpio.h"
#define USART USART2
#define USART_CLK RCC_APB1Periph_USART2
@@ -73,6 +74,19 @@ DMA_InitTypeDef DMA_TX_InitStructure;
DMA_InitTypeDef DMA_RX_InitStructure;
// private functions
+void dyn_master_reset(void)
+{
+ DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,DISABLE);
+ USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
+ DMA_Cmd(USART_RX_DMA_STREAM,DISABLE);
+ /* wait until the transaction ends */
+ while(DMA_GetCmdStatus(USART_RX_DMA_STREAM)==ENABLE);
+ DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+ DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+ dyn_master_receiving_header=0x01;
+ dyn_master_packet_ready=0x00;
+}
+
void dyn_master_send(void)
{
// wait until any previous transmission ends
@@ -95,7 +109,10 @@ uint8_t dyn_master_receive(void)
delay_ms(10);
timeout_left-=10;
if(timeout_left<=0)
+ {
+ dyn_master_reset();
return DYN_TIMEOUT;
+ }
}
dyn_master_packet_ready=0x00;
// check the input packet checksum
@@ -144,10 +161,9 @@ uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
// wait for the replay within the given timeout
error=dyn_master_receive();
if(error==DYN_NO_ERROR)
- {
- // read the data from the status packet
- dyn_get_status_data(dyn_master_rx_buffer,data);
- }
+ dyn_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
+ else
+ dyn_master_reset();
return error;
}
else
@@ -180,6 +196,8 @@ uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *d
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
+ if(error!=DYN_NO_ERROR)
+ dyn_master_reset();
return error;
}
else
@@ -493,12 +511,6 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len
// send the data
dyn_master_send();
// wait for the transmission to end
-// while(dyn_master_sending_packet);
-}
-
-uint8_t dyn_master_reset(uint8_t id)
-{
- return DYN_SUCCESS;
}
// repeater functions
diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
index a32011a0d89f180d9b787eca10966963d293aa06..6782d65dfe5f3b9eebeb78ba83fce319decaf417 100644
--- a/src/dynamixel_slave_uart_dma.c
+++ b/src/dynamixel_slave_uart_dma.c
@@ -45,9 +45,10 @@
#define DYN_SLAVE_DMA_RX_IRQHandler DMA1_Stream1_IRQHandler
#define MAX_BUFFER_LEN 1024
-
// private variables
uint8_t dyn_slave_address;// this module slave address
+uint8_t dyn_slave_return_level;// type of response
+uint8_t dyn_slave_return_delay;// delay in th response
// input buffer
uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
volatile uint8_t dyn_slave_receiving_header;
@@ -232,16 +233,31 @@ void dyn_slave_init(void)
USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
}
-void dyn_slave_set_address(uint8_t id)
+inline void dyn_slave_set_address(uint8_t id)
{
dyn_slave_address=id;
}
-uint8_t dyn_slave_get_address(void)
+inline uint8_t dyn_slave_get_address(void)
{
return dyn_slave_address;
}
+inline void dyn_slave_set_return_delay(uint8_t delay)
+{
+ dyn_slave_return_delay=delay;
+}
+
+inline void dyn_slave_set_return_level(uint8_t level)
+{
+ dyn_slave_return_level=level;
+}
+
+inline uint8_t dyn_slave_get_return_level(void)
+{
+ return dyn_slave_return_level;
+}
+
uint8_t dyn_slave_is_packet_ready(void)
{
return dyn_slave_packet_ready;
diff --git a/src/gpio.c b/src/gpio.c
index a5c60edecca04ef793dd0273c52e8bf6596773af..19d621fac682a6529c9074da99beabbf485d2ed5 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,4 +1,5 @@
#include "gpio.h"
+#include "ram.h"
#define LED1_GPIO_CLK RCC_AHB1Periph_GPIOE
#define LED1_PIN GPIO_Pin_2
@@ -81,6 +82,7 @@ void GPI_EXTI1_IRQHandler(void)
{
if(north_pb_callback!=0)
north_pb_callback();
+ ram_set_bit(BIOLOID_PUSHBUTTON,0);
/* Clear the EXTI line 0 pending bit */
EXTI_ClearITPendingBit(PUSH_BUTTON1_EXTI_LINE);
}
@@ -92,6 +94,7 @@ void GPI_EXTI2_IRQHandler(void)
{
if(south_pb_callback!=0)
south_pb_callback();
+ ram_set_bit(BIOLOID_PUSHBUTTON,1);
/* Clear the EXTI line 0 pending bit */
EXTI_ClearITPendingBit(PUSH_BUTTON2_EXTI_LINE);
}
@@ -99,6 +102,7 @@ void GPI_EXTI2_IRQHandler(void)
{
if(east_pb_callback!=0)
east_pb_callback();
+ ram_set_bit(BIOLOID_PUSHBUTTON,2);
/* Clear the EXTI line 0 pending bit */
EXTI_ClearITPendingBit(PUSH_BUTTON3_EXTI_LINE);
}
@@ -106,6 +110,7 @@ void GPI_EXTI2_IRQHandler(void)
{
if(west_pb_callback!=0)
west_pb_callback();
+ ram_set_bit(BIOLOID_PUSHBUTTON,3);
/* Clear the EXTI line 0 pending bit */
EXTI_ClearITPendingBit(PUSH_BUTTON4_EXTI_LINE);
}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index b7042885f285fab8459a97a878019ce110ca5f47..2f1b4d172b2051f4a83230a9e2ede2d9db764335 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -2,6 +2,7 @@
#include "dynamixel_master_uart_dma.h"
#include "dyn_servos.h"
#include "gpio.h"
+#include "ram.h"
#define MOTION_TIMER TIM3
#define MOTION_TIMER_IRQn TIM3_IRQn
@@ -72,7 +73,7 @@ void MOTION_TIMER_IRQHandler(void)
// balance the robot
// manager_balance();
// send the motion commands to the servos
- manager_send_motion_command();
+ // manager_send_motion_command();
}
}
@@ -83,14 +84,14 @@ void manager_init(uint16_t period_us)
TIM_OCInitTypeDef TIM_OCInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
uint8_t servo_ids[MAX_NUM_SERVOS];
+ uint8_t i,num=0;
uint16_t model;
- uint8_t i,num;
RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE);
// detect the servos connected
- manager_num_servos=0;
dyn_master_scan(&num,servo_ids);
+ ram_data[BIOLOID_MM_NUM_SERVOS]=num;
for(i=0;i<MAX_NUM_SERVOS;i++)
{
if(i<num)
@@ -172,7 +173,7 @@ void manager_init(uint16_t period_us)
else
{
manager_servos[i].id=servo_ids[i];
- manager_servos[i].model=model;
+ manager_servos[i].model=0x0000;
manager_servos[i].module=MM_NONE;
manager_servos[i].encoder_resolution=0;
manager_servos[i].gear_ratio=0;
@@ -183,6 +184,7 @@ void manager_init(uint16_t period_us)
manager_servos[i].current_angle=0;
}
}
+ manager_num_servos=num;
// initialize the timer interrupts
NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
diff --git a/src/motion_pages.c b/src/motion_pages.c
index ae8536cabacafc4a808a59668c7b94d95bf95848..673c9b26fcef8988ba7ecbf63ada8891f99c757d 100644
--- a/src/motion_pages.c
+++ b/src/motion_pages.c
@@ -21,7 +21,8 @@ uint8_t pages_check_checksum(TPage *page)
return 0x00;
}
-TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages")))=
+TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages")));
+/*TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages")))=
{
{
{
@@ -14558,4 +14559,4 @@ TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages")))=
}
}
}
-};
+};*/
diff --git a/src/ram.c b/src/ram.c
index 860bea4738c0c92823b09e262d8fb350fde4bcbe..b0d5119d3521d6891a9537528dbb97b81e5c6f77 100644
--- a/src/ram.c
+++ b/src/ram.c
@@ -1,8 +1,114 @@
#include "ram.h"
-uint8_t ram_data[256];
+uint8_t ram_data[RAM_SIZE];
void ram_init(void)
{
+ uint16_t eeprom_data;
+
// read contents from EEPROM to RAM
+ if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
+ ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0)
+ ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0)
+ ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
+ ram_data[DEVICE_ID_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0)
+ 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(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
+ ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
+}
+
+inline void ram_read_byte(uint8_t address,uint8_t *data)
+{
+ (*data)=ram_data[address];
+}
+
+inline void ram_read_word(uint8_t address, uint16_t *data)
+{
+ (*data)=ram_data[address];
+ (*data)+=ram_data[address+1]*256;
+}
+
+uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data)
+{
+ uint8_t i;
+
+ if((address+length)<=(RAM_SIZE-1))
+ {
+ for(i=0;i<length;i++)
+ data[i]=ram_data[address+i];
+ return RAM_SUCCESS;
+ }
+ else
+ return RAM_BAD_ADDRESS;
+}
+
+uint8_t ram_set_bit(uint8_t address, uint8_t bit)
+{
+ if(bit>=0 && bit<8)
+ {
+ ram_data[address]|=(0x01<<bit);
+ return RAM_SUCCESS;
+ }
+ else
+ return RAM_BAD_BIT;
+}
+
+uint8_t ram_clear_bit(uint8_t address, uint8_t bit)
+{
+ if(bit>=0 && bit<8)
+ {
+ ram_data[address]&=(~(0x01<<bit));
+ return RAM_SUCCESS;
+ }
+ else
+ return RAM_BAD_BIT;
}
+
+uint8_t ram_write_byte(uint8_t address, uint8_t data)
+{
+ ram_data[address]=data;
+
+ return RAM_SUCCESS;
+}
+
+uint8_t ram_write_word(uint8_t address, uint16_t data)
+{
+ if(address < (RAM_SIZE-1))
+ {
+ ram_data[address]=data%256;
+ ram_data[address+1]=data/256;
+
+ return RAM_SUCCESS;
+ }
+ else
+ return RAM_BAD_ADDRESS;
+}
+
+uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data)
+{
+ uint8_t i;
+
+ if((address+length)<RAM_SIZE)
+ {
+ for(i=0;i<length;i++)
+ data[i]=ram_data[address+i];
+ return RAM_SUCCESS;
+ }
+ else
+ return RAM_BAD_ADDRESS;
+}
+
+inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length)
+{
+ if(reg>=address && reg<=(address+length))
+ return 0x01;
+ else
+ return 0x00;
+}
+