Skip to content

Fix servo 4 on Tilda Mk3 #72

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jburgess777 wants to merge 1 commit into
base: tilda-master
Choose a base branch
from
Open

Fix servo 4 on Tilda Mk3 #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions stmhal/irq.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,12 +126,12 @@ MP_DECLARE_CONST_FUN_OBJ(pyb_irq_stats_obj);
#define IRQ_PRI_OTG_HS 6
#define IRQ_SUBPRI_OTG_HS 0

#define IRQ_PRI_TIM2 6
#define IRQ_SUBPRI_TIM2 0

#define IRQ_PRI_TIM3 6
#define IRQ_SUBPRI_TIM3 0

#define IRQ_PRI_TIM5 6
#define IRQ_SUBPRI_TIM5 0

#define IRQ_PRI_CAN 7
#define IRQ_SUBPRI_CAN 0

Expand Down
48 changes: 26 additions & 22 deletions stmhal/servo.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,10 @@
///
/// Servo controls standard hobby servos with 3-wires (ground, power, signal).

// this servo driver uses hardware PWM to drive servos on PA0, PA1, PA2, PA3 = X1, X2, X3, X4
// TIM2 and TIM5 have CH1, CH2, CH3, CH4 on PA0-PA3 respectively
// they are both 32-bit counters with 16-bit prescaler
// we use TIM5
// this servo driver uses hardware PWM to drive servos on PA0, PA1, PA2, PB11 = X1, X2, X3, X4
// TIM2 has CH1, CH2, CH3, CH4 on PA0, PA1, PA2, PB11 respectively
// it is a 32-bit counters with 16-bit prescaler
// Note: the use of TIM2 is specific to Tilda, the pyboard used TIM5

#define PYB_SERVO_NUM (4)

Expand All @@ -62,7 +62,7 @@ typedef struct _pyb_servo_obj_t {
STATIC pyb_servo_obj_t pyb_servo_obj[PYB_SERVO_NUM];

void servo_init(void) {
timer_tim5_init();
timer_tim2_init();

// reset servo objects
for (int i = 0; i < PYB_SERVO_NUM; i++) {
Expand Down Expand Up @@ -103,17 +103,17 @@ void servo_timer_irq_callback(void) {
}
// set the pulse width
switch (s->servo_id) {
case 1: TIM5->CCR1 = s->pulse_cur; break;
case 2: TIM5->CCR2 = s->pulse_cur; break;
case 3: TIM5->CCR3 = s->pulse_cur; break;
case 4: TIM5->CCR4 = s->pulse_cur; break;
case 1: TIM2->CCR1 = s->pulse_cur; break;
case 2: TIM2->CCR2 = s->pulse_cur; break;
case 3: TIM2->CCR3 = s->pulse_cur; break;
case 4: TIM2->CCR4 = s->pulse_cur; break;
}
}
}
if (need_it) {
__HAL_TIM_ENABLE_IT(&TIM5_Handle, TIM_IT_UPDATE);
__HAL_TIM_ENABLE_IT(&TIM2_Handle, TIM_IT_UPDATE);
} else {
__HAL_TIM_DISABLE_IT(&TIM5_Handle, TIM_IT_UPDATE);
__HAL_TIM_DISABLE_IT(&TIM2_Handle, TIM_IT_UPDATE);
}
}

Expand All @@ -124,7 +124,7 @@ STATIC void servo_init_channel(pyb_servo_obj_t *s) {
case 1: pin = GPIO_PIN_0; channel = TIM_CHANNEL_1; break;
case 2: pin = GPIO_PIN_1; channel = TIM_CHANNEL_2; break;
case 3: pin = GPIO_PIN_2; channel = TIM_CHANNEL_3; break;
case 4: pin = GPIO_PIN_3; channel = TIM_CHANNEL_4; break;
case 4: pin = GPIO_PIN_11; channel = TIM_CHANNEL_4; break;
default: return;
}

Expand All @@ -134,19 +134,23 @@ STATIC void servo_init_channel(pyb_servo_obj_t *s) {
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
GPIO_InitStructure.Speed = GPIO_SPEED_FAST;
GPIO_InitStructure.Pull = GPIO_NOPULL;
GPIO_InitStructure.Alternate = GPIO_AF2_TIM5;
HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.Alternate = GPIO_AF1_TIM2;
if (s->servo_id == 4) {
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
} else {
HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
}

// PWM mode configuration
TIM_OC_InitTypeDef oc_init;
oc_init.OCMode = TIM_OCMODE_PWM1;
oc_init.Pulse = s->pulse_cur; // units of 10us
oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
oc_init.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(&TIM5_Handle, &oc_init, channel);
HAL_TIM_PWM_ConfigChannel(&TIM2_Handle, &oc_init, channel);

// start PWM
HAL_TIM_PWM_Start(&TIM5_Handle, channel);
HAL_TIM_PWM_Start(&TIM2_Handle, channel);
}

