Positioning servomechanism for DC motor

&

DC

01/400
00/343 , 22.07.2007.

............................................................................................................................ 2

................................................................................................................................... 3

........................................................................................................... 4

................................................................................................... 6

e PWM-a ....................................................................................................... 10

USART ............................................................................................................................. 11

.................................................................................................... 12

............................................................................................ 13

.......................................................................................... 15

................................................................................ 20

........................................................................... 21

C .............................................................................................................. 23

.................................................................................................................... 33

2

.
, .

.
USB .
1Hz GPS.

( 100ms).
.

. MSP430F449 Texas Instruments
C- .

3

USB
. USB
UART-a USB .

.

duty
cycle PWM-a. PWM-a nMOS
.

.
C
.

-

4

__interrupt void mainroutine() 100µs
Timer B 10kHz.
readEncoders() 100.
control_calc() duty
cycle PWM-a. T=10ms.
100.

.

TBCCRx Timer- B PWM
duty cycle
PWM-a.

5

,
, .

. PID
P D .
.
.

.
. ,
,
.

T = 0.01; % [s]; perioda odabiranja regulatora pozicije
%---------------------------------------------------------------------%
% ELEKTRICNI PODSISTEM
Km = 1; % pojacanje aktuatora
Tmax = 10; % [Nm]; maksimalna vrednost pokretackog momenta
%---------------------------------------------------------------------%
% PARAMETRI MEHANICKOG PODSISTEMA SA KRUTOM OSOVINOM
Jm = 0.0008; % [kgm2]; inercija elektricne masine
JL = 0.0002; % [kgm2]; inercija opterecenja
J = Jm + JL; % [kgm2]; ukupna inercija
%---------------------------------------------------------------------%
% DAVAC-ENKODER
N = 10; % [broj bita]; rezolucija enkodera
Kn = 2^N/(2*pi); % prenosni odnos enkodera; [izraz (11)]
%---------------------------------------------------------------------%
maxbrzina=20;
% PRORACUN PARAMETARA REGULATORA POZICIJE
sigma=1/1.4667;
Dopt=sigma^4;
Popt=4*sigma^3-sigma^4-1;
Iopt=6*sigma^2+sigma^4-3;

Ki=(2*J*Iopt)/(Kn*Km*T^2);
Kp=(2*J*Popt)/(Kn*Km*T^2);
Kd=(2*J*Dopt)/(Kn*Km*T^2);
ogranicenje=maxbrzina*Kn*T*Kd;

6

SIMULINK. :

.
(
).

.

.
,
.
,
. ,
, .

.

7

.
.
,
.
( ) ,
.
Tmax.

,
, . y2

y1 .
y1 . y1
,
( ):

2Tmax ∆θ
fp( ∆θ ) =
J
a ωmax ( )

8

.
.
0.2rad .

,
, .
. ,
. ,
. .
, ,
, , .
.

:

. C - sqrt
2000 .
root
500 .

9

PWM-

e PWM-a
PWM je Timer B up-down
10kHz. PWM-a MC33883 4
NMOS ( ).

. ,

. ,
, .

ENABLE,
, . ,
,
.

nMOS :

10

USART

USART
.
.
MSP- USART-a.
8- .
115200 baud-a USART
clock 8MHz. .
2 USART- USART1
P4.0 P4.1. USART init().

7..3 DATA 2 reserved 1 viši/~niži biti 0 prvi/~drugi motor
:
5 5 .

,
.

7..3 DATA 2 position/~message 1 viši/~niži biti 0 prvi/~drugi motor

. 5 5
.

.
:
0xAC -
0xBC - init failed
0xEC - init failed
0xCC -

11

. 3 , , B C (
)

10kHz. void readEncoders()
__interrupt void mainroutine().

.

(01,00,10,11)
(01,11,10,00).
.
, . 10 11. XOR
0 XOR 1 = 1.
1.
0
.
.

. C

. C neutral.

12

void init()
:

- clock
MSP430 clock- MCLK, SMCLK ACLK
. MCLK
8MHz 32,768kHz.

WatchDogTimer.

-

:

P1.0 A1
P1.1 B1
P1.2 A2
P1.3 B2
P2.0 C1
P2.1 C2

:

P2.2 L1
P2.3 D1
P2.4 L2
P2.5 D2

:

P2.7

:

P3.0 EN1
P3.1 EN2
P3.4 IN11
P3.5 IN12
P3.6 IN21
P3.7 IN22

