ServoMotor.cpp 5.42 KB
Newer Older
Werner Sembach's avatar
Werner Sembach committed
1
2
// huge codechuncks copied from ezs_dac.c by Florian Jung of libEZS

3
#include "ServoMotor.h"
Werner Sembach's avatar
Werner Sembach committed
4

5
6
#include <cstdio>

Werner Sembach's avatar
Werner Sembach committed
7
8
9
#include <cyg/hal/hal_arch.h>

#include <libopencm3/cm3/common.h>
10
#include <libopencm3/stm32/timer.h>
Werner Sembach's avatar
Werner Sembach committed
11
12
13
#include <libopencm3/stm32/f4/rcc.h>
#include <libopencm3/stm32/f4/gpio.h>

14
15
16
17
18
19
#define GPIO_D12 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,12, 2, PUSHPULL,NONE,LOW)
#define GPIO_D13 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,13, 2, PUSHPULL,NONE,LOW)
#define GPIO_D14 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,14, 2, PUSHPULL,NONE,LOW)
#define GPIO_D15 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,15, 2, PUSHPULL,NONE,LOW)

// The timer ticks per microsecond. Can be adjusted by measuring PWM on an oscilloscope.
20
#define PWM_TIMER_TICKS_PER_MICROSECOND 84
21
22
23

// PWM Period. Must be more then maximum high time.
#define PWM_PERIOD (3000 * PWM_TIMER_TICKS_PER_MICROSECOND)
Werner Sembach's avatar
Werner Sembach committed
24

25
26
27
28
// Use this to calibrate the minimum and maximum position of your servo. Default: 150 and 300
#define MIN_UPTIME 155
#define MAX_UPTIME 305

Werner Sembach's avatar
Werner Sembach committed
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
static void pwm_init(uint32_t timer, uint8_t channel, uint32_t period) {
    // Function stolen from somewhere on stackoverflow

    // Convert channel number to internal rep
    enum tim_oc_id chan;
    switch (channel) {
        case 1:
            chan = TIM_OC1;
            break;
        case 2:
            chan = TIM_OC2;
            break;
        case 3:
            chan = TIM_OC3;
            break;
        case 4:
            chan = TIM_OC4;
            break;
        default:
            chan = TIM_OC4;
            break;
    }

    // Timer Base Configuration
    // timer_reset(timer);
    // clock division, Center-aligned mode selection, TIMx_CR1 DIR: Direction
    timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
    timer_continuous_mode(timer);                       // Disables TIM_CR1_OPM (One pulse mode)
    timer_set_period(timer, period);                    // Sets TIMx_ARR
    timer_set_prescaler(timer, 1);                      // Adjusts speed of timer
    timer_set_clock_division(timer, 0);                 // Adjusts speed of timer
    timer_set_master_mode(timer, TIM_CR2_MMS_UPDATE);   // Master Mode Selection
    timer_enable_preload(timer);                        // Set ARPE bit in TIMx_CR1

    // Channel-specific settings
    timer_set_oc_value(timer, chan, 0);                 // sets TIMx_CCRx
    timer_set_oc_mode(timer, chan, TIM_OCM_PWM1);       // Sets PWM Mode 1
    timer_enable_oc_preload(timer, chan);               // Sets OCxPE in TIMx_CCMRx
    timer_set_oc_polarity_high(timer, chan);            // set desired polarity in TIMx_CCER
    timer_enable_oc_output(timer, chan);                // set CCxE bit in TIMx_CCER  (enable output)

    // Initialize all counters in the register
71
    switch(timer) {
Werner Sembach's avatar
Werner Sembach committed
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
        case TIM1:
            TIM1_EGR |= TIM_EGR_UG;
            break;
        case TIM2:
            TIM2_EGR |= TIM_EGR_UG;
            break;
        case TIM3:
            TIM3_EGR |= TIM_EGR_UG;
            break;
        case TIM4:
            TIM4_EGR |= TIM_EGR_UG;
            break;
        case TIM5:
            TIM5_EGR |= TIM_EGR_UG;
            break;
        case TIM6:
            TIM6_EGR |= TIM_EGR_UG;
            break;
        case TIM7:
            TIM7_EGR |= TIM_EGR_UG;
            break;
        case TIM8:
            TIM8_EGR |= TIM_EGR_UG;
            break;
96
        default:;
Werner Sembach's avatar
Werner Sembach committed
97
98
99
    }
}

100
static void pwm_setup() {
Werner Sembach's avatar
Werner Sembach committed
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
    rcc_periph_clock_enable(RCC_TIM4);
    pwm_init(TIM4, 1, PWM_PERIOD);
    pwm_init(TIM4, 2, PWM_PERIOD);
    pwm_init(TIM4, 3, PWM_PERIOD);
    pwm_init(TIM4, 4, PWM_PERIOD);

    // LED channels = PD12..15
    // AHB1-bus ? _ gpIO Port D ENable
    rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPDEN);

    // AF = alternate function
    gpio_mode_setup(GPIOD, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO12 | GPIO13 | GPIO14 | GPIO15);
    // AF2 = TIM4_CH1..4
    gpio_set_af(GPIOD, GPIO_AF2, GPIO12 | GPIO13 | GPIO14 | GPIO15);
    timer_enable_counter(TIM4);
}

extern "C" void servo_init() {
    pwm_setup();
120
121
122
123
124

    CYGHWR_HAL_STM32_GPIO_SET(GPIO_D12);
    CYGHWR_HAL_STM32_GPIO_SET(GPIO_D13);
    CYGHWR_HAL_STM32_GPIO_SET(GPIO_D14);
    CYGHWR_HAL_STM32_GPIO_SET(GPIO_D15);
Werner Sembach's avatar
Werner Sembach committed
125
126
}

127
128
129
130
131
extern "C" void servo_set_percent(uint8_t servo, double position) {
    if (position > 1.0) {
        printf("Error: servo_set_percent only accepts values between 0.0 and 1.0");
        return;
    };
132
133
    switch (servo) {
        case SERVO_0:
134
            timer_set_oc_value(TIM4, TIM_OC1, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
135
136
            break;
        case SERVO_1:
137
            timer_set_oc_value(TIM4, TIM_OC2, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
138
139
            break;
        case SERVO_2:
140
            timer_set_oc_value(TIM4, TIM_OC3, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
141
142
            break;
        case SERVO_3:
143
            timer_set_oc_value(TIM4, TIM_OC4, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
144
145
146
147
            break;
        default:
            break;
    }
Werner Sembach's avatar
Werner Sembach committed
148
}
149
150
151
152
153
154
155
156

extern "C" void servo_set_degree(uint8_t servo, double position) {
    if (position > 180) {
        printf("Error: servo_set_degree only accepts values between 0 and 180");
        return;
    };
    servo_set_percent(servo, position / 180.0);
}