diff --git a/include/gpio.h b/include/gpio.h
new file mode 100644
index 0000000000000000000000000000000000000000..9031bf666bc01fd74840f84397d4e0da3a6d13ce
--- /dev/null
+++ b/include/gpio.h
@@ -0,0 +1,15 @@
+#ifndef _GPIO_H
+#define _GPIO_H
+
+#include "stm32f4xx.h"
+
+typedef enum {NORTH_LED,SOUTH_LED,EAST_LED,WEST_LED} led_t;
+
+typedef enum {NORTH_PB,SOUTH_PB,EAST_PB,WEST_PB} pushbutton_t;
+
+void gpio_init(void);
+void gpio_set_led(led_t led_id);
+void gpio_clear_led(led_t led_id);
+void gpio_blink_led(led_t led_id, int16_t period_ms);
+
+#endif
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index a4595d926154b004f3ddbeb40c0f60d56e912b8c..3fb2476a3f87cb8443e8f3aab408892d899cded8 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -7,6 +7,7 @@
#include "dynamixel_slave_spi.h"
#include "eeprom.h"
#include "imu_9dof_dma.h"
+#include "gpio.h"
uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
{
@@ -49,34 +50,30 @@ int32_t main(void)
GPIO_Init(GPIOD, &GPIO_InitStructure);
/* initialize EEPROM */
-// EE_Init();
+ EE_Init();
/* initialize the 1ms system timer */
time_init();
/* initialize the dynamixel master interface */
-// dyn_master_init();
-// dyn_master_set_timeout(20);
+ 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);
-
+ dyn_slave_init();
+ EE_ReadVariable(DEVICE_ID_OFFSET,&address);
+ dyn_slave_set_address((uint8_t)address);
+ /* initialize the IMU */
imu_init();
+ /* initialize the gpio */
+ gpio_init();
- if(imu_detect_accel())
+ if(imu_accel_detect())
GPIO_SetBits(GPIOD,GPIO_Pin_12);
else
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
- while(1)
- {
- imu_config_accel();
-// if(dyn_master_ping(1))
-// GPIO_SetBits(GPIOD,GPIO_Pin_12);
-// else
-// GPIO_ResetBits(GPIOD,GPIO_Pin_12);
- GPIO_ToggleBits(GPIOD,GPIO_Pin_15);
- delay_ms(1000);
- }
-
+
+ gpio_blink_led(NORTH_LED,1000);
+ gpio_blink_led(SOUTH_LED,2000);
+ gpio_blink_led(EAST_LED,3000);
+ gpio_blink_led(WEST_LED,4000);
while(1) /* main function does not return */
{
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
diff --git a/src/gpio.c b/src/gpio.c
new file mode 100644
index 0000000000000000000000000000000000000000..9aa05b7b7666b0f60cf9ea3d8eca65962a025ee9
--- /dev/null
+++ b/src/gpio.c
@@ -0,0 +1,251 @@
+#include "gpio.h"
+
+#define LED1_GPIO_CLK RCC_AHB1Periph_GPIOE
+#define LED1_PIN GPIO_Pin_2
+#define LED1_GPIO_PORT GPIOE
+#define LED1_SOURCE GPIO_PinSource2
+
+#define LED2_GPIO_CLK RCC_AHB1Periph_GPIOE
+#define LED2_PIN GPIO_Pin_3
+#define LED2_GPIO_PORT GPIOE
+#define LED2_SOURCE GPIO_PinSource3
+
+#define LED3_GPIO_CLK RCC_AHB1Periph_GPIOE
+#define LED3_PIN GPIO_Pin_4
+#define LED3_GPIO_PORT GPIOE
+#define LED3_SOURCE GPIO_PinSource4
+
+#define LED4_GPIO_CLK RCC_AHB1Periph_GPIOE
+#define LED4_PIN GPIO_Pin_5
+#define LED4_GPIO_PORT GPIOE
+#define LED4_SOURCE GPIO_PinSource5
+
+#define PUSH_BUTTON1_GPIO_CLK RCC_AHB1Periph_GPIOE
+#define PUSH_BUTTON1_PIN GPIO_Pin_6
+#define PUSH_BUTTON1_GPIO_PORT GPIOE
+#define PUSH_BUTTON1_SOURCE GPIO_PinSource6
+
+#define PUSH_BUTTON2_GPIO_CLK RCC_AHB1Periph_GPIOC
+#define PUSH_BUTTON2_PIN GPIO_Pin_13
+#define PUSH_BUTTON2_GPIO_PORT GPIOC
+#define PUSH_BUTTON2_SOURCE GPIO_PinSource13
+
+#define PUSH_BUTTON3_GPIO_CLK RCC_AHB1Periph_GPIOC
+#define PUSH_BUTTON3_PIN GPIO_Pin_14
+#define PUSH_BUTTON3_GPIO_PORT GPIOC
+#define PUSH_BUTTON3_SOURCE GPIO_PinSource14
+
+#define PUSH_BUTTON4_GPIO_CLK RCC_AHB1Periph_GPIOC
+#define PUSH_BUTTON4_PIN GPIO_Pin_15
+#define PUSH_BUTTON4_GPIO_PORT GPIOC
+#define PUSH_BUTTON4_SOURCE GPIO_PinSource15
+
+#define GPIO_TIMER TIM2
+#define GPIO_TIMER_CLK RCC_APB1Periph_TIM2
+#define GPIO_TIMER_IRQn TIM2_IRQn
+#define GPIO_TIMER_IRQHandler TIM2_IRQHandler
+
+// private variables
+__IO uint16_t north_led_period;
+__IO uint16_t south_led_period;
+__IO uint16_t east_led_period;
+__IO uint16_t west_led_period;
+
+// IRQ hanfler functions
+void TIM2_IRQHandler(void)
+{
+ uint16_t capture;
+
+ if(TIM_GetITStatus(GPIO_TIMER, TIM_IT_CC1)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPIO_TIMER,TIM_IT_CC1);
+ GPIO_ToggleBits(LED1_GPIO_PORT,LED1_PIN);
+ capture = TIM_GetCapture1(GPIO_TIMER);
+ TIM_SetCompare1(GPIO_TIMER, capture + north_led_period);
+ }
+ if(TIM_GetITStatus(GPIO_TIMER, TIM_IT_CC2)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPIO_TIMER,TIM_IT_CC2);
+ GPIO_ToggleBits(LED2_GPIO_PORT,LED2_PIN);
+ capture = TIM_GetCapture2(GPIO_TIMER);
+ TIM_SetCompare2(GPIO_TIMER, capture + south_led_period);
+ }
+ if(TIM_GetITStatus(GPIO_TIMER, TIM_IT_CC3)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPIO_TIMER,TIM_IT_CC3);
+ GPIO_ToggleBits(LED3_GPIO_PORT,LED3_PIN);
+ capture = TIM_GetCapture3(GPIO_TIMER);
+ TIM_SetCompare3(GPIO_TIMER, capture + east_led_period);
+ }
+ if(TIM_GetITStatus(GPIO_TIMER, TIM_IT_CC4)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPIO_TIMER,TIM_IT_CC4);
+ GPIO_ToggleBits(LED4_GPIO_PORT,LED4_PIN);
+ capture = TIM_GetCapture4(GPIO_TIMER);
+ TIM_SetCompare4(GPIO_TIMER, capture + west_led_period);
+ }
+}
+
+// private functions
+
+// public functions
+void gpio_init(void)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ /* enable clocks */
+ RCC_AHB1PeriphClockCmd(LED1_GPIO_CLK | LED2_GPIO_CLK | LED3_GPIO_CLK | LED4_GPIO_CLK, ENABLE);
+ RCC_AHB1PeriphClockCmd(PUSH_BUTTON1_GPIO_CLK | PUSH_BUTTON2_GPIO_CLK | PUSH_BUTTON3_GPIO_CLK | PUSH_BUTTON4_GPIO_CLK, ENABLE);
+ RCC_APB1PeriphClockCmd(GPIO_TIMER_CLK,ENABLE);
+
+ /* GPIO Configuration */
+ GPIO_InitStructure.GPIO_Pin = LED1_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ GPIO_Init(LED1_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED2_PIN;
+ GPIO_Init(LED2_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED3_PIN;
+ GPIO_Init(LED3_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED4_PIN;
+ GPIO_Init(LED4_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON1_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ GPIO_Init(PUSH_BUTTON1_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON2_PIN;
+ GPIO_Init(PUSH_BUTTON2_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON3_PIN;
+ GPIO_Init(PUSH_BUTTON3_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON4_PIN;
+ GPIO_Init(PUSH_BUTTON4_GPIO_PORT, &GPIO_InitStructure);
+
+ // initialize the timer interrupts
+ NVIC_InitStructure.NVIC_IRQChannel = GPIO_TIMER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* Time base configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseInit(GPIO_TIMER,&TIM_TimeBaseStructure);
+ TIM_Cmd(GPIO_TIMER, ENABLE);
+ TIM_PrescalerConfig(GPIO_TIMER, 42000, TIM_PSCReloadMode_Immediate);
+ TIM_SetClockDivision(GPIO_TIMER,TIM_CKD_DIV2);
+}
+
+void gpio_set_led(led_t led_id)
+{
+ switch(led_id)
+ {
+ case NORTH_LED:
+ GPIO_SetBits(LED1_GPIO_PORT,LED1_PIN);
+ break;
+ case SOUTH_LED:
+ GPIO_SetBits(LED2_GPIO_PORT,LED2_PIN);
+ break;
+ case EAST_LED:
+ GPIO_SetBits(LED3_GPIO_PORT,LED3_PIN);
+ break;
+ case WEST_LED:
+ GPIO_SetBits(LED4_GPIO_PORT,LED4_PIN);
+ break;
+ }
+}
+
+void gpio_clear_led(led_t led_id)
+{
+ switch(led_id)
+ {
+ case NORTH_LED:
+ GPIO_ResetBits(LED1_GPIO_PORT,LED1_PIN);
+ break;
+ case SOUTH_LED:
+ GPIO_ResetBits(LED2_GPIO_PORT,LED2_PIN);
+ break;
+ case EAST_LED:
+ GPIO_ResetBits(LED3_GPIO_PORT,LED3_PIN);
+ break;
+ case WEST_LED:
+ GPIO_ResetBits(LED4_GPIO_PORT,LED4_PIN);
+ break;
+ }
+}
+
+void gpio_blink_led(led_t led_id, int16_t period_ms)
+{
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ switch(led_id)
+ {
+ case NORTH_LED:
+ if(period_ms>1)
+ {
+ north_led_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = north_led_period;
+ TIM_OC1Init(GPIO_TIMER, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(GPIO_TIMER, TIM_OCPreload_Disable);
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC1, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC1, DISABLE);
+ break;
+ case SOUTH_LED:
+ if(period_ms>1)
+ {
+ south_led_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = south_led_period;
+ TIM_OC2Init(GPIO_TIMER, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(GPIO_TIMER, TIM_OCPreload_Disable);
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC2, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC2, DISABLE);
+ break;
+ case EAST_LED:
+ if(period_ms>1)
+ {
+ east_led_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = east_led_period;
+ TIM_OC3Init(GPIO_TIMER, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(GPIO_TIMER, TIM_OCPreload_Disable);
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC3, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC3, DISABLE);
+ break;
+ case WEST_LED:
+ if(period_ms>1)
+ {
+ west_led_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = west_led_period;
+ TIM_OC4Init(GPIO_TIMER, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(GPIO_TIMER, TIM_OCPreload_Disable);
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC4, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPIO_TIMER, TIM_IT_CC4, DISABLE);
+ break;
+ }
+}