1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
|
#include <stdio.h>
#include <stm32f4xx.h>
#include <stm32f4xx_rcc.h>
#include <stm32f4xx_gpio.h>
#include <stm32f4xx_tim.h>
#include "misc.h"
#include "mpconfig.h"
#include "qstr.h"
#include "obj.h"
#include "servo.h"
// PWM
// TIM2 and TIM5 have CH1, CH2, CH3, CH4 on PA0-PA3 respectively
// they are both 32-bit counters
// 16-bit prescaler
// TIM2_CH3 also on PB10 (used below)
void servo_init(void) {
// TIM2 clock enable
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
// for PB10
/*
// GPIOB Configuration: TIM2_CH3 (PB10)
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Connect TIM2 pins to AF1
GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_TIM2);
*/
// for PA0, PA1, PA2, PA3
{
// GPIOA Configuration: TIM2_CH0, TIM2_CH1 (PA0, PA1)
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Connect TIM2 pins to AF1
GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM2);
}
// Compute the prescaler value so TIM2 runs at 100kHz
uint16_t PrescalerValue = (uint16_t) ((SystemCoreClock / 2) / 100000) - 1;
// Time base configuration
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructure.TIM_Period = 2000; // timer cycles at 50Hz
TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
// PWM Mode configuration
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 150; // units of 10us
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(TIM2, &TIM_OCInitStructure); // channel 1
TIM_OC2Init(TIM2, &TIM_OCInitStructure); // channel 2
TIM_OC3Init(TIM2, &TIM_OCInitStructure); // channel 3
TIM_OC4Init(TIM2, &TIM_OCInitStructure); // channel 4
// ?
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 1
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 2
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 3
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 4
// ?
TIM_ARRPreloadConfig(TIM2, ENABLE);
// TIM2 enable counter
TIM_Cmd(TIM2, ENABLE);
}
/******************************************************************************/
/* Micro Python bindings */
STATIC mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t value) {
int p = mp_obj_get_int(port);
int v = mp_obj_get_int(value);
if (v < 50) { v = 50; }
if (v > 250) { v = 250; }
switch (p) {
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;
}
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);
TIM2->ARR = pe;
TIM2->CCR3 = pu;
return mp_const_none;
}
MP_DEFINE_CONST_FUN_OBJ_2(pyb_pwm_set_obj, pyb_pwm_set);
typedef struct _pyb_servo_obj_t {
mp_obj_base_t base;
uint servo_id;
} pyb_servo_obj_t;
STATIC void servo_obj_print(void (*print)(void *env, const char *fmt, ...), void *env, mp_obj_t self_in, mp_print_kind_t kind) {
pyb_servo_obj_t *self = self_in;
print(env, "<Servo %lu>", self->servo_id);
}
STATIC mp_obj_t servo_obj_angle(mp_obj_t self_in, mp_obj_t angle) {
pyb_servo_obj_t *self = self_in;
#if MICROPY_ENABLE_FLOAT
machine_int_t v = 152 + 85.0 * mp_obj_get_float(angle) / 90.0;
#else
machine_int_t v = 152 + 85 * mp_obj_get_int(angle) / 90;
#endif
if (v < 65) { v = 65; }
if (v > 210) { v = 210; }
switch (self->servo_id) {
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;
}
STATIC MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_angle_obj, servo_obj_angle);
STATIC const mp_map_elem_t servo_locals_dict_table[] = {
{ MP_OBJ_NEW_QSTR(MP_QSTR_angle), (mp_obj_t)&servo_obj_angle_obj },
};
STATIC MP_DEFINE_CONST_DICT(servo_locals_dict, servo_locals_dict_table);
STATIC const mp_obj_type_t servo_obj_type = {
{ &mp_type_type },
.name = MP_QSTR_Servo,
.print = servo_obj_print,
.locals_dict = (mp_obj_t)&servo_locals_dict,
};
STATIC mp_obj_t pyb_Servo(mp_obj_t servo_id) {
pyb_servo_obj_t *o = m_new_obj(pyb_servo_obj_t);
o->base.type = &servo_obj_type;
o->servo_id = mp_obj_get_int(servo_id);
return o;
}
MP_DEFINE_CONST_FUN_OBJ_1(pyb_Servo_obj, pyb_Servo);
|