13

UART:

P4.0 UTXD
P4.1 URXD

Watchdog Timer:

P1.7 WDI

- USART

-

. TBCCRx
.
Timer B PWM
,
PWM- .
( .
) .
.
C

( NEUTRAL ). C
.
C USART-a
.
.
__interrupt void position_init().
C
.
.

.

14

.
: 3,3V, USB-
UART.
2000 x 4000 mils- .
:
• , 12V,
• 2
• 1
• 1 USB-B PC
• 2

,
.
3,3V . : ,
, , USB- UART (
FTDI ), . ,
, .

, 1,5 .
pull-up , .
, RC
.

nMOS
, .
nMOS ,
.
pMOS.
, ,
.

JTAG .

15

1 2 3 4 5 6 7 8

D D

Vcc
Vcc
Vcc IC4 Vcc
5 4
WDR Vdd
3 6
WDT RESET
R40 2 1
GND WDI
R3 R4 R5 R6
Vcc TPS3813K33 10k 10k 10k 10k

JP1
1 2
J3
3 4 R7
R38 5 6 R8 1
R39 7 8 200
R9 2
9 10 200
R10 3
200 4
HEADER 5X2 200 5
C C
C4 CON5
C5 C6 C7
100n 100n 100n 100n

A1B1A2B2 C1C2

C3 Output
Output.Sch
Vcc Vcc
Vcc
Vcc 100n 12V
12V

D1 EN1
R31 R32 R33

16
100
99
98
97
96
95
94
93
92
91
90
89
88
87
86
85
84
83
82
81
80
79
78
77
76
1k5 1k5 1k5 LED

I N11
I N12
I N21
I N22
EN2
uC1
1 75
DVcc1 P2.4/UTXD0
2 74
P6.3/A3 P2.5/URXD0

TCK
TMS

AVcc
AVss
3 73

DVss1
XT2IN
P6.4/A4 P2.6/CAOUT R37

P6.2/A2
P6.1/A1
P6.0/A0
J1 Vcc 4 72

P2.1/TB0
P2.2/TB1
P2.3/TB2

P1.0/TA0
P1.2/TA1
P1.6/CA0
P1.7/CA1
P2.0/TA2

RST/NMI
XT2OUT
P6.5/A5 P2.7/ADC12CLK

TDO/TDI
5 71

TDI/TCLK
5 R28 P6.6/A6 P3.0/STE0 330
C2 6 70
4 R29 P6.7/A7/SVSIN P3.1/SIMO0
B2 7 69

P1.1/TA0/MCLK
3 1k5
R30 VREF+ P3.2/SOMI0
1k5 A2 8 68
2 XIN P3.3/UCKL0

P1.5/TACLK/ACLK
9 67

P1.4/TBCLK/SMCLK
1 1k5 Q1 XOUT P3.4/TB3
10 66

P1.3/TBOUTH/SVSOUT
VeREF+ P3.5/TB4
CON5 11 65
LF32768 VREF-/VeREF- P3.6/TB5
12 64
C1 C2 P5.1/S0 P3.7/TB6
13 63 fromMSP
12p 12p P5.0/S1 P4.0/UTXD1 UTXD
14 62
S2 P4.1/URXD1
15 61 toMSP
S3 DVss2 URXD
J2 Vcc 16 60
S4 DVcc2
17 59
5 R25 S5 P5.7/R33
B C1 18 58 B
4 R26 S6 P5.6/R23
1k5 B1 19 57
3 R27 S7 P5.5/R13
1k5 A1 20 56
2 S8 R03
1k5 21 55
1 S9 P5.4/COM3
22 54
S10 P5.3/COM2
CON5 23 53
S11 P5.2/COM1
24 52
S12 COM0
25 51

S14
S15
S16
S17
S18
S19
S20
S21
S22
S23
S24
S25
S26
S27
S28
S29
S30
S31
S32
S33
P4.7/S34
P4.6/S35
P4.5/UCLK1/S36
P4.4/SOMI 1/S37
P4.3/SIMO 1/S38
S13 P4.2/STE1/S39
R34 R35 R36
1k5 1k5 1k5 MSP430F449

26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
Vcc

F1
1 2 napajanje
1 2
napajanje.Sch
THERMAL FUSE Vcc
J4 12V
Vout=3.3V
Nap

GND
GND
2 12V
1
12V FTDI
VCC FTDI.Sch fromMSP

