Rev Author Line No. Line
2573 povik 1 #include <libopencm3/stm32/f1/rcc.h>
2 #include <libopencm3/stm32/f1/gpio.h>
3 #include <libopencm3/stm32/timer.h>
4 #include <libopencm3/stm32/nvic.h>
5 #include <libopencm3/stm32/usart.h>
6  
7 #include <stdio.h>
8 #include <errno.h>
9  
10 int pulse_width_abs = 130;
11 int pulse_width_speed_div = 15;
12  
13 int min_speed = 500;
14 int max_speed = 2000;
15 int accel = 800;
16  
17 /* sinewave in degrees */
18 int8_t sinewave[360] = {0, 2, 4, 6, 8, 11, 13, 15, 17, 19, 22, 24, 26, 28, 30, 32, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 72, 74, 76, 78, 79, 81, 83, 84, 86, 88, 89, 91, 92, 94, 95, 97, 98, 100, 101, 102, 104, 105, 106, 107, 108, 109, 111, 112, 113, 114, 115, 116, 116, 117, 118, 119, 120, 120, 121, 122, 122, 123, 123, 124, 124, 125, 125, 125, 126, 126, 126, 126, 126, 126, 126, 127, 126, 126, 126, 126, 126, 126, 126, 125, 125, 125, 124, 124, 123, 123, 122, 122, 121, 120, 120, 119, 118, 117, 116, 116, 115, 114, 113, 112, 111, 109, 108, 107, 106, 105, 104, 102, 101, 100, 98, 97, 95, 94, 92, 91, 89, 88, 86, 84, 83, 81, 79, 78, 76, 74, 72, 71, 69, 67, 65, 63, 61, 59, 57, 55, 53, 51, 49, 47, 45, 43, 41, 39, 37, 35, 32, 30, 28, 26, 24, 22, 19, 17, 15, 13, 11, 8, 6, 4, 2, 0, -2, -4, -6, -8, -11, -13, -15, -17, -19, -22, -24, -26, -28, -30, -32, -35, -37, -39, -41, -43, -45, -47, -49, -51, -53, -55, -57, -59, -61, -63, -65, -67, -69, -71, -72, -74, -76, -78, -79, -81, -83, -84, -86, -88, -89, -91, -92, -94, -95, -97, -98, -100, -101, -102, -104, -105, -106, -107, -108, -109, -111, -112, -113, -114, -115, -116, -116, -117, -118, -119, -120, -120, -121, -122, -122, -123, -123, -124, -124, -125, -125, -125, -126, -126, -126, -126, -126, -126, -126, -127, -126, -126, -126, -126, -126, -126, -126, -125, -125, -125, -124, -124, -123, -123, -122, -122, -121, -120, -120, -119, -118, -117, -116, -116, -115, -114, -113, -112, -111, -109, -108, -107, -106, -105, -104, -102, -101, -100, -98, -97, -95, -94, -92, -91, -89, -88, -86, -84, -83, -81, -79, -78, -76, -74, -72, -71, -69, -67, -65, -63, -61, -59, -57, -55, -53, -51, -49, -47, -45, -43, -41, -39, -37, -35, -32, -30, -28, -26, -24, -22, -19, -17, -15, -13, -11, -8, -6, -4, -2};
19  
20 const int pwm_freq = 56250; /* 56.25 kHz */
21  
22 void usart_setup(void)
23 {
24 rcc_peripheral_enable_clock(&RCC_APB2ENR,
25 RCC_APB2ENR_IOPAEN | RCC_APB2ENR_AFIOEN | RCC_APB2ENR_USART1EN);
26  
27 gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9);
28  
29 usart_set_baudrate(USART1, 9600);
30 usart_set_databits(USART1, 8);
31  
32 usart_set_stopbits(USART1, USART_STOPBITS_1);
33 usart_set_mode(USART1, USART_MODE_TX_RX);
34 usart_set_parity(USART1, USART_PARITY_NONE);
35 usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE);
36  
37 usart_enable(USART1);
38 }
39  
40 int _write(int file, char *ptr, int len)
41 {
42 int i;
43  
44 if (file == 1) {
45 for (i = 0; i < len; i++) {
46 usart_send_blocking(USART1, ptr[i]);
47 }
48  
49 return i;
50 }
51  
52 errno = EIO;
53 return -1;
54 }
55  
56 int _read(int file, char *ptr, int len)
57 {
58 int i;
59  
60 if (file == 0) {
61 for (i = 0; i < len; i++) {
62 *ptr++ = usart_recv_blocking(USART1);
63 }
64  
65 return i;
66 }
67  
68 errno = EIO;
69 return -1;
70 }
71  
72 uint32_t *phase_reg[4];
73 int phases_c = sizeof(phase_reg) / sizeof(uint32_t *);
74  
75 void set_angle(unsigned int deg, unsigned int pulse_width)
76 {
77 unsigned int i;
78 for (i = 0; i < phases_c; i++) {
79 int16_t val = sinewave[(deg + i * (360 / phases_c)) % 360];
80 val *= pulse_width;
81 val /= 255;
82 *(phase_reg[i]) = (val > 0 ? val : 0);
83 }
84 }
85  
86 int pulse_width;
87  
88 int counter = 0;
89 int last_change = 0;
90  
91 int stop = 0;
92  
93 int speed = 0;
94  
95 int pos = 0;
96 int tgt_pos = 0;
97  
98 int curr_accel = 0;
99 int change = 0;
100  
101 int calc_speed()
102 {
103 int dir = pos < tgt_pos ? 1 : -1;
104  
105 unsigned int abs_speed = speed > 0 ? speed : -speed;
106  
107 if (pos == tgt_pos && abs_speed <= min_speed) {
108 last_change = counter;
109 return 0;
110 }
111  
112 curr_accel = 0;
113  
114 /* FIXME: buggy */
115  
116 if (speed * dir < 0) {
117 curr_accel = accel * dir;
118 } else if (abs_speed * abs_speed >= ((tgt_pos - pos) * dir - 1) * accel * 2) {
119 curr_accel = accel * -dir;
120 } else {
121 if (abs_speed < max_speed)
122 curr_accel = accel * dir;
123  
124 if (abs_speed > max_speed)
125 curr_accel = accel * -dir;
126 }
127  
128 if (curr_accel) {
129 change = curr_accel * (counter - last_change) / pwm_freq;
130 last_change += change * pwm_freq / curr_accel;
131  
132 return speed + change;
133 } else {
134 last_change = counter;
135  
136 return speed;
137 }
138 }
139  
140 void tim4_isr()
141 {
142 if (stop) {
143 TIM4_CCR1 = 0;
144 TIM4_CCR2 = 0;
145 TIM4_CCR3 = 0;
146 TIM4_CCR4 = 0;
147 TIM_SR(TIM4) &= ~TIM_SR_UIF;
148 return;
149 }
150  
151 counter++;
152  
153 int abs_speed = speed > 0 ? speed : -speed;
154  
155 if (speed) {
156 if (counter % (pwm_freq / abs_speed) == 0) {
157 pos += speed > 0 ? 1 : -1;
158 }
159 }
160  
161 if (counter % (pwm_freq / 1000) == 0) {
162 speed = calc_speed();
163 }
164  
165 pulse_width = pulse_width_abs + abs_speed / pulse_width_speed_div;
166  
167 /* TODO */
168 set_angle((pos + 100000) % 360, pulse_width < 255 ? pulse_width : 255);
169  
170 TIM_SR(TIM4) &= ~TIM_SR_UIF;
171 }
172  
173 void set_tgt_pos(int pos)
174 {
175 tgt_pos = pos;
176 }
177  
178 void timer_setup()
179 {
180 rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);
181 rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN);
182  
183 gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
184 GPIO6 | GPIO7 | GPIO8 | GPIO9);
185  
186 /* 72MHz / 10 = 7.2MHz */
187 TIM_PSC(TIM4) = 9;
188  
189 /* 7.2MHz / 128 = 56.250kHz */
190 TIM_CNT(TIM4) = 0;
191 TIM_ARR(TIM4) = 128;
192  
193 TIM_EGR(TIM4) = TIM_EGR_UG;
194  
195 TIM_CCMR1(TIM4) = TIM_CCMR1_OC1M_PWM1 | TIM_CCMR1_OC1PE
196 | TIM_CCMR1_OC2M_PWM1 | TIM_CCMR1_OC2PE;
197 TIM_CCMR2(TIM4) = TIM_CCMR2_OC3M_PWM1 | TIM_CCMR2_OC3PE
198 | TIM_CCMR2_OC4M_PWM1 | TIM_CCMR2_OC4PE;
199  
200 TIM_CCER(TIM4) = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E;
201  
202 TIM_CCR1(TIM4) = 0;
203 TIM_CCR2(TIM4) = 0;
204 TIM_CCR3(TIM4) = 0;
205 TIM_CCR4(TIM4) = 0;
206  
207 TIM_DIER(TIM4) = TIM_DIER_UIE;
208 nvic_enable_irq(NVIC_TIM4_IRQ);
209 nvic_set_priority(NVIC_TIM4_IRQ, 1);
210  
211 TIM_CR1(TIM4) |= TIM_CR1_CEN;
212 }
213  
214 int main(void)
215 {
216 int i;
217  
218 setvbuf(stdin, NULL, _IONBF, 0);
219 setvbuf(stdout, NULL, _IONBF, 0);
220  
221 rcc_clock_setup_in_hse_8mhz_out_72mhz();
222  
223 phase_reg[0] = &TIM4_CCR1;
224 phase_reg[1] = &TIM4_CCR2;
225 phase_reg[2] = &TIM4_CCR3;
226 phase_reg[3] = &TIM4_CCR4;
227  
228 speed = 0;
229 set_tgt_pos(0);
230  
231 usart_setup();
232 timer_setup();
233  
234 char c;
235 while (1) {
236 c = getchar();
237  
238 switch(c) {
239 case 's':
240 stop = 1;
241 break;
242 case 'r':
243 printf("current position is %d\r\n", pos);
244 printf("enter target position: ");
245 scanf("%d", &tgt_pos);
246 printf("\r\n");
247 printf("target position is %d\r\n", tgt_pos);
248 set_tgt_pos(tgt_pos);
249 break;
250 case 'v':
251 printf("current pulse_width_abs is %d\r\n", pulse_width_abs);
252 printf("enter new pulse_width_abs: ");
253 scanf("%d", &pulse_width_abs);
254 printf("\r\n");
255 break;
256 case 'b':
257 printf("current pulse_width_speed_div is %d\r\n", pulse_width_speed_div);
258 printf("enter new pulse_width_speed_div: ");
259 scanf("%d", &pulse_width_speed_div);
260 printf("\r\n");
261 break;
262 case 'n':
263 printf("current min_speed is %d\r\n", min_speed);
264 printf("enter new min_speed: ");
265 scanf("%d", &min_speed);
266 printf("\r\n");
267 break;
268 case 'm':
269 printf("current max_speed is %d\r\n", max_speed);
270 printf("enter new max_speed: ");
271 scanf("%d", &max_speed);
272 printf("\r\n");
273 break;
274 default:
275 printf("pos: %d tgt_pos: %d speed: %d pulse_width: %d curr_accel: %d change: %d last_change: %d\r\n",
276 pos, tgt_pos, speed, pulse_width, curr_accel, change, last_change);
277 }
278 }
279  
280 return 0;
281 }