1 | EQep1Regs.QDECCTL.bit.QSRC=00; // QEP quadrature count mode
|
2 | EQep1Regs.QEPCTL.bit.FREE_SOFT=2; //unaffected by emulation suspend
|
3 | EQep1Regs.QEPCTL.bit.PCRM=00; // PCRM=00 mode - QPOSCNT reset on
|
4 | ///index event
|
5 | EQep1Regs.QEPCTL.bit.UTE=1; // Unit Timeout Enable
|
6 | EQep1Regs.QEPCTL.bit.QCLM=1; // Latch on unit time out
|
7 | EQep1Regs.QEPCTL.bit.IEL = 1; // IEL_RISING
|
8 | EQep1Regs.QEPCTL.bit.IEI = 2; // IEL_RISING
|
9 | EQep1Regs.QPOSMAX=4*500 - 1;
|
10 | EQep1Regs.QEPCTL.bit.QPEN=1; // QEP enable
|
11 |
|
12 | EALLOW; // Enable EALLOW
|
13 | GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1; // GPIO20 is EQEP1A
|
14 | GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1; // GPIO21 is EQEP1B
|
15 | GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1; // GPIO23 is EQEP1I
|
16 | EDIS;
|
17 |
|
18 |
|
19 | In interrupt routine mit 10khz = fabtast
|
20 |
|
21 | interrupt void main_isr(void)
|
22 | {
|
23 | float Tmp1;
|
24 | //……………
|
25 | //…………….
|
26 | //………………
|
27 |
|
28 |
|
29 |
|
30 | if (SpeedMessungcount == SpeedMessungAbtast)
|
31 | // damit kann ich drehzahl_abtastfrequenz kleiner
|
32 | // machen , drehzahl_abtastfrequenz = fabtast/ SpeedMessungAbtast
|
33 | {
|
34 | // Check the position counter for EQEP1
|
35 | RawTheta = (Uint16)EQep1Regs.QPOSCNT ;// QPOSCNT 32 bit
|
36 |
|
37 | // Compute the mechanical angle
|
38 | MechTheta = RawTheta/(4*500);
|
39 |
|
40 | // erste verfahren, da MechTheta leigt im interval (0 - 1), dann es gibt unableitbaren Bereich
|
41 |
|
42 | if ((MechTheta < 0.95)&&(MechTheta > 0.05)) // ableitbar Bereich
|
43 | Tmp1 = fabtast*(MechTheta - OldMechTheta)*60;//rpm
|
44 | else Tmp1 = Speed;//rpm
|
45 |
|
46 |
|
47 |
|
48 |
|
49 | /*
|
50 | // zeite verfahren,
|
51 |
|
52 | if (DirectionQep == 0) // POSCNT is counting down
|
53 | {
|
54 | if (MechTheta > OldMechTheta)
|
55 | Tmp1 = -(1.0 - MechTheta + OldMechTheta)*fabtast;
|
56 | //x2-x1 should be negative
|
57 | else
|
58 | Tmp1 = (v->MechTheta - v->OldMechTheta)*v->K1;
|
59 | }
|
60 | else if (DirectionQep ==1 ) // POSCNT is counting up
|
61 | {
|
62 | if (MechTheta < OldMechTheta)
|
63 | Tmp1 = (1.0 + MechTheta - OldMechTheta)* fabtast;
|
64 | else
|
65 | Tmp1 = (MechTheta - OldMechTheta)* fabtast;
|
66 | // x2-x1 should be positive
|
67 | }
|
68 |
|
69 | Tmp1 *= 60; //rpm
|
70 |
|
71 | */
|
72 |
|
73 |
|
74 | // Low-pass filter
|
75 | Tmp1 = K2*Speed + K3*Tmp1; // e.g. fc = 50hz , 1. Order
|
76 |
|
77 | if (Tmp1 > BaseRpm) // BaseRpm = 3000 rpm
|
78 | Speed = BaseRpm;
|
79 | else if (Tmp1 < - BaseRpm)
|
80 | Speed = - BaseRpm;
|
81 | else
|
82 | Speed = Tmp1;
|
83 |
|
84 | // Update the electrical angle
|
85 | OldMechTheta = MechTheta;
|
86 |
|
87 | SpeedMessungcount = 1;
|
88 | }
|
89 | else SpeedMessungcount++;
|
90 | }
|