Nikola Cvetković
/
19E042PIM_V5_Z1
19E042PIM Servomotor SG90
Diff: main.cpp
- Revision:
- 0:35383655148b
diff -r 000000000000 -r 35383655148b main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jan 04 14:34:40 2022 +0000 @@ -0,0 +1,107 @@ +/* + * An example of motor control, using step SG90 and NUCLEO-L476RG. + * + * Katedra za Elektroniku i digitalne sisteme + * Elektrotehnicki fakultet + * Beograd + * + * Decembar 2021. + * + */ + +/* + * Biblioteke za uvoz: + */ +#include "mbed.h" +#include "mb_pins.h" + +/* + * Definisanje makroa: + */ +#define DEBOUNCE_DELAY_MS 50 +#define MOTOR_PERIOD_MS 20 +#define MOTOR_PULSE_INIT_MS 1 +#define MOTOR_PULSE_INC_US 50 // 4.5 degrees increment +#define MAX_PULSE_US 3000 +#define MIN_PULSE_US 500 + + +/* + * Globalne promenljive: + */ +PwmOut motor_out(PB_0); // orange -- A3 pin +InterruptIn clkwise (MB_SW1); +InterruptIn cntclkwise (MB_SW2); + +int clkwise_dir = 0; +int cntclkwise_dir = 0; + +/* + * Deklaracija funkcija: + */ +void ISR_clkwise(void); +void ISR_cntclkwise(void); + +/* + * Glavna funkcija: + */ + +int main() +{ + clkwise.fall(&ISR_clkwise); + cntclkwise.fall(&ISR_cntclkwise); + + motor_out.period_ms(MOTOR_PERIOD_MS); + motor_out.pulsewidth_ms(MOTOR_PULSE_INIT_MS); + + int pulsewidth = 0; + while(1) + { + if (clkwise_dir) + { + pulsewidth = motor_out.read()*MOTOR_PERIOD_MS*1000; + printf("Pulsewidth when SW1 pressed: %d us\r\n", pulsewidth); + if (pulsewidth < MAX_PULSE_US - MOTOR_PULSE_INC_US) + { + pulsewidth = pulsewidth + MOTOR_PULSE_INC_US; + printf("Pulsewidth after SW1 increment: %d us\r\n", pulsewidth); + motor_out.pulsewidth_us(pulsewidth); + } + clkwise_dir = 0; + //wait_ms(DEBOUNCE_DELAY_MS); + } + else if(cntclkwise_dir) + { + pulsewidth = motor_out.read()*MOTOR_PERIOD_MS*1000; + printf("Pulsewidth when SW2 pressed: %d us\r\n", pulsewidth); + if (pulsewidth > MIN_PULSE_US + MOTOR_PULSE_INC_US) + { + pulsewidth = pulsewidth - MOTOR_PULSE_INC_US; + printf("Pulsewidth after SW1 increment: %d us\r\n", pulsewidth); + motor_out.pulsewidth_us(pulsewidth); + } + cntclkwise_dir = 0; + //wait_ms(DEBOUNCE_DELAY_MS); + } + } +} + + +/* + * Definicija funkcija: + */ +void ISR_clkwise(void){ + if(!clkwise.read()) + { + wait_ms(DEBOUNCE_DELAY_MS); + if (!clkwise.read()) clkwise_dir = 1; + } +} + +void ISR_cntclkwise(void){ + if(!cntclkwise.read()) + { + wait_ms(DEBOUNCE_DELAY_MS); + if (!cntclkwise.read()) cntclkwise_dir = 1; + } +}