Added a module to handle the GPIO.

include/gpio.h

+#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

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

src/gpio.c

+#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;
+  }
+}