3.3V
toMSP
GND TXD
RXD

A A

Title

Size Number Revision
A2
Date: 24-Jul-2007 Sheet of
File: C:Docum ents and SettingsThinkPadDesktopprotel antenaantena.ddb
Drawn By:

1 2 3 4 5 6 7 8

1 2 3 4 5 6

D D

C C

S1
12V LM117T Vcc
3 2
12V Vin +Vout Vout=3.3V
+ + C22
C21 R16 1uF/6.3V * C23

ADJ
100uF/25V 240 100nF/25V
GND

1

17
* tantal
R17
** electrolitic 390 + C24
10uF/6.3V **

B B

A A

Title

B
Drawn By:
1 2 3 4 5 6

1 2 3 4 5 6 7 8

EN2

I N22
I N21
D D
12V

+ C27 Q8
Q6 470u
R15 MOSFET N
50 MOSFET N

R23
IC2 10
C18
C8 12V 10n
1 20
Vcc G_EN C19 R19
2 19
C2 SRC_HS2 10n R24
470n 3 18 50
CP_OUT GATE_HS2
4 17 10
SRC_HS1 IN_HS2
5 16 J6
GATE _HS1 IN_LS2
6 15
IN_HS1 GATE_LS2 1
7 14
IN_LS1 GND2 2
8 13
GATE _LS1 C1
9 12 Motor1
GND1 GND_A Q9
10 11
LR_OUT Vcc2 Q7
C9
MC33883 Vcc R18 MOSFET N R20
50 MOSFET N 50
470n
C20

33n

C C

EN1

I N12
I N11
12V
Vcc
Vcc

18
+ C26 Q3
Q2 470u
R11 MOSFET N
50 MOSFET N
12V
12V
R21
IC3 10
C16
C1512V 10n
1 20
Vcc G_EN C17 R12
2 19
C2 SRC_HS2 10n R22
470n 3 18 50
CP_OUT GATE_HS2
4 17 10
SRC_HS1 IN_HS2
5 16 J5
GATE_HS1 IN_LS2
6 15
IN_HS1 GATE_LS2 1
7 14
IN_LS1 GND2 2
8 13
GATE_LS1 C1
9 12 Motor2
GND1 GND_A Q5
B 10 11 B
LR_OUT Vcc2 Q4
C10
MC33883 Vcc R13 MOSFET N R14
50 MOSFET N 50
470n
C25

33n

A A

Title

A2
Drawn By:

1 2 3 4 5 6 7 8

1 2 3 4 5 6

D D

3.3V
C C

IC1
FT232RL
4
VCCIO
CON1 1
TXD TXD
20 5
R1 Vcc RXD
1 3
5V RTS# RXD
4.7k 11
CTS#
2 16 2
USBDM USBDM DTR#
15 9
USBDP DSR#
3 10
USBDP DCD#
8 6
NC RI#
4 19 23
GND RESET# CBUS0
24 22
NC CBUS1

19
R2 27 13
OSCI CBUS2
10k 28 14
OSCO CBUS3
USB-B 12
CBUS4
17

GND
GND
GND
TEST
AGND
3V3OUT
+ C14
C11 C12 4.7u C13

7
18
21
26
25
100n 100n 100n
B B

A GND A

Title

B
Drawn By:
1 2 3 4 5 6

