Add servo library for Type C board

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;

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

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)

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 <http://www.gnu.org/licenses/>.  #
+#                                                                        #
+# ---------------------------------------------------------------------- #
+
+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)

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 <http://www.gnu.org/licenses/>.    *
+ *                                                                          *
+ ****************************************************************************/
+
+#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);
+  }
+}

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 <http://www.gnu.org/licenses/>.    *
+ *                                                                          *
+ ****************************************************************************/
+
+#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);
+  }
+}

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 <http://www.gnu.org/licenses/>.    *
+ *                                                                          *
+ ****************************************************************************/
+
+#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<double>(_angle, MIN_ANGLE, MAX_ANGLE);
+  uint32_t p_width = static_cast<uint32_t>(_angle * ANGLE_TO_WIDTH);
+  p_width += BASE_WIDTH;
+  servo->SetPulseWidth(p_width);
+}
+
+}  // namespace control

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 <http://www.gnu.org/licenses/>.    *
+ *                                                                          *
+ ****************************************************************************/
+
+#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