Added a second scheduler with a higher priority for the dynamixel slave loop.

include/darwin_sch2.h

+#ifndef _DARWIN_SCH2_H
+#define _DARWIN_SCH2_H
+
+#include "stm32f1xx.h"
+#include "scheduler.h"
+
+TScheduler *darwin_sch2_init(void);
+
+#endif
+

src/darwin_sch2.c

+#include "darwin_sch2.h"
+
+/* private variables */
+TIM_HandleTypeDef darwin_sch2_timer_handle;
+TScheduler darwin_scheduler2;
+
+/* interrupt handlers */
+void TIM5_IRQHandler(void)
+{
+  /* Capture compare 1 event */
+  if(__HAL_TIM_GET_FLAG(&darwin_sch2_timer_handle, TIM_FLAG_CC1) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch2_timer_handle, TIM_IT_CC1) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&darwin_sch2_timer_handle, TIM_IT_CC1);
+      __HAL_TIM_CLEAR_FLAG(&darwin_sch2_timer_handle,TIM_FLAG_CC1);
+      scheduler_interrupt(&darwin_scheduler2,SCHED_CH1);
+    }
+  }
+  /* Capture compare 2 event */
+  if(__HAL_TIM_GET_FLAG(&darwin_sch2_timer_handle, TIM_FLAG_CC2) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch2_timer_handle, TIM_IT_CC2) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&darwin_sch2_timer_handle, TIM_IT_CC2);
+      __HAL_TIM_CLEAR_FLAG(&darwin_sch2_timer_handle,TIM_FLAG_CC2);
+      scheduler_interrupt(&darwin_scheduler2,SCHED_CH2);
+    }
+  }
+  /* Capture compare 3 event */
+  if(__HAL_TIM_GET_FLAG(&darwin_sch2_timer_handle, TIM_FLAG_CC3) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch2_timer_handle, TIM_IT_CC3) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&darwin_sch2_timer_handle, TIM_IT_CC3);
+      __HAL_TIM_CLEAR_FLAG(&darwin_sch2_timer_handle,TIM_FLAG_CC3);
+      scheduler_interrupt(&darwin_scheduler2,SCHED_CH3);
+    }
+  }
+  /* Capture compare 4 event */
+  if(__HAL_TIM_GET_FLAG(&darwin_sch2_timer_handle, TIM_FLAG_CC4) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch2_timer_handle, TIM_IT_CC4) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&darwin_sch2_timer_handle, TIM_IT_CC4);
+      __HAL_TIM_CLEAR_FLAG(&darwin_sch2_timer_handle,TIM_FLAG_CC4);
+      scheduler_interrupt(&darwin_scheduler2,SCHED_CH4);
+    }
+  }
+}
+
+/* private functions */
+void darwin_sch2_start(unsigned short int channel_id)
+{
+  HAL_TIM_OC_Start_IT(&darwin_sch2_timer_handle,channel_id);
+}
+
+void darwin_sch2_stop(unsigned short int channel_id)
+{
+  HAL_TIM_OC_Stop_IT(&darwin_sch2_timer_handle,channel_id); 
+}
+
+void darwin_sch2_set_pulse(unsigned short int channel_id,unsigned short int pulse, unsigned char running)
+{
+  TIM_OC_InitTypeDef out_comp_config;
+  unsigned short int capture;
+
+  if(running==0x00)
+  {
+    if(darwin_scheduler2.channels[scheduler_get_id(channel_id)].enabled==0x00)
+    {
+      out_comp_config.OCMode = TIM_OCMODE_TIMING;
+      capture = HAL_TIM_ReadCapturedValue(&darwin_sch2_timer_handle,channel_id);
+      out_comp_config.Pulse = capture+pulse;
+      out_comp_config.OCPolarity = TIM_OCPOLARITY_HIGH;
+      out_comp_config.OCFastMode = TIM_OCFAST_DISABLE;
+      HAL_TIM_OC_ConfigChannel(&darwin_sch2_timer_handle, &out_comp_config, channel_id);
+    }
+  }
+  else
+  {
+    capture = HAL_TIM_ReadCapturedValue(&darwin_sch2_timer_handle, channel_id);
+    __HAL_TIM_SetCompare(&darwin_sch2_timer_handle,channel_id, capture+pulse);
+  } 
+}
+
+/* public functions */
+TScheduler *darwin_sch2_init(void)
+{
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  __HAL_RCC_TIM5_CLK_ENABLE();
+
+  darwin_sch2_timer_handle.Instance = TIM5;
+  darwin_sch2_timer_handle.Init.Prescaler = 72;
+  darwin_sch2_timer_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+  darwin_sch2_timer_handle.Init.Period = 0xFFFF;
+  darwin_sch2_timer_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  HAL_TIM_Base_Init(&darwin_sch2_timer_handle);
+
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  HAL_TIM_ConfigClockSource(&darwin_sch2_timer_handle, &sClockSourceConfig);
+
+  HAL_TIM_OC_Init(&darwin_sch2_timer_handle);
+  
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  HAL_TIMEx_MasterConfigSynchronization(&darwin_sch2_timer_handle, &sMasterConfig);
+
+  /* Peripheral interrupt init */
+  HAL_NVIC_SetPriority(TIM5_IRQn, 2, 0);
+  HAL_NVIC_EnableIRQ(TIM5_IRQn);
+
+  scheduler_init(&darwin_scheduler2,4,72);
+  darwin_scheduler2.start=darwin_sch2_start;
+  darwin_scheduler2.stop=darwin_sch2_stop;
+  darwin_scheduler2.set_pulse=darwin_sch2_set_pulse;
+
+  return &darwin_scheduler2;
+}