From b79309bad81e8ee433dc2d579429512060faa572 Mon Sep 17 00:00:00 2001 From: youhy Date: Wed, 15 Jun 2022 20:31:03 -0700 Subject: [PATCH 1/5] servo test 1.0.0 --- examples/CMakeLists.txt | 1 + examples/servo/.main.cc.swp | Bin 0 -> 12288 bytes examples/servo/.typeC.cc.swp | Bin 0 -> 12288 bytes examples/servo/CMakeLists.txt | 29 ++++++++++++++++++ examples/servo/main.cc | 53 +++++++++++++++++++++++++++++++++ examples/servo/typeC.cc | 54 ++++++++++++++++++++++++++++++++++ shared/libraries/servo.cc | 51 ++++++++++++++++++++++++++++++++ shared/libraries/servo.h | 46 +++++++++++++++++++++++++++++ 8 files changed, 234 insertions(+) create mode 100644 examples/servo/.main.cc.swp create mode 100644 examples/servo/.typeC.cc.swp create mode 100644 examples/servo/CMakeLists.txt create mode 100644 examples/servo/main.cc create mode 100644 examples/servo/typeC.cc create mode 100644 shared/libraries/servo.cc create mode 100644 shared/libraries/servo.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 9198c2f..381db05 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/.main.cc.swp b/examples/servo/.main.cc.swp new file mode 100644 index 0000000000000000000000000000000000000000..b337f9363421f7c59b7512841607c2f70ab4b11a GIT binary patch literal 12288 zcmeI2&2Jk;7{;d@_$VzPDskX|URnt`k!w3CMKpnen>dYyV~6#c5*1;My<_jF>ltfz z)+U9*rQ*T?4v0U1LnSzH;0#FIxNrc0dgB5|jz}Eg*PgW*PbuU&lkm8d@u||RE$J8(!~Lns?hE3ZEPbf0ZU*X1WvMZ^K+#n z@XYC_>D1$`eT=aomVhN-30MM_fF)oFSOS)SCGdYCV1~!odsxhUlLeihe2?AnomB0| z60ig;0ZYIVummgtOTZGa1S|ndz!Iab3TYvw5`6y!^^aS+X zBaB^#^3X3Q8T$yj1pRS>v0KpV&~fOihZ$24K|da6>?U*`dKP*LIsyIp5M#eVze3+b zUqBy228y6JpsUbJ(Cr5q`x*KPx&?g#-GoNaRcIAD3q1+_h5Ubqet^D#K7~GnK7g)6 z$@*f*7E8bqummgtOW@xlaD#{urLWx1UToqqTyAC&4L2Yn9W9H1Z|3GorBd2FjTEM= z`hm#J&6f&WeftQ%aG>bt4itUvK+$P%vi7kr13|gD8G0MFZ4x{*8H!fD)vPS%c-W7? zV5Fr-crNI;jb?jU^mrVYw#%byxvo}`p`?G7(qQ$xDBV>Xt|SmvtF*&+STP+rR}ad z4cDzz?o6zzq{$_zU{Tm$wtL#yETu)fz4Am)DnWO#qzk3ecK_}a==`*2ce0e66qN0O zI~GQG*m*{(ZV-EdPNz@`{^=b}ooLt|jt9FN)0daK!n%Vag1hvtb0MYYUsvbN0a zh!#4@?H-f%bVvgo3i5>EGKkU@{nPAOZfg}Mxe#voQnglfHwX?ZRkvPgHfg2dP?^@t zj$2)7)yfX7x19AxvjW#9U?u>jC3EYPaLv6$|RMz0tNl@677 z_bWkk%)^i?lQP}5%iP=@8%fwxPevw`m;=LP=HlyNk_k5?4dw^}>IjM>(Tjup9d6sz zL;jHEmcx{I+#He~8mZL`akWVSHzK5w2qQGz6aID%nOnkgB?Q!_?imn4O~=aPiFKX? zPW9B6&TSpP$yv)Mo0gT#4Ja@eBNAI=G>EoF+~e$U+?nJEM?UCwHWROb@30FImecw=SX@-*b={ubFx1FhF<~DUd+)uNtc!a-u GXW8F2BWWT4 literal 0 HcmV?d00001 diff --git a/examples/servo/.typeC.cc.swp b/examples/servo/.typeC.cc.swp new file mode 100644 index 0000000000000000000000000000000000000000..27c1c29736f33dcc360e85e3a1074b45abce4361 GIT binary patch literal 12288 zcmeI2&2Jk;7{;d@fR+{zNF32iDdDs<>BOH6Ru)3NTZ}8#B6Jb6S!M4il$w-uPz23pjriUS52<%HB(*4utPp~tq zD}}hNJ{r&&-V~jb_ zQ_zo(GWI5vh5k5!{?KLU=5fZpfL?=+LEk>Ym=6(j;}|@lbI@7nY3Mlg&r!zyhW>=U zhrWb9fZm5*his?@RiRaA1^Vq_#(srcypiiI= zq4%IogE@Zv;)n45{)7+6!us;+NFzBsaGt!*{Qb6_GV?> zx_Tk$EBlmL?01Q9xu>v1ZzZjZEzeyfw4|fbd5BcFE<8TTprNpIXTyuP&pdOVjXpOY zJXBf*E2l5Dg>J^dZ^p4N#+OVocBLmMvy%GE$7mktOwyrK@3hP786FHm)VGy%2=@eA zR-mBW;^)hgHlsa^Tn9D;b?8Gxmn9R>uNb%$r3)> zrK`nyyExXaNruAF7Bci<_ReY&4aS-u(*le=pX$65Q?mO>%uMOW+5T0TD>k8=Y*%Bf(G_he7_A#R{v%EbYtZd)p%&{dJ=kju9P1)?Y9 zws443PmLy|`j|z~m3oJ+2ww!;qh{2F6;)Ha>Dp4aLt5xGwtXh;s{s2j5abBWr57eE z`mfp3*lOigd^W7&ERx zmej2qY1*txj7&;(o*?L#Qh0* zT#JCVMG)fEzVPQUq;4_GjQ~*Fs;@x=H5K^|kFB#Lu&Z}{>C9~VP4`+hPFh-0H=w{^ zjEHR!(J-8i*k|u>oSAqFhc4)LC$Zn1v(S&@45x9f#So~+eH@Y9^u)>FzLQV3qa*tZ zZqq>T`jN&~@jU5EYN@WO@i2z}5_No>RAeCCQb`Ym&4YY(f-d*@>5_{(=Ufi_-5^r8 m5{`Bt2d<_}X^GbC2Ccb}G7{t&-=m)XDcQEH?B literal 0 HcmV?d00001 diff --git a/examples/servo/CMakeLists.txt b/examples/servo/CMakeLists.txt new file mode 100644 index 0000000..b9077dc --- /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}_typeC + TARGET DJI_Board_TypeC + SOURCES typeC.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..29ef09a --- /dev/null +++ b/examples/servo/main.cc @@ -0,0 +1,53 @@ +/**************************************************************************** + * * + * 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 "cmsis_os.h" +#include "main.h" +#include "bsp_pwm.h" +#include "servo.h" + +#define PWM_CHANNEL 1 +#define TIM_CLOCK_FREQ 1000000 +#define MOTOR_OUT_FREQ 50 +#define SNAIL_IDLE_THROTTLE 900 + +control::Servo* servo; + +void RM_RTOS_Init(void) { + servo = new control::Servo(&htim1, PWM_CHANNEL, TIM_CLOCK_FREQ, MOTOR_OUT_FREQ, SNAIL_IDLE_THROTTLE); +} + +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); + } + servo->Stop(); +} diff --git a/examples/servo/typeC.cc b/examples/servo/typeC.cc new file mode 100644 index 0000000..b46224b --- /dev/null +++ b/examples/servo/typeC.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 "cmsis_os.h" +#include "main.h" +#include "bsp_pwm.h" + +#define PWM_CHANNEL 1 +#define TIM_CLOCK_FREQ 1000000 +#define MOTOR_OUT_FREQ 50 +#define INIT_PULSE_WIDTH 900 + +uint32_t pulse_width = INIT_PULSE_WIDTH; + +bsp::PWM* motor1; + +void RM_RTOS_Init(void) { + + motor1 = new bsp::PWM(&htim1, PWM_CHANNEL, TIM_CLOCK_FREQ, MOTOR_OUT_FREQ, INIT_PULSE_WIDTH); + +} + +void RM_RTOS_Default_Task(const void* args) { + UNUSED(args); + motor1->Start(); + while (1) { + motor1->SetPulseWidth(pulse_width); + osDelay(500); + if (pulse_width == INIT_PULSE_WIDTH) { + osDelay(1000); + } + pulse_width += 100; + if (pulse_width > 2100) { + pulse_width = INIT_PULSE_WIDTH; + } + } +} diff --git a/shared/libraries/servo.cc b/shared/libraries/servo.cc new file mode 100644 index 0000000..1e38a6a --- /dev/null +++ b/shared/libraries/servo.cc @@ -0,0 +1,51 @@ +/**************************************************************************** + * * + * 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 "cmsis_os.h" +#include "bsp_pwm.h" +#include "utils.h" + +namespace control { + + 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); + } + +} diff --git a/shared/libraries/servo.h b/shared/libraries/servo.h new file mode 100644 index 0000000..2455783 --- /dev/null +++ b/shared/libraries/servo.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * * + * 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 "cmsis_os.h" +#include "bsp_pwm.h" + +constexpr double MIN_ANGLE = 0.0; +constexpr double MAX_ANGLE = 120.0; +constexpr uint32_t BASE_WIDTH = 900; +constexpr double ANGLE_TO_WIDTH = 1.0 / 120 * 1200; + +namespace control { + + class Servo { + public: + 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; + }; + +} From ff25b42c74299ce00f77d879a7a585a05f17d49f Mon Sep 17 00:00:00 2001 From: AlchemicRonin <1216532005@qq.com> Date: Wed, 15 Jun 2022 22:38:46 -0500 Subject: [PATCH 2/5] add two more PWM for servo --- boards/DJI_Board_TypeC/Core/Src/tim.c | 12 +++++- boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc | 46 +++++++++++++++------- boards/DJI_Board_TypeC/Makefile | 2 +- 3 files changed, 44 insertions(+), 16 deletions(-) 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 519f8ee..57c22a5 100644 --- a/boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc +++ b/boards/DJI_Board_TypeC/DJI_Board_TypeC.ioc @@ -159,25 +159,27 @@ Mcu.Pin29=PA0-WKUP Mcu.Pin3=PB3 Mcu.Pin30=PA4 Mcu.Pin31=PC4 -Mcu.Pin32=PC5 -Mcu.Pin33=PE9 -Mcu.Pin34=PA7 -Mcu.Pin35=PB0 -Mcu.Pin36=PB15 -Mcu.Pin37=VP_FREERTOS_VS_CMSIS_V2 -Mcu.Pin38=VP_SYS_VS_tim3 -Mcu.Pin39=VP_TIM1_VS_ClockSourceINT +Mcu.Pin32=PE13 +Mcu.Pin33=PC5 +Mcu.Pin34=PE9 +Mcu.Pin35=PE11 +Mcu.Pin36=PA7 +Mcu.Pin37=PB0 +Mcu.Pin38=PB15 +Mcu.Pin39=VP_FREERTOS_VS_CMSIS_V2 Mcu.Pin4=PA14 -Mcu.Pin40=VP_TIM4_VS_ClockSourceINT -Mcu.Pin41=VP_TIM5_VS_ClockSourceINT -Mcu.Pin42=VP_TIM10_VS_ClockSourceINT -Mcu.Pin43=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS +Mcu.Pin40=VP_SYS_VS_tim3 +Mcu.Pin41=VP_TIM1_VS_ClockSourceINT +Mcu.Pin42=VP_TIM4_VS_ClockSourceINT +Mcu.Pin43=VP_TIM5_VS_ClockSourceINT +Mcu.Pin44=VP_TIM10_VS_ClockSourceINT +Mcu.Pin45=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS Mcu.Pin5=PA13 Mcu.Pin6=PB7 Mcu.Pin7=PB6 Mcu.Pin8=PD0 Mcu.Pin9=PC11 -Mcu.PinsNb=44 +Mcu.PinsNb=46 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32F407IGHx @@ -315,6 +317,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 @@ -455,6 +467,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 @@ -472,7 +488,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/boards/DJI_Board_TypeC/Makefile b/boards/DJI_Board_TypeC/Makefile index 7691174..8c2ea63 100644 --- a/boards/DJI_Board_TypeC/Makefile +++ b/boards/DJI_Board_TypeC/Makefile @@ -1,5 +1,5 @@ ########################################################################################################################## -# File automatically-generated by tool: [projectgenerator] version: [3.16.0] date: [Sat Jun 11 18:23:36 CDT 2022] +# File automatically-generated by tool: [projectgenerator] version: [3.16.0] date: [Wed Jun 15 22:38:28 CDT 2022] ########################################################################################################################## # ------------------------------------------------ From 0b9593b5a6c886e8de54dd56cccf81a1f8c40666 Mon Sep 17 00:00:00 2001 From: youhy Date: Wed, 15 Jun 2022 21:33:14 -0700 Subject: [PATCH 3/5] change pulse width range to 600 - 2100 --- examples/servo/.main.cc.swp | Bin 12288 -> 0 bytes examples/servo/.typeC.cc.swp | Bin 12288 -> 0 bytes examples/servo/main.cc | 9 +++------ examples/servo/typeC.cc | 30 +++++++++++++++++++----------- shared/libraries/servo.cc | 5 +++++ shared/libraries/servo.h | 9 +++++++-- 6 files changed, 34 insertions(+), 19 deletions(-) delete mode 100644 examples/servo/.main.cc.swp delete mode 100644 examples/servo/.typeC.cc.swp diff --git a/examples/servo/.main.cc.swp b/examples/servo/.main.cc.swp deleted file mode 100644 index b337f9363421f7c59b7512841607c2f70ab4b11a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI2&2Jk;7{;d@_$VzPDskX|URnt`k!w3CMKpnen>dYyV~6#c5*1;My<_jF>ltfz z)+U9*rQ*T?4v0U1LnSzH;0#FIxNrc0dgB5|jz}Eg*PgW*PbuU&lkm8d@u||RE$J8(!~Lns?hE3ZEPbf0ZU*X1WvMZ^K+#n z@XYC_>D1$`eT=aomVhN-30MM_fF)oFSOS)SCGdYCV1~!odsxhUlLeihe2?AnomB0| z60ig;0ZYIVummgtOTZGa1S|ndz!Iab3TYvw5`6y!^^aS+X zBaB^#^3X3Q8T$yj1pRS>v0KpV&~fOihZ$24K|da6>?U*`dKP*LIsyIp5M#eVze3+b zUqBy228y6JpsUbJ(Cr5q`x*KPx&?g#-GoNaRcIAD3q1+_h5Ubqet^D#K7~GnK7g)6 z$@*f*7E8bqummgtOW@xlaD#{urLWx1UToqqTyAC&4L2Yn9W9H1Z|3GorBd2FjTEM= z`hm#J&6f&WeftQ%aG>bt4itUvK+$P%vi7kr13|gD8G0MFZ4x{*8H!fD)vPS%c-W7? zV5Fr-crNI;jb?jU^mrVYw#%byxvo}`p`?G7(qQ$xDBV>Xt|SmvtF*&+STP+rR}ad z4cDzz?o6zzq{$_zU{Tm$wtL#yETu)fz4Am)DnWO#qzk3ecK_}a==`*2ce0e66qN0O zI~GQG*m*{(ZV-EdPNz@`{^=b}ooLt|jt9FN)0daK!n%Vag1hvtb0MYYUsvbN0a zh!#4@?H-f%bVvgo3i5>EGKkU@{nPAOZfg}Mxe#voQnglfHwX?ZRkvPgHfg2dP?^@t zj$2)7)yfX7x19AxvjW#9U?u>jC3EYPaLv6$|RMz0tNl@677 z_bWkk%)^i?lQP}5%iP=@8%fwxPevw`m;=LP=HlyNk_k5?4dw^}>IjM>(Tjup9d6sz zL;jHEmcx{I+#He~8mZL`akWVSHzK5w2qQGz6aID%nOnkgB?Q!_?imn4O~=aPiFKX? zPW9B6&TSpP$yv)Mo0gT#4Ja@eBNAI=G>EoF+~e$U+?nJEM?UCwHWROb@30FImecw=SX@-*b={ubFx1FhF<~DUd+)uNtc!a-u GXW8F2BWWT4 diff --git a/examples/servo/.typeC.cc.swp b/examples/servo/.typeC.cc.swp deleted file mode 100644 index 27c1c29736f33dcc360e85e3a1074b45abce4361..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI2&2Jk;7{;d@fR+{zNF32iDdDs<>BOH6Ru)3NTZ}8#B6Jb6S!M4il$w-uPz23pjriUS52<%HB(*4utPp~tq zD}}hNJ{r&&-V~jb_ zQ_zo(GWI5vh5k5!{?KLU=5fZpfL?=+LEk>Ym=6(j;}|@lbI@7nY3Mlg&r!zyhW>=U zhrWb9fZm5*his?@RiRaA1^Vq_#(srcypiiI= zq4%IogE@Zv;)n45{)7+6!us;+NFzBsaGt!*{Qb6_GV?> zx_Tk$EBlmL?01Q9xu>v1ZzZjZEzeyfw4|fbd5BcFE<8TTprNpIXTyuP&pdOVjXpOY zJXBf*E2l5Dg>J^dZ^p4N#+OVocBLmMvy%GE$7mktOwyrK@3hP786FHm)VGy%2=@eA zR-mBW;^)hgHlsa^Tn9D;b?8Gxmn9R>uNb%$r3)> zrK`nyyExXaNruAF7Bci<_ReY&4aS-u(*le=pX$65Q?mO>%uMOW+5T0TD>k8=Y*%Bf(G_he7_A#R{v%EbYtZd)p%&{dJ=kju9P1)?Y9 zws443PmLy|`j|z~m3oJ+2ww!;qh{2F6;)Ha>Dp4aLt5xGwtXh;s{s2j5abBWr57eE z`mfp3*lOigd^W7&ERx zmej2qY1*txj7&;(o*?L#Qh0* zT#JCVMG)fEzVPQUq;4_GjQ~*Fs;@x=H5K^|kFB#Lu&Z}{>C9~VP4`+hPFh-0H=w{^ zjEHR!(J-8i*k|u>oSAqFhc4)LC$Zn1v(S&@45x9f#So~+eH@Y9^u)>FzLQV3qa*tZ zZqq>T`jN&~@jU5EYN@WO@i2z}5_No>RAeCCQb`Ym&4YY(f-d*@>5_{(=Ufi_-5^r8 m5{`Bt2d<_}X^GbC2Ccb}G7{t&-=m)XDcQEH?B diff --git a/examples/servo/main.cc b/examples/servo/main.cc index 29ef09a..36eacd1 100644 --- a/examples/servo/main.cc +++ b/examples/servo/main.cc @@ -23,15 +23,13 @@ #include "bsp_pwm.h" #include "servo.h" -#define PWM_CHANNEL 1 -#define TIM_CLOCK_FREQ 1000000 -#define MOTOR_OUT_FREQ 50 -#define SNAIL_IDLE_THROTTLE 900 + +#define PWM_CHANNEL 2 control::Servo* servo; void RM_RTOS_Init(void) { - servo = new control::Servo(&htim1, PWM_CHANNEL, TIM_CLOCK_FREQ, MOTOR_OUT_FREQ, SNAIL_IDLE_THROTTLE); + servo = new control::Servo(&htim1, PWM_CHANNEL); } void RM_RTOS_Default_Task(const void* args) { @@ -49,5 +47,4 @@ void RM_RTOS_Default_Task(const void* args) { servo->SetAngle(120.0); osDelay(1000); } - servo->Stop(); } diff --git a/examples/servo/typeC.cc b/examples/servo/typeC.cc index b46224b..d8044eb 100644 --- a/examples/servo/typeC.cc +++ b/examples/servo/typeC.cc @@ -22,33 +22,41 @@ #include "main.h" #include "bsp_pwm.h" -#define PWM_CHANNEL 1 +#define PWM_CHANNEL 2 #define TIM_CLOCK_FREQ 1000000 #define MOTOR_OUT_FREQ 50 -#define INIT_PULSE_WIDTH 900 +#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); - osDelay(500); - if (pulse_width == INIT_PULSE_WIDTH) { - osDelay(1000); - } - pulse_width += 100; - if (pulse_width > 2100) { - pulse_width = INIT_PULSE_WIDTH; - } + //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 index 1e38a6a..2b30e72 100644 --- a/shared/libraries/servo.cc +++ b/shared/libraries/servo.cc @@ -23,8 +23,13 @@ #include "bsp_pwm.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); } diff --git a/shared/libraries/servo.h b/shared/libraries/servo.h index 2455783..6617e2e 100644 --- a/shared/libraries/servo.h +++ b/shared/libraries/servo.h @@ -25,13 +25,18 @@ constexpr double MIN_ANGLE = 0.0; constexpr double MAX_ANGLE = 120.0; -constexpr uint32_t BASE_WIDTH = 900; -constexpr double ANGLE_TO_WIDTH = 1.0 / 120 * 1200; +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(); From e4bc792d98fc3153f88c101f0ce091e7cdd27664 Mon Sep 17 00:00:00 2001 From: youhy Date: Wed, 15 Jun 2022 21:55:19 -0700 Subject: [PATCH 4/5] fix format --- examples/servo/main.cc | 12 ++++----- examples/servo/typeC.cc | 20 +++++++-------- shared/libraries/servo.cc | 54 +++++++++++++++++++-------------------- shared/libraries/servo.h | 29 +++++++++++---------- 4 files changed, 55 insertions(+), 60 deletions(-) diff --git a/examples/servo/main.cc b/examples/servo/main.cc index 36eacd1..6e0aaaf 100644 --- a/examples/servo/main.cc +++ b/examples/servo/main.cc @@ -18,25 +18,23 @@ * * ****************************************************************************/ -#include "cmsis_os.h" #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_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); + servo->SetAngle(0.0); osDelay(1000); servo->SetAngle(30.0); osDelay(1000); @@ -47,4 +45,4 @@ void RM_RTOS_Default_Task(const void* args) { servo->SetAngle(120.0); osDelay(1000); } -} +} diff --git a/examples/servo/typeC.cc b/examples/servo/typeC.cc index d8044eb..dc60993 100644 --- a/examples/servo/typeC.cc +++ b/examples/servo/typeC.cc @@ -18,9 +18,9 @@ * * ****************************************************************************/ +#include "bsp_pwm.h" #include "cmsis_os.h" #include "main.h" -#include "bsp_pwm.h" #define PWM_CHANNEL 2 #define TIM_CLOCK_FREQ 1000000 @@ -33,10 +33,8 @@ 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) { @@ -44,19 +42,19 @@ void RM_RTOS_Default_Task(const void* args) { motor1->Start(); motor2->Start(); while (1) { - //motor1->SetPulseWidth(pulse_width); - //motor2->SetPulseWidth(pulse_width); - //osDelay(500); - //if (pulse_width == INIT_PULSE_WIDTH) { + // 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 += 100; + // if (pulse_width > 2400) { // pulse_width = INIT_PULSE_WIDTH; //} motor2->SetPulseWidth(600); osDelay(1500); motor2->SetPulseWidth(2100); - osDelay(1500); + osDelay(1500); } -} +} diff --git a/shared/libraries/servo.cc b/shared/libraries/servo.cc index 2b30e72..80987b6 100644 --- a/shared/libraries/servo.cc +++ b/shared/libraries/servo.cc @@ -19,38 +19,36 @@ ****************************************************************************/ #include "servo.h" -#include "cmsis_os.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); - } +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 index 6617e2e..40a7489 100644 --- a/shared/libraries/servo.h +++ b/shared/libraries/servo.h @@ -20,8 +20,8 @@ #pragma once -#include "cmsis_os.h" #include "bsp_pwm.h" +#include "cmsis_os.h" constexpr double MIN_ANGLE = 0.0; constexpr double MAX_ANGLE = 120.0; @@ -34,18 +34,19 @@ 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); +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; - }; + private: + bsp::PWM* servo; + uint32_t pulse_width; +}; -} +} // namespace control From 03fd995745de1f40c191ad14a6a5b789cf8c4d3c Mon Sep 17 00:00:00 2001 From: youhy Date: Thu, 16 Jun 2022 07:43:00 -0700 Subject: [PATCH 5/5] rename typeC.cc to test.cc --- examples/servo/CMakeLists.txt | 4 ++-- examples/servo/{typeC.cc => test.cc} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename examples/servo/{typeC.cc => test.cc} (100%) diff --git a/examples/servo/CMakeLists.txt b/examples/servo/CMakeLists.txt index b9077dc..5b5f318 100644 --- a/examples/servo/CMakeLists.txt +++ b/examples/servo/CMakeLists.txt @@ -20,9 +20,9 @@ project(example_servo ASM C CXX) -irm_add_arm_executable(${PROJECT_NAME}_typeC +irm_add_arm_executable(${PROJECT_NAME}_test TARGET DJI_Board_TypeC - SOURCES typeC.cc) + SOURCES test.cc) irm_add_arm_executable(${PROJECT_NAME} TARGET DJI_Board_TypeC diff --git a/examples/servo/typeC.cc b/examples/servo/test.cc similarity index 100% rename from examples/servo/typeC.cc rename to examples/servo/test.cc