C1 1206 12p Q6 TO-220V MOSFET N
C10 1206 470n Q7 TO-220V MOSFET N
C11 1206 100n Q8 TO-220V MOSFET N
C12 1206 100n Q9 TO-220V MOSFET N
C13 1206 100n R1 1206 4.7k
C14 RB.2/.4 4.7u R10 1206 200
C15 1206 470n R11 1206 50
C16 1206 10n R12 1206 50
C17 1206 10n R13 1206 50
C18 1206 10n R14 1206 50
C19 1206 10n R15 1206 50
C2 1206 12p R16 1206 240
C20 1206 33n R17 1206 390
C21 RB.2/.4 100uF/25V R18 1206 50
C22 RB-.1/.2 1uF/6.3V * R19 1206 50
C23 1206 100nF/25V R2 1206 10k
C24 RB.2/.4 10uF/6.3V ** R20 1206 50
C25 1206 33n R21 1206 10
C26 RB.2/.4 470u R22 1206 10
C27 RB.2/.4 470u R23 1206 10
C3 1206 100n R24 1206 10
C4 1206 100n R25 603 1k5
C5 1206 100n R26 603 1k5
C6 1206 100n R27 603 1k5
C7 1206 100n R28 603 1k5
C8 1206 470n R29 603 1k5
C9 1206 470n R3 1206 10k
CON1 USB-B USB-B R30 603 1k5
D1 SMD_LED_B LED R31 603 1k5
F1 OSIGURAC THERMAL FUSE R32 603 1k5
IC1 SSOP28 FT232RL R33 603 1k5
IC2 SOL-20 MC33883 R34 603 1k5
IC3 SOL-20 MC33883 R35 603 1k5
IC4 SOT-23(6) TPS3813K33 R36 603 1k5
J1 SIP5 CON5 R37 1206 330
J2 SIP5 CON5 R38 603 10k
J3 SIP5 CON5 R39 603 10k
J4 KLEMA2R5 12V R4 1206 10k
J5 SIP2 Motor2 R40 1206 10k
J6 SIP2 Motor1 R5 1206 10k
JP1 IDC10 HEADER 5X2 R6 1206 10k
Q1 XTAL1 LF32768 R7 1206 200
Q2 TO-220V MOSFET N R8 1206 200
Q3 TO-220V MOSFET N R9 1206 200
Q4 TO-220V MOSFET N S1 TO-220 LM117T
Q5 TO-220V MOSFET N uC1 LQFP MSP430F449

20

C

main.c

#include <msp430x44x.h>
#include "veze.h"

float D1,D2; //brojaci pozicije:
int count1; // ocitana pozicija

int count2;

int posref1; // zadata pozicija
int posref2;

int mode; // sluzi u inicijalizaciji i prekidnoj rutini

void main(void)
{

init();

}

init.c

/*************************/
/* Inicijalizacija */
/*************************/

// U toku inicijalizacije, na P2.7 trepce led dioda sa periodom oko 100/100ms,
// kada se inicijalizacija zavrsi, dioda sija stalno

#include "veze.h"

