Nikola Cvetković
/
19E042PIM_V5_Z1
19E042PIM Servomotor SG90
main.cpp@0:35383655148b, 2022-01-04 (annotated)
- Committer:
- tzwell
- Date:
- Tue Jan 04 14:34:40 2022 +0000
- Revision:
- 0:35383655148b
First publish, first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tzwell | 0:35383655148b | 1 | /* |
tzwell | 0:35383655148b | 2 | * An example of motor control, using step SG90 and NUCLEO-L476RG. |
tzwell | 0:35383655148b | 3 | * |
tzwell | 0:35383655148b | 4 | * Katedra za Elektroniku i digitalne sisteme |
tzwell | 0:35383655148b | 5 | * Elektrotehnicki fakultet |
tzwell | 0:35383655148b | 6 | * Beograd |
tzwell | 0:35383655148b | 7 | * |
tzwell | 0:35383655148b | 8 | * Decembar 2021. |
tzwell | 0:35383655148b | 9 | * |
tzwell | 0:35383655148b | 10 | */ |
tzwell | 0:35383655148b | 11 | |
tzwell | 0:35383655148b | 12 | /* |
tzwell | 0:35383655148b | 13 | * Biblioteke za uvoz: |
tzwell | 0:35383655148b | 14 | */ |
tzwell | 0:35383655148b | 15 | #include "mbed.h" |
tzwell | 0:35383655148b | 16 | #include "mb_pins.h" |
tzwell | 0:35383655148b | 17 | |
tzwell | 0:35383655148b | 18 | /* |
tzwell | 0:35383655148b | 19 | * Definisanje makroa: |
tzwell | 0:35383655148b | 20 | */ |
tzwell | 0:35383655148b | 21 | #define DEBOUNCE_DELAY_MS 50 |
tzwell | 0:35383655148b | 22 | #define MOTOR_PERIOD_MS 20 |
tzwell | 0:35383655148b | 23 | #define MOTOR_PULSE_INIT_MS 1 |
tzwell | 0:35383655148b | 24 | #define MOTOR_PULSE_INC_US 50 // 4.5 degrees increment |
tzwell | 0:35383655148b | 25 | #define MAX_PULSE_US 3000 |
tzwell | 0:35383655148b | 26 | #define MIN_PULSE_US 500 |
tzwell | 0:35383655148b | 27 | |
tzwell | 0:35383655148b | 28 | |
tzwell | 0:35383655148b | 29 | /* |
tzwell | 0:35383655148b | 30 | * Globalne promenljive: |
tzwell | 0:35383655148b | 31 | */ |
tzwell | 0:35383655148b | 32 | PwmOut motor_out(PB_0); // orange -- A3 pin |
tzwell | 0:35383655148b | 33 | InterruptIn clkwise (MB_SW1); |
tzwell | 0:35383655148b | 34 | InterruptIn cntclkwise (MB_SW2); |
tzwell | 0:35383655148b | 35 | |
tzwell | 0:35383655148b | 36 | int clkwise_dir = 0; |
tzwell | 0:35383655148b | 37 | int cntclkwise_dir = 0; |
tzwell | 0:35383655148b | 38 | |
tzwell | 0:35383655148b | 39 | /* |
tzwell | 0:35383655148b | 40 | * Deklaracija funkcija: |
tzwell | 0:35383655148b | 41 | */ |
tzwell | 0:35383655148b | 42 | void ISR_clkwise(void); |
tzwell | 0:35383655148b | 43 | void ISR_cntclkwise(void); |
tzwell | 0:35383655148b | 44 | |
tzwell | 0:35383655148b | 45 | /* |
tzwell | 0:35383655148b | 46 | * Glavna funkcija: |
tzwell | 0:35383655148b | 47 | */ |
tzwell | 0:35383655148b | 48 | |
tzwell | 0:35383655148b | 49 | int main() |
tzwell | 0:35383655148b | 50 | { |
tzwell | 0:35383655148b | 51 | clkwise.fall(&ISR_clkwise); |
tzwell | 0:35383655148b | 52 | cntclkwise.fall(&ISR_cntclkwise); |
tzwell | 0:35383655148b | 53 | |
tzwell | 0:35383655148b | 54 | motor_out.period_ms(MOTOR_PERIOD_MS); |
tzwell | 0:35383655148b | 55 | motor_out.pulsewidth_ms(MOTOR_PULSE_INIT_MS); |
tzwell | 0:35383655148b | 56 | |
tzwell | 0:35383655148b | 57 | int pulsewidth = 0; |
tzwell | 0:35383655148b | 58 | while(1) |
tzwell | 0:35383655148b | 59 | { |
tzwell | 0:35383655148b | 60 | if (clkwise_dir) |
tzwell | 0:35383655148b | 61 | { |
tzwell | 0:35383655148b | 62 | pulsewidth = motor_out.read()*MOTOR_PERIOD_MS*1000; |
tzwell | 0:35383655148b | 63 | printf("Pulsewidth when SW1 pressed: %d us\r\n", pulsewidth); |
tzwell | 0:35383655148b | 64 | if (pulsewidth < MAX_PULSE_US - MOTOR_PULSE_INC_US) |
tzwell | 0:35383655148b | 65 | { |
tzwell | 0:35383655148b | 66 | pulsewidth = pulsewidth + MOTOR_PULSE_INC_US; |
tzwell | 0:35383655148b | 67 | printf("Pulsewidth after SW1 increment: %d us\r\n", pulsewidth); |
tzwell | 0:35383655148b | 68 | motor_out.pulsewidth_us(pulsewidth); |
tzwell | 0:35383655148b | 69 | } |
tzwell | 0:35383655148b | 70 | clkwise_dir = 0; |
tzwell | 0:35383655148b | 71 | //wait_ms(DEBOUNCE_DELAY_MS); |
tzwell | 0:35383655148b | 72 | } |
tzwell | 0:35383655148b | 73 | else if(cntclkwise_dir) |
tzwell | 0:35383655148b | 74 | { |
tzwell | 0:35383655148b | 75 | pulsewidth = motor_out.read()*MOTOR_PERIOD_MS*1000; |
tzwell | 0:35383655148b | 76 | printf("Pulsewidth when SW2 pressed: %d us\r\n", pulsewidth); |
tzwell | 0:35383655148b | 77 | if (pulsewidth > MIN_PULSE_US + MOTOR_PULSE_INC_US) |
tzwell | 0:35383655148b | 78 | { |
tzwell | 0:35383655148b | 79 | pulsewidth = pulsewidth - MOTOR_PULSE_INC_US; |
tzwell | 0:35383655148b | 80 | printf("Pulsewidth after SW1 increment: %d us\r\n", pulsewidth); |
tzwell | 0:35383655148b | 81 | motor_out.pulsewidth_us(pulsewidth); |
tzwell | 0:35383655148b | 82 | } |
tzwell | 0:35383655148b | 83 | cntclkwise_dir = 0; |
tzwell | 0:35383655148b | 84 | //wait_ms(DEBOUNCE_DELAY_MS); |
tzwell | 0:35383655148b | 85 | } |
tzwell | 0:35383655148b | 86 | } |
tzwell | 0:35383655148b | 87 | } |
tzwell | 0:35383655148b | 88 | |
tzwell | 0:35383655148b | 89 | |
tzwell | 0:35383655148b | 90 | /* |
tzwell | 0:35383655148b | 91 | * Definicija funkcija: |
tzwell | 0:35383655148b | 92 | */ |
tzwell | 0:35383655148b | 93 | void ISR_clkwise(void){ |
tzwell | 0:35383655148b | 94 | if(!clkwise.read()) |
tzwell | 0:35383655148b | 95 | { |
tzwell | 0:35383655148b | 96 | wait_ms(DEBOUNCE_DELAY_MS); |
tzwell | 0:35383655148b | 97 | if (!clkwise.read()) clkwise_dir = 1; |
tzwell | 0:35383655148b | 98 | } |
tzwell | 0:35383655148b | 99 | } |
tzwell | 0:35383655148b | 100 | |
tzwell | 0:35383655148b | 101 | void ISR_cntclkwise(void){ |
tzwell | 0:35383655148b | 102 | if(!cntclkwise.read()) |
tzwell | 0:35383655148b | 103 | { |
tzwell | 0:35383655148b | 104 | wait_ms(DEBOUNCE_DELAY_MS); |
tzwell | 0:35383655148b | 105 | if (!cntclkwise.read()) cntclkwise_dir = 1; |
tzwell | 0:35383655148b | 106 | } |
tzwell | 0:35383655148b | 107 | } |