add Encoder and PID control and Abstacle control

fun/encoder.c

@@ -0,0 +1,119 @@
+#include "encoder.h"
+
+volatile int32_t tick_L = 0,tick_R = 0;
+volatile int16_t rpm_L = 0,rpm_R = 0;
+
+extern TIM_HandleTypeDef htim1;
+extern TIM_HandleTypeDef htim8;
+
+static TX_THREAD enc_thread;
+static UCHAR enc_stack[512]; 
+
+static void enc_thread_entry(ULONG);
+
+UINT Encoder_ThreadCreate(void)
+{
+	UINT status;
+	
+	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
+	HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
+	
+	__HAL_TIM_SET_COUNTER(&htim1,0);
+	__HAL_TIM_SET_COUNTER(&htim8,0);
+	
+    status = tx_thread_create(&enc_thread, "Encoder", enc_thread_entry, 0,
+                     enc_stack, sizeof(enc_stack),
+                     6, 6, TX_NO_TIME_SLICE, TX_AUTO_START);
+					 
+	return status;
+}
+
+
+static void enc_thread_entry(ULONG arg)
+{
+	const float msPerMin = 6000.0f;
+	const float ticksPerRev = 1024.0f; // <20><><EFBFBD><EFBFBD>  * 4
+	uint16_t lastL = 0, lastR = 0;
+	
+    while (1)
+    {
+        uint16_t cntL = __HAL_TIM_GET_COUNTER(&htim1);
+        uint16_t cntR = __HAL_TIM_GET_COUNTER(&htim8);
+
+        int16_t dL = (int16_t)(cntL - lastL);
+        int16_t dR = (int16_t)(cntR - lastR);
+
+        lastL = cntL;
+        lastR = cntR;
+
+        tick_L += dL;
+        tick_R += dR;
+
+        rpm_L = (int16_t)(dL * msPerMin / (ticksPerRev * 10));
+        rpm_R = (int16_t)(dR * msPerMin / (ticksPerRev * 10));
+
+        tx_thread_sleep(10);
+    }
+	
+}
+
+
+//
+
+//ControlThread
+//
+
+static TX_THREAD control_thread;
+static UCHAR control_stack[512];
+
+volatile int16_t target_rpm_L = 0;
+volatile int16_t target_rpm_R = 0;
+
+static void control_thread_entry(ULONG arg);
+
+
+UINT ControlThreadCreate(void)
+{
+	UINT status;
+    status = tx_thread_create(&control_thread, "Control", control_thread_entry, 0,
+                     control_stack, sizeof(control_stack),
+                     7, 7, TX_NO_TIME_SLICE, TX_AUTO_START);
+					 
+	return status;
+}
+
+
+static void control_thread_entry(ULONG arg)
+{
+    int16_t pwmL = 0, pwmR = 0;
+    float integralL = 0, integralR = 0;
+    float Kp = 1.0f, Ki = 0.1f;
+
+    while (1)
+    {
+        int16_t errorL = target_rpm_L - rpm_L;
+        int16_t errorR = target_rpm_R - rpm_R;
+
+        integralL += errorL;
+        integralR += errorR;
+
+        pwmL += (int16_t)(Kp * errorL + Ki * integralL);
+        pwmR += (int16_t)(Kp * errorR + Ki * integralR);
+
+        if (pwmL < 0) pwmL = 0;
+        if (pwmL > 255) pwmL = 255;
+        if (pwmR < 0) pwmR = 0;
+        if (pwmR > 255) pwmR = 255;
+
+        DriveBOTH(pwmL, pwmR);
+
+        tx_thread_sleep(10);
+    }
+}
+
+int16_t map_speed_to_rpm(int speed)
+{
+    if (speed < 60) return 0;
+    return (speed - 60) * 1.5;  // <20><><EFBFBD><EFBFBD>
+}
+