From 695e75d0819ac5628c4559caa87182c8734f5d5c Mon Sep 17 00:00:00 2001
From: EN <none>
Date: Thu, 11 Jul 2024 18:07:41 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=9B=9B=E8=BD=AE=E7=94=B5?=
 =?UTF-8?q?=E6=9C=BA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Core/Inc/motor.h | 15 +++++++++
 Core/Src/motor.c | 82 ++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 97 insertions(+)
 create mode 100644 Core/Inc/motor.h
 create mode 100644 Core/Src/motor.c

diff --git a/Core/Inc/motor.h b/Core/Inc/motor.h
new file mode 100644
index 0000000..5eaf124
--- /dev/null
+++ b/Core/Inc/motor.h
@@ -0,0 +1,15 @@
+#ifndef __MOTOR_H
+#define __MOTOR_H
+
+// 控制电机转动
+
+#include "main.h"
+
+// duty in [-100, 100]
+
+void MOTOR_Start(void);
+void MOTOR_Slip(void);
+void MOTOR_Stop(void);
+void MOTOR_SetDuty(int8_t leftDuty, int8_t rightDuty);
+
+#endif
diff --git a/Core/Src/motor.c b/Core/Src/motor.c
new file mode 100644
index 0000000..f1b8041
--- /dev/null
+++ b/Core/Src/motor.c
@@ -0,0 +1,82 @@
+#include "motor.h"
+#include "gpio.h"
+#include "tim.h"
+
+#define MAX_SPEED 100
+#define MOTOR_L 1
+#define MOTOR_R 2
+#define MOTOR_TIM (&htim4)
+#define MOTOR_L_CHAN TIM_CHANNEL_2
+#define MOTOR_R_CHAN TIM_CHANNEL_1
+
+int8_t preProcess(int8_t duty, uint8_t motor);
+
+void MOTOR_Start(void)
+{
+	HAL_TIM_PWM_Start(MOTOR_TIM, MOTOR_L_CHAN);
+	HAL_TIM_PWM_Start(MOTOR_TIM, MOTOR_R_CHAN);
+}
+
+void MOTOR_Slip(void)
+{
+	HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin | L_BACKWARD_Pin | R_FORWARD_Pin | R_BACKWARD_Pin, GPIO_PIN_RESET);
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, MAX_SPEED);
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, MAX_SPEED);
+}
+
+void MOTOR_Stop(void)
+{
+	HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin | L_BACKWARD_Pin | R_FORWARD_Pin | R_BACKWARD_Pin, GPIO_PIN_RESET);
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, 0);
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, 0);
+}
+
+void MOTOR_SetDuty(int8_t leftDuty, int8_t rightDuty)
+{
+	leftDuty = preProcess(leftDuty, MOTOR_L);
+	rightDuty = preProcess(rightDuty, MOTOR_R);
+
+	uint16_t leftSpeed = (uint32_t)MAX_SPEED * leftDuty / 100;
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, leftSpeed);
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, leftSpeed);
+
+	uint16_t rightSpeed = (uint32_t)MAX_SPEED * rightDuty / 100;
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, rightSpeed);
+	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, rightSpeed);
+}
+
+int8_t preProcess(int8_t duty, uint8_t motor)
+{
+	if (duty > 100)
+		duty = 100;
+	else if (duty < -100)
+		duty = -100;
+
+	if (duty >= 0)
+	{
+		if (motor & MOTOR_L)
+		{
+			HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_SET);
+			HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_RESET);
+		}
+		if (motor & MOTOR_R)
+		{
+			HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_SET);
+			HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_RESET);
+		}
+	}
+	else
+	{
+		if (motor & MOTOR_L)
+		{
+			HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_RESET);
+			HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_SET);
+		}
+		if (motor & MOTOR_R)
+		{
+			HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_RESET);
+			HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_SET);
+		}
+	}
+	return duty;
+}