/******************************************************************************/
Expand All @@ -158,10 +162,10 @@ STATIC mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t value) {
if (v < 50) { v = 50; }
if (v > 250) { v = 250; }
switch (p) {
case 1: TIM5->CCR1 = v; break;
case 2: TIM5->CCR2 = v; break;
case 3: TIM5->CCR3 = v; break;
case 4: TIM5->CCR4 = v; break;
case 1: TIM2->CCR1 = v; break;
case 2: TIM2->CCR2 = v; break;
case 3: TIM2->CCR3 = v; break;
case 4: TIM2->CCR4 = v; break;
}
return mp_const_none;
}
Expand All @@ -171,8 +175,8 @@ MP_DEFINE_CONST_FUN_OBJ_2(pyb_servo_set_obj, pyb_servo_set);
STATIC mp_obj_t pyb_pwm_set(mp_obj_t period, mp_obj_t pulse) {
int pe = mp_obj_get_int(period);
int pu = mp_obj_get_int(pulse);
TIM5->ARR = pe;
TIM5->CCR3 = pu;
TIM2->ARR = pe;
TIM2->CCR3 = pu;
return mp_const_none;
}

Expand Down
2 changes: 1 addition & 1 deletion stmhal/stm32_it.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -559,6 +559,7 @@ void TIM1_TRG_COM_TIM17_IRQHandler(void) {
void TIM2_IRQHandler(void) {
IRQ_ENTER(TIM2_IRQn);
timer_irq_handler(2);
HAL_TIM_IRQHandler(&TIM2_Handle);
IRQ_EXIT(TIM2_IRQn);
}

Expand All @@ -577,7 +578,6 @@ void TIM4_IRQHandler(void) {
void TIM5_IRQHandler(void) {
IRQ_ENTER(TIM5_IRQn);
timer_irq_handler(5);
HAL_TIM_IRQHandler(&TIM5_Handle);
IRQ_EXIT(TIM5_IRQn);
}

Expand Down
32 changes: 16 additions & 16 deletions stmhal/timer.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@
/// tim.callback(lambda t: ...) # set callback for update interrupt (t=tim instance)
/// tim.callback(None) # clear callback
///
/// *Note:* Timer 3 is used for fading the blue LED. Timer 5 controls
/// *Note:* Timer 3 is used for fading the blue LED. Timer 2 controls
/// the servo driver, and Timer 6 is used for timed ADC/DAC reading/writing.
/// It is recommended to use the other timers in your programs.

Expand All @@ -78,7 +78,7 @@
// TIM3:
// - LED 4, PWM to set the LED intensity
//
// TIM5:
// TIM2:
// - servo controller, PWM
//
// TIM6:
Expand Down Expand Up @@ -142,7 +142,7 @@ typedef struct _pyb_timer_obj_t {
#define TIMER_CNT_MASK(self) ((self)->is_32bit ? 0xffffffff : 0xffff)
#define TIMER_CHANNEL(self) ((((self)->channel) - 1) << 2)

TIM_HandleTypeDef TIM5_Handle;
TIM_HandleTypeDef TIM2_Handle;
TIM_HandleTypeDef TIM6_Handle;

#define PYB_TIMER_OBJ_ALL_NUM MP_ARRAY_SIZE(MP_STATE_PORT(pyb_timer_obj_all))
Expand All @@ -167,24 +167,24 @@ void timer_deinit(void) {
}
}

// TIM5 is set-up for the servo controller
// TIM2 is set-up for the servo controller
// This function inits but does not start the timer
void timer_tim5_init(void) {
// TIM5 clock enable
__TIM5_CLK_ENABLE();
void timer_tim2_init(void) {
// TIM2 clock enable
__TIM2_CLK_ENABLE();

// set up and enable interrupt
HAL_NVIC_SetPriority(TIM5_IRQn, IRQ_PRI_TIM5, IRQ_SUBPRI_TIM5);
HAL_NVIC_EnableIRQ(TIM5_IRQn);
HAL_NVIC_SetPriority(TIM2_IRQn, IRQ_PRI_TIM2, IRQ_SUBPRI_TIM2);
HAL_NVIC_EnableIRQ(TIM2_IRQn);

// PWM clock configuration
TIM5_Handle.Instance = TIM5;
TIM5_Handle.Init.Period = 2000 - 1; // timer cycles at 50Hz
TIM5_Handle.Init.Prescaler = (timer_get_source_freq(5) / 100000) - 1; // timer runs at 100kHz
TIM5_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
TIM5_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
TIM2_Handle.Instance = TIM2;
TIM2_Handle.Init.Period = 2000 - 1; // timer cycles at 50Hz
TIM2_Handle.Init.Prescaler = (timer_get_source_freq(2) / 100000) - 1; // timer runs at 100kHz
TIM2_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
TIM2_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;

HAL_TIM_PWM_Init(&TIM5_Handle);
HAL_TIM_PWM_Init(&TIM2_Handle);
}

#if defined(TIM6)
Expand Down Expand Up @@ -218,7 +218,7 @@ TIM_HandleTypeDef *timer_tim6_init(uint freq) {

// Interrupt dispatch
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim == &TIM5_Handle) {
if (htim == &TIM2_Handle) {
servo_timer_irq_callback();
}
}
Expand Down
4 changes: 2 additions & 2 deletions stmhal/timer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,12 @@
* THE SOFTWARE.
*/

extern TIM_HandleTypeDef TIM5_Handle;
extern TIM_HandleTypeDef TIM2_Handle;

extern const mp_obj_type_t pyb_timer_type;

void timer_init0(void);
void timer_tim5_init(void);
void timer_tim2_init(void);
TIM_HandleTypeDef *timer_tim6_init(uint freq);
void timer_deinit(void);
uint32_t timer_get_source_freq(uint32_t tim_id);
Expand Down