void init()
{

//stabilizacija oscilatora
volatile unsigned int i;
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
SCFI0 |= FN_4; // x2 DCO frequency, 8MHz nominal DCO
SCFQCTL = 121; // (121+1) x 32768 x 2 = 7.99 Mhz
FLL_CTL0 |= DCOPLUS + XCAP18PF; // DCO+ set so freq = xtal x D x N+1
for(i=0; i<10; i++) ; // Delay for FLL to lock, only needed at
// initial power up

// Port 1:
P1DIR = 0xF0; // P1(0..3 ulazi enc)
P1SEL = 0x00;
P1OUT = 0x00; // Resetovan pin za WDT

// Port 2:
P2DIR = 0xC0; // 0..5 (c1,c2,l1,d1,l2,d2)
P2SEL = 0x00;
P2OUT |=0x80; // Ukljucena signalizaciona dioda
P2IFG = 0x00;
P2IES = 0xFF; // Stavili smo pull-up, silazna ivica int

// Port 3:
P3DIR = 0xFF; //4..7 izlazi PWM, timerB, out3,4,5,6

23

P3SEL = 0xF0;

// Port 4:
P4DIR = 0xFD; //USART1, P4.0 TX, P4.1 RX
P4SEL = 0x03;

// Kasnije cemo ukljuciti interapte
//podesiti interrupt edge select prema potrebama

// inicijalizacija USART:
ME2 |= UTXE1; // Enabled USART1 TXD
UCTL1 |= CHAR+PENA; // 8-bit character, parity enabled
UTCTL1 |= SSEL1; // UCLK = SMCLK
URCTL1 |= URXEIE;
// ukljucen prijem podataka sa greskom
UBR01 = 0x45; // 8MHz 115200
UBR11 = 0x00; // 8MHz 115200
UMCTL1 = 0x00; // 8MHz 115200 modulation
UCTL1 &= ~SWRST; // Initalize USART state machine UKLJUCITI TEK POSLE INIT
// I JAVITI SE RACUNARU

posref1 = 1024; //obavezno iskljuciti prekide kada menjamo vrednosti posref i count
posref2 = 1024; //tj. u prekidnoj rutini menjamo vrednosti i ne dozvoljavamo prekide
count1 = 1024;
count2 = 1024;

D1=0;
D2=0;
TBCCR0 = PEAK; // PWM Period/2

//prvi motor
TBCCTL3 = OUTMOD_6; // TBCCR3 toggle/set
TBCCR3 = (PEAK*(1-D1)+TD)/2; // TBCCR3 PWM set
TBCCTL4 = OUTMOD_2; // TBCCR4 /* PWM output mode: 2 - PWM toggle/reset
TBCCR4 = TBCCR3-TD; // TBCCR4 PWM reset

//drugi motor
TBCCTL5 = OUTMOD_6; // TBCCR5 toggle/set
TBCCR5 = (PEAK*(1-D2)+TD)/2; // TBCCR5 PWM set
TBCCTL6 = OUTMOD_2; // TBCCR6 /* PWM output mode: 2 - PWM toggle/reset */
TBCCR6 = TBCCR5-TD; // TBCCR6 PWM reset

TBCTL = TBSSEL_2 + MC_3 + TBIE; // ACLK, up-down mode, ukljuceni prekidi
_BIS_SR(GIE); // GIE
P2IE = 0x0C; // Ukljuceni su samo za prvi motor
P3OUT |=0x01; // Ukljucujemo energetsku elektroniku,
// ovaj bit u spoljnjem hardveru blokira
// izlazne tranzistore motora

mode=1;
if (P2IN & 0x04) mode=2; //U slucaju da je motor vec u krajnjoj levoj poziciji
//krecemo inicijalizaciju na desno

while (mode==1) //cekamo prekid na P2;
{
delay();
posref1-=5;
P2OUT ^= 0x80; // toggle dioda za treptanje
}

while (mode==2) //ako se obrnuo smer u rutini, inace skip this
{
delay();
posref1+=5;

24

}

P2IE = 0x32; //ukljuceni su samo za drugi motor
if (P2IN & 0x10) mode=4; //U slucaju da je motor vec u krajnjoj levoj poziciji

//krecemo inicijalizaciju na desno
while (mode==3)
{
delay();
posref2-=5;
}

while (mode==4)
{
delay();
posref1+=5;
}

P2IE = 0;
P2OUT |= 0x80; // dioda ukljucena, init zavrsena

// u prekidu TA se ocitava poz, i racuna upravljanje svaki 100 put
//ukljuciti prekide p2
//konst brzinom init 1 motor, dok drugi radi poziciono
//cekati prekid
//2 motor
//cekati prekid
//poslati poruku racunaru da moze da krene sa radom

ME2 |= URXE1; // Enabled USART1 RX
IE2 |= URXIE1; // Enabled USART1 RX interrupt

while (!(IFG2 & UTXIFG1)); // USART1 TX buffer ready?
TXBUF1 = 0xCC; //inicijalizacija uspela
}

delay.c

long del;
void delay(void)
{
del = 40000; // Delay treba da bude oko 100ms
do (del--);
while (del != 0);
}

position_init.c

// Kodovi za signaliziranje:
// 0xBC - prvi motor init failed
// 0xEC - drugi motor init failed
// 0xCC - inicijalizacija uspela
#include "veze.h"

25

#pragma vector = PORT2_VECTOR
__interrupt void position_init()

{
int flagreg = P2IFG;

//prvi motor init
if (flagreg & 0x04)
{
mode = 2;
P2IFG &= ~0x04;
}

if (flagreg & 0x08)
{
P3OUT &= ~0x01; //Iskljucujemo energetsku elektroniku
TXBUF1 = 0xBC; //Kod za prvi motor init failed
P2IFG &= ~0x08;

}

if (flagreg & 0x01)
{
mode = 3;
posref1= NEUTRAL;
count1 = NEUTRAL;
P2IFG &= ~0x01;
}

//Drugi motor init
if (flagreg & 0x10)
{
mode = 4;
P2IFG &= ~0x10;
}

if (flagreg & 0x20)
{
P3OUT &= ~0x02; //Iskljucujemo energetsku elektroniku
TXBUF1 = 0xEC; //Kod za drugi motor init failed
P2IFG &= ~0x20;

}

if (flagreg & 0x02)
{
mode = 0;
posref2= NEUTRAL;
count2 = NEUTRAL;
P2IFG &= ~0x02;

}

}

26

main_routine.c

#include "veze.h"

