1 | /*---------------------------------------------------
|
2 | ROUTINE Name : fire_time_calc
|
3 |
|
4 | Description: Calculates the value for Timer 1 as phase angle
|
5 | Input/Output: Set Speed (us of one motor turn)/None
|
6 | Comments:
|
7 | Execution Time: 80us
|
8 | ------------------------------------------------------------*/
|
9 | void fire_time_calc (unsigned int set_speed)
|
10 | {
|
11 | signed long temp;
|
12 | unsigned int speed_avg;
|
13 | signed int kp_temp;
|
14 | signed int ki_temp;
|
15 |
|
16 | speed_avg = Get_turn_time (); //Get current speed in time per turn in us
|
17 |
|
18 |
|
19 | if((speed_avg < 50000) && flags.stop == 0)
|
20 | {
|
21 | flags.startup = 0; // motor is turning
|
22 | }
|
23 | else if(speed_avg >= 50000 && flags.stop == 0)
|
24 | {
|
25 | flags.startup = 1; // <900rpm
|
26 | }
|
27 |
|
28 | kp_temp = 0 - Get_kp(set_speed);
|
29 | ki_temp = 0 - Get_ki(set_speed);
|
30 |
|
31 |
|
32 | //----------------------------------
|
33 | // firtime = KP * e - KP * (1-T/Tn) * e(t-1) + firetime (t-1) ->FH Skrip
|
34 | //e = w - x; //Vergleich
|
35 | //esum = esum + e; //Integration I-Anteil
|
36 | //y = Kp*e + Ki*Ta*esum + Kd/Ta*(e – ealt); //Reglergleichung
|
37 | //ealt = e; //-> rn-wissen.de
|
38 | //---------------------------
|
39 |
|
40 | if(flags.startup == 0 && flags.stop == 0) //normal operation
|
41 | {
|
42 | SpeedError =(signed long)set_speed - (signed long) speed_avg; // Calculate Differenece Between Desired Speed and Actual Speed
|
43 | // if speederror <0 -> motorspeed is to slow
|
44 | // if speederror >0 -> motorspeed is to high
|
45 |
|
46 | if(kp>kp_temp) //soft start
|
47 | kp--;
|
48 | else if (kp<kp_temp)
|
49 | kp++;
|
50 | else
|
51 | kp = kp_temp;
|
52 |
|
53 | SpeedProportional = SpeedError * kp/100;
|
54 |
|
55 | if(ki>ki_temp) //soft start
|
56 | ki--; //negative
|
57 | else if(ki<ki_temp)
|
58 | ki++;
|
59 | else
|
60 | ki = ki_temp;
|
61 |
|
62 | esum = esum + SpeedError;
|
63 |
|
64 | if (esum < -200000) {esum = -200000;} //Limit Integral part
|
65 | if (esum > 200000) {esum = 200000;}
|
66 |
|
67 | SpeedIntegral = esum*ki/10000; // Translation from speed to fire_time incl. in KP + KI
|
68 |
|
69 |
|
70 |
|
71 | temp = (SpeedProportional + SpeedIntegral); // calculate time in which triac is fired (0 - negHW time)
|
72 |
|
73 | temp = (signed long)(neg_halfwave-MAX_FIR_TIME) - temp; //change to fireangle time (time in which no voltage at motor)
|
74 |
|
75 | if(temp<MIN_FIR_TIME) // CHECK FOR LOWER LIMIT OF FIRING TIME
|
76 | {
|
77 | fir_time = MIN_FIR_TIME; //limit to min fireangle ~0°
|
78 | }
|
79 | else if(temp>(signed long)(neg_halfwave-MAX_FIR_TIME)) // slowest possibility
|
80 | {
|
81 | fir_time = neg_halfwave-MAX_FIR_TIME; //limit to max fireangle ~180°
|
82 | }
|
83 | else
|
84 | fir_time = (unsigned int) temp; //normal operation
|
85 |
|
86 | }
|
87 |
|
88 |
|
89 | if(flags.startup == 1 && flags.stop == 0)//open loop startup
|
90 | {
|
91 | if(fir_time<MIN_FIR_TIME) // CHECK FOR LOWER LIMIT OF FIRING TIME
|
92 | {
|
93 | fir_time = MIN_FIR_TIME;
|
94 | }
|
95 | else
|
96 | {
|
97 | fir_time-= SOFTSTART_STEPS;
|
98 | }
|
99 | }
|
100 | }
|
101 |
|
102 |
|
103 | /*-----------------------------------------------------------------------------
|
104 | ROUTINE Name : Get_ki
|
105 |
|
106 | Description: Calculates the ki Parameter in regard of setspeed and state
|
107 | Input/Output: setspeed/ki
|
108 | Comments:
|
109 | Execution Time:
|
110 | -----------------------------------------------------------------------------*/
|
111 | unsigned int Get_ki (unsigned int set_n)
|
112 | {
|
113 | unsigned long param;
|
114 |
|
115 | if(set_n > 5800)
|
116 | {
|
117 | param = 96-(unsigned long)set_n*29/10000;
|
118 | }
|
119 | else
|
120 | {
|
121 | param = 284-(unsigned long)set_n*34/1000;
|
122 | }
|
123 |
|
124 | return((unsigned int)param);
|
125 | }
|
126 | /*-----------------------------------------------------------------------------
|
127 | ROUTINE Name : Get_kp
|
128 |
|
129 | Description: Calculates the kp Parameter in regard of setspeed and state
|
130 | Input/Output: setspeed/kp
|
131 | Comments:
|
132 | yp = 1E-06x2 - 0,0086x + 25,381
|
133 | Execution Time:
|
134 | -----------------------------------------------------------------------------*/
|
135 | unsigned int Get_kp (unsigned int set_n)
|
136 | {
|
137 | unsigned long param;
|
138 |
|
139 | if(set_n > 5800)
|
140 | {
|
141 | param = 88-(unsigned long)set_n*58/10000;
|
142 | }
|
143 | else
|
144 | {
|
145 | param = 420-(unsigned long)set_n*618/10000;
|
146 | }
|
147 | return((unsigned int)param);
|
148 | }
|