diff --git a/boards/DJI_Board_TypeC/Core/Src/tim.c b/boards/DJI_Board_TypeC/Core/Src/tim.c
index 745dfd2..d617137 100644
--- a/boards/DJI_Board_TypeC/Core/Src/tim.c
+++ b/boards/DJI_Board_TypeC/Core/Src/tim.c
@@ -82,6 +82,14 @@ void MX_TIM1_Init(void)
{
Error_Handler();
}
+ if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
+ {
+ Error_Handler();
+ }
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
@@ -316,9 +324,11 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
/* USER CODE END TIM1_MspPostInit 0 */
__HAL_RCC_GPIOE_CLK_ENABLE();
/**TIM1 GPIO Configuration
+ PE13 ------> TIM1_CH3
PE9 ------> TIM1_CH1
+ PE11 ------> TIM1_CH2
*/
- GPIO_InitStruct.Pin = GPIO_PIN_9;
+ GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_9|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
diff --git a/boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc b/boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc
index 6e26d9d..6637cc4 100644
--- a/boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc
+++ b/boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc
@@ -321,6 +321,16 @@ PD14.GPIO_PuPd=GPIO_PULLUP
PD14.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PD14.Locked=true
PD14.Signal=S_TIM4_CH3
+PE11.GPIOParameters=GPIO_Speed,GPIO_PuPd
+PE11.GPIO_PuPd=GPIO_PULLUP
+PE11.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
+PE11.Locked=true
+PE11.Signal=S_TIM1_CH2
+PE13.GPIOParameters=GPIO_Speed,GPIO_PuPd
+PE13.GPIO_PuPd=GPIO_PULLUP
+PE13.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
+PE13.Locked=true
+PE13.Signal=S_TIM1_CH3
PE9.GPIOParameters=GPIO_Speed,GPIO_PuPd
PE9.GPIO_PuPd=GPIO_PULLUP
PE9.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
@@ -461,6 +471,10 @@ SH.S_TIM10_CH1.0=TIM10_CH1,PWM Generation1 CH1
SH.S_TIM10_CH1.ConfNb=1
SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1
SH.S_TIM1_CH1.ConfNb=1
+SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2
+SH.S_TIM1_CH2.ConfNb=1
+SH.S_TIM1_CH3.0=TIM1_CH3,PWM Generation3 CH3
+SH.S_TIM1_CH3.ConfNb=1
SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
SH.S_TIM4_CH3.ConfNb=1
SH.S_TIM5_CH1.0=TIM5_CH1,PWM Generation1 CH1
@@ -478,7 +492,9 @@ SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler
SPI1.Mode=SPI_MODE_MASTER
SPI1.VirtualType=VM_MASTER
TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
-TIM1.IPParameters=Channel-PWM Generation1 CH1,Period,Prescaler
+TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
+TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
+TIM1.IPParameters=Channel-PWM Generation1 CH1,Period,Prescaler,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3
TIM1.Period=0
TIM1.Prescaler=167
TIM10.Channel=TIM_CHANNEL_1
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 6de8444..b5b2f04 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -32,6 +32,7 @@ add_subdirectory(turret)
add_subdirectory(uart)
add_subdirectory(usb)
add_subdirectory(shooter)
+add_subdirectory(servo)
add_subdirectory(rtos)
add_subdirectory(laser)
add_subdirectory(ultrasonic)
diff --git a/examples/servo/CMakeLists.txt b/examples/servo/CMakeLists.txt
new file mode 100644
index 0000000..5b5f318
--- /dev/null
+++ b/examples/servo/CMakeLists.txt
@@ -0,0 +1,29 @@
+# ---------------------------------------------------------------------- #
+# #
+# Copyright (C) 2022 #
+# Illini RoboMaster @ University of Illinois at Urbana-Champaign. #
+# #
+# This program is free software: you can redistribute it and/or modify #
+# it under the terms of the GNU General Public License as published by #
+# the Free Software Foundation, either version 3 of the License, or #
+# (at your option) any later version. #
+# #
+# This program is distributed in the hope that it will be useful, #
+# but WITHOUT ANY WARRANTY; without even the implied warranty of #
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the #
+# GNU General Public License for more details. #
+# #
+# You should have received a copy of the GNU General Public License #
+# along with this program. If not, see . #
+# #
+# ---------------------------------------------------------------------- #
+
+project(example_servo ASM C CXX)
+
+irm_add_arm_executable(${PROJECT_NAME}_test
+ TARGET DJI_Board_TypeC
+ SOURCES test.cc)
+
+irm_add_arm_executable(${PROJECT_NAME}
+ TARGET DJI_Board_TypeC
+ SOURCES main.cc)
diff --git a/examples/servo/main.cc b/examples/servo/main.cc
new file mode 100644
index 0000000..6e0aaaf
--- /dev/null
+++ b/examples/servo/main.cc
@@ -0,0 +1,48 @@
+/****************************************************************************
+ * *
+ * Copyright (C) 2022 RoboMaster. *
+ * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
+ * *
+ * This program is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program. If not, see . *
+ * *
+ ****************************************************************************/
+
+#include "main.h"
+
+#include "bsp_pwm.h"
+#include "cmsis_os.h"
+#include "servo.h"
+
+#define PWM_CHANNEL 2
+
+control::Servo* servo;
+
+void RM_RTOS_Init(void) { servo = new control::Servo(&htim1, PWM_CHANNEL); }
+
+void RM_RTOS_Default_Task(const void* args) {
+ UNUSED(args);
+ servo->Start();
+ while (1) {
+ servo->SetAngle(0.0);
+ osDelay(1000);
+ servo->SetAngle(30.0);
+ osDelay(1000);
+ servo->SetAngle(60.0);
+ osDelay(1000);
+ servo->SetAngle(90.0);
+ osDelay(1000);
+ servo->SetAngle(120.0);
+ osDelay(1000);
+ }
+}
diff --git a/examples/servo/test.cc b/examples/servo/test.cc
new file mode 100644
index 0000000..dc60993
--- /dev/null
+++ b/examples/servo/test.cc
@@ -0,0 +1,60 @@
+/****************************************************************************
+ * *
+ * Copyright (C) 2022 RoboMaster. *
+ * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
+ * *
+ * This program is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program. If not, see . *
+ * *
+ ****************************************************************************/
+
+#include "bsp_pwm.h"
+#include "cmsis_os.h"
+#include "main.h"
+
+#define PWM_CHANNEL 2
+#define TIM_CLOCK_FREQ 1000000
+#define MOTOR_OUT_FREQ 50
+#define INIT_PULSE_WIDTH 600
+
+uint32_t pulse_width = INIT_PULSE_WIDTH;
+
+bsp::PWM* motor1;
+bsp::PWM* motor2;
+
+void RM_RTOS_Init(void) {
+ motor1 = new bsp::PWM(&htim1, PWM_CHANNEL, TIM_CLOCK_FREQ, MOTOR_OUT_FREQ, INIT_PULSE_WIDTH);
+ motor2 = new bsp::PWM(&htim1, PWM_CHANNEL + 1, TIM_CLOCK_FREQ, MOTOR_OUT_FREQ, INIT_PULSE_WIDTH);
+}
+
+void RM_RTOS_Default_Task(const void* args) {
+ UNUSED(args);
+ motor1->Start();
+ motor2->Start();
+ while (1) {
+ // motor1->SetPulseWidth(pulse_width);
+ // motor2->SetPulseWidth(pulse_width);
+ // osDelay(500);
+ // if (pulse_width == INIT_PULSE_WIDTH) {
+ // osDelay(1000);
+ //}
+ // pulse_width += 100;
+ // if (pulse_width > 2400) {
+ // pulse_width = INIT_PULSE_WIDTH;
+ //}
+ motor2->SetPulseWidth(600);
+ osDelay(1500);
+ motor2->SetPulseWidth(2100);
+ osDelay(1500);
+ }
+}
diff --git a/shared/libraries/servo.cc b/shared/libraries/servo.cc
new file mode 100644
index 0000000..80987b6
--- /dev/null
+++ b/shared/libraries/servo.cc
@@ -0,0 +1,54 @@
+/****************************************************************************
+ * *
+ * Copyright (C) 2022 RoboMaster. *
+ * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
+ * *
+ * This program is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program. If not, see . *
+ * *
+ ****************************************************************************/
+
+#include "servo.h"
+
+#include "bsp_pwm.h"
+#include "cmsis_os.h"
+#include "utils.h"
+
+namespace control {
+
+Servo::Servo(TIM_HandleTypeDef* htim, uint8_t channel) {
+ servo = new bsp::PWM(htim, channel, TIM_CLOCK_FREQ, MOTOR_OUT_FREQ, INIT_PULSE_WIDTH);
+}
+
+Servo::Servo(TIM_HandleTypeDef* htim, uint8_t channel, uint32_t clock_freq, uint32_t output_freq,
+ uint32_t pulse_width)
+ : pulse_width(pulse_width) {
+ servo = new bsp::PWM(htim, channel, clock_freq, output_freq, pulse_width);
+}
+
+Servo::~Servo() {
+ delete servo;
+ servo = nullptr;
+}
+void Servo::Start() { servo->Start(); }
+
+void Servo::Stop() { servo->Stop(); }
+
+void Servo::SetAngle(double _angle) {
+ _angle = clip(_angle, MIN_ANGLE, MAX_ANGLE);
+ uint32_t p_width = static_cast(_angle * ANGLE_TO_WIDTH);
+ p_width += BASE_WIDTH;
+ servo->SetPulseWidth(p_width);
+}
+
+} // namespace control
diff --git a/shared/libraries/servo.h b/shared/libraries/servo.h
new file mode 100644
index 0000000..40a7489
--- /dev/null
+++ b/shared/libraries/servo.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ * *
+ * Copyright (C) 2022 RoboMaster. *
+ * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
+ * *
+ * This program is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program. If not, see . *
+ * *
+ ****************************************************************************/
+
+#pragma once
+
+#include "bsp_pwm.h"
+#include "cmsis_os.h"
+
+constexpr double MIN_ANGLE = 0.0;
+constexpr double MAX_ANGLE = 120.0;
+constexpr uint32_t BASE_WIDTH = 600;
+// control pulse width = 600 - 2100.
+constexpr double ANGLE_TO_WIDTH = 1.0 / 120 * 1500;
+constexpr uint32_t TIM_CLOCK_FREQ = 1000000;
+constexpr uint32_t MOTOR_OUT_FREQ = 50;
+constexpr uint32_t INIT_PULSE_WIDTH = 600;
+
+namespace control {
+
+class Servo {
+ public:
+ Servo(TIM_HandleTypeDef* htim, uint8_t channel);
+ Servo(TIM_HandleTypeDef* htim, uint8_t channel, uint32_t clock_freq, uint32_t output_freq,
+ uint32_t pulse_width);
+ ~Servo();
+ void Start();
+ void Stop();
+ void SetAngle(double _angle);
+
+ private:
+ bsp::PWM* servo;
+ uint32_t pulse_width;
+};
+
+} // namespace control