tmp

robot/modules/sc60224/encoder.cpp

@@ -10,6 +10,11 @@ uint32_t test_buf[TEST_BUF_LEN];
 
 #define RESOLUTION      1024
 
+uint8_t direction;
+uint32_t forw_cnt;
+uint32_t back_cnt;
+uint32_t forw_turns;
+uint32_t back_turns;
 
 
 //
@@ -110,11 +115,30 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
 
 void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
 {
+    if (htim->Instance->CR1 & TIM_CR1_DIR) {
+        direction = 1;
+        back_cnt++;
+        if (back_cnt >= RESOLUTION) {
+            back_cnt = 0;
+            back_turns++;
+        }
+    }
+    else {
+        direction = 0;
+        forw_cnt++;
+        if (forw_cnt >= RESOLUTION) {
+            forw_cnt = 0;
+            forw_turns++;
+        }
+    }
+    
+#if 0    
     //test_buf[test_index++] = (uint16_t)TIM1->CNT;
     test_buf[test_index++] = (uint16_t)TIM1->CCR1;
     
     if (test_index == TEST_BUF_LEN)
         test_index = 0;
+#endif
 }