#pragma vector = TIMERB0_VECTOR
__interrupt void mainroutine()
{
static int flag = 0, counter100 = 98; //Ovakve su vrednosti zbog prvog ulaska u
//rutinu i prvog racuna upravljanja
static int new_ccr3;

if (flag == 1)
{
TBCCR3 = new_ccr3;
TBCCR4 = new_ccr4;
TBCCR5 = new_ccr5;
TBCCR6 = new_ccr6;
flag = 0;
};

readEncoders();
counter100++;

if (counter100 == 99)
{
_BIS_SR(GIE); // omoguciti interapte

control_calc1(); // racunanje upravljanja za 1. motor vraca D1

new_ccr4 = (TD + (1 - D1) * PEAK) / 2;
new_ccr3 = new_ccr4 - TD;

flag = 1;

};

if (counter100 == 100)
{
counter100 = 0;
_BIS_SR(GIE); // omoguciti interapte

control_calc2(); // racunanje upravljanja za 2. motor vraca D2

new_ccr6 = (TD + (1 - D2) * PEAK) / 2;
new_ccr5 = new_ccr6 - TD;

flag = 1;

P1OUT ^= 0x80; //watchdog timer toggle
};

}

readencoders.c

/**********************************************/
/* Ocitavanje enkodera i proracun pozicije */
/**********************************************/

27

void readEncoders()
{
static int A1old, A2old, B1old, B2old, first=0;
int A1new, A2new, B1new, B2new;
int reading, mask;

reading=P1IN; //0..3 (A1, B1, A2, B2) enkoderi

if (first==0)
{
mask=1;
A1old=reading&mask;
mask=mask<<1;
B1old=reading&mask;
B1old=B1old>>1;
mask=mask<<1;
A2old=reading&mask;
mask=mask<<1;
B2old=reading&mask;
B2old=B2old>>1;
first=1;
};

mask=1;
A1new=reading&mask;
mask=mask<<1;
B1new=reading&mask;
B1new=B1new>>1;
mask=mask<<1;
A2new=reading&mask;
mask=mask<<1;
B2new=reading&mask;
B2new=B2new>>1;

if ((A1old != A1new) || (B1old != B1new))
{

if (A1old ^ B1new)
{
count1++;

}
else
{
count1--;

};
};

if ((A2old != A2new) || (B2old != B2new))
{
if (A2old ^ B2new)
{
count2++;
}
else
{
count2--;

};
};

A1old=A1new;
A2old=A2new;
B1old=B1new;

28

B2old=B2new;
};
/**********************************************/

control_calc1.c

#define PTORQMAX1 5
#define LIMIT1 20 //parametri motora

#include "veze.h"

void control_calc1(void)
{
int tetanew, tetaref, deltet, greska, pripoz, znak, koren;
static int tetaold1, first = 0;
static real yoneold1 = 0;
real reftorque;
real pkkp=0.0063;
real pkki=6.4053e-004;
real pkkd=0.0265;

/*
sigma=1/1.4667;
Dopt=sigma^4;
Popt=4*sigma^3-sigma^4-1;
Iopt=6*sigma^2+sigma^4-3;

Ki=(2*J*Iopt)/(Kn*Km*T^2);
Kp=(2*J*Popt)/(Kn*Km*T^2);
Kd=(2*J*Dopt)/(Kn*Km*T^2);
Izracunao Matlab
*/

real krikelis=11.0778; // konstanta izvucena iz korena sqrt(2*Tmax/Kn/J)
real nelin, delty, sumiks, min;
real reftorque, yonenew;

// u prvom pozivu f-je postavljamo tetaold1
if (first == 0) {tetaold1 = count1;};

// proracun upravljanja za prvi motor

tetanew = count1;
tetaref = posref1;

greska = tetaref - tetanew;
deltet = greska;
if (deltet > 0) {deltet = -deltet;};
pripoz = tetanew - tetaold1;
delty = pkki * greska - pkkp * pripoz;
znak = 1;
sumiks = delty + yoneold1;
if (sumiks < 0) {sumiks = -sumiks; znak = 1;};
koren = root(deltet);
nelin = krikelis * koren;
min = sumiks;
if (nelin < min) min = nelin;
if (LIMIT1 < min) min = LIMIT1;
yonenew = min * znak;
reftorque = yonenew - pkkd * pripoz;
yoneold1 = yonenew;
tetaold1 = tetanew;

if (reftorque > PTORQMAX1) {reftorque = PTORQMAX;};

29

Positioning servomechanism for DC motor

Positioning servomechanism for DC motor

Recommended

Recommended

More Related Content

Featured

Featured (20)

Positioning servomechanism for DC motor