Nikola Cvetković / Mbed 2 deprecated 19E042PIM_V5_Z1

19E042PIM Servomotor SG90

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }