Lephenixnoir
/
PythonExtra
1
0
Fork 2
Code Issues 9 Pull Requests 3 Projects Releases 2 Wiki Activity

stmhal/servo: Make pyb.Servo(n) map to Pin('Xn') on all MCUs. 

Prior to this patch Servo numbers 1, 2, 3, 4 mapped to pins X3, X4, X1, X2
on PYBLITE which doesn't match the standard PYB mapping.  This patch fixes
the mapping.
This commit is contained in:
Damien George 2017-07-14 16:38:15 +10:00
parent f1d260d878
commit 2b70757411
1 changed files with 32 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
stmhal/servo.c
View File

@ -26,26 +26,28 @@
#include <stdio.h>
#include "py/nlr.h"
#include "py/runtime.h"
#include "py/mphal.h"
#include "pin.h"
#include "genhdr/pins.h"
#include "timer.h"
#include "servo.h"
/// \moduleref pyb
/// \class Servo - 3-wire hobby servo driver
///
/// 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 file implements the pyb.Servo class which controls standard hobby servo
// motors that have 3-wires (ground, power, signal).
//
// The driver uses hardware PWM to drive servos on pins X1, X2, X3, X4 which are
// assumed to be on PA0, PA1, PA2, PA3 but not necessarily in that order (the
// pins PA0-PA3 are used directly if the X pins are not defined).
//
// TIM2 and TIM5 have CH1-CH4 on PA0-PA3 respectively. They are both 32-bit
// counters with 16-bit prescaler. TIM5 is used by this driver.
#define PYB_SERVO_NUM (4)
typedef struct _pyb_servo_obj_t {
mp_obj_base_t base;
uint8_t servo_id;
const pin_obj_t *pin;
uint8_t pulse_min; // units of 10us
uint8_t pulse_max; // units of 10us
uint8_t pulse_centre; // units of 10us
@ -65,7 +67,6 @@ void servo_init(void) {
// reset servo objects
for (int i = 0; i < PYB_SERVO_NUM; i++) {
pyb_servo_obj[i].base.type = &pyb_servo_type;
pyb_servo_obj[i].servo_id = i + 1;
pyb_servo_obj[i].pulse_min = 64;
pyb_servo_obj[i].pulse_max = 242;
pyb_servo_obj[i].pulse_centre = 150;
@ -75,6 +76,19 @@ void servo_init(void) {
pyb_servo_obj[i].pulse_dest = 0;
pyb_servo_obj[i].time_left = 0;
}
// assign servo objects to specific pins (must be some permutation of PA0-PA3)
#ifdef pyb_pin_X1
pyb_servo_obj[0].pin = &pyb_pin_X1;
pyb_servo_obj[1].pin = &pyb_pin_X2;
pyb_servo_obj[2].pin = &pyb_pin_X3;
pyb_servo_obj[3].pin = &pyb_pin_X4;
#else
pyb_servo_obj[0].pin = &pin_A0;
pyb_servo_obj[1].pin = &pin_A1;
pyb_servo_obj[2].pin = &pin_A2;
pyb_servo_obj[3].pin = &pin_A3;
#endif
}
void servo_timer_irq_callback(void) {
@ -100,12 +114,7 @@ void servo_timer_irq_callback(void) {
need_it = true;
}
// 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;
}
*(&TIM5->CCR1 + s->pin->pin) = s->pulse_cur;
}
}
if (need_it) {
@ -116,24 +125,12 @@ void servo_timer_irq_callback(void) {
}
STATIC void servo_init_channel(pyb_servo_obj_t *s) {
uint32_t pin;
uint32_t channel;
switch (s->servo_id) {
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;
default: return;
}
static const uint8_t channel_table[4] =
{TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4};
uint32_t channel = channel_table[s->pin->pin];
// GPIO configuration
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.Pin = pin;
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);
mp_hal_pin_config(s->pin, MP_HAL_PIN_MODE_ALT, MP_HAL_PIN_PULL_NONE, GPIO_AF2_TIM5);
// PWM mode configuration
TIM_OC_InitTypeDef oc_init;
@ -178,7 +175,7 @@ MP_DEFINE_CONST_FUN_OBJ_2(pyb_pwm_set_obj, pyb_pwm_set);
STATIC void pyb_servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
pyb_servo_obj_t *self = self_in;
mp_printf(print, "<Servo %lu at %luus>", self->servo_id, 10 * self->pulse_cur);
mp_printf(print, "<Servo %lu at %luus>", self - &pyb_servo_obj[0] + 1, 10 * self->pulse_cur);
}
/// \classmethod \constructor(id)