| BLDC control on ATAVRMC303 with ATxMega128A1

mc_control.c
Go to the documentation of this file.
1
11 //_____ I N C L U D E S ___________________________________________________
12 #include "config.h"
13 #include "mc_control.h"
14 #include "mc_drv.h"
15 #include "mc_interface.h"
16
17 U8 duty_cycle = 0;
18 U8 regulation_type = OPEN_LOOP;
19
20 /* Speed regulation variables */
21 S16 speed_error = 0;
22 S16 last_speed_error = 0;
23 S16 speed_integral = 0;
24 S16 speed_integ = 0;
25 S16 speed_proportional = 0;
26 S16 speed_derivative = 0;
27 S16 speed_der = 0;
28
29 /* Current regulation variables */
30 S16 cur_error = 0;
31 S16 last_cur_error = 0;
32 S16 cur_integral = 0;
33 S16 cur_integ = 0;
34 S16 cur_proportional = 0;
35 S16 cur_derivative = 0;
36 S16 cur_der = 0;
37
38
39 /************************************************************************************************************/
40 /* Speed Regulation */
41 /************************************************************************************************************/
48 U8 mc_control_speed(U8 speed_cmd)
49 {
50 U8 Duty = 0;
51 S32 increment = 0;
52
53 // Error calculation
54 speed_error = speed_cmd - mci_get_measured_speed();// value -255 <=> 255
55
56 // proportional term calculation
57 speed_proportional = Kp_speed*speed_error;
58
59 // integral term calculation
60 speed_integral = speed_integral + speed_error;
61
62 if(speed_integral > 255) speed_integral = 255;
63 if(speed_integral < -255) speed_integral = -255;
64
65 speed_integ = Ki_speed*speed_integral;
66
67 // derivative term calculation
68 /*speed_derivative = speed_error - last_speed_error;
69
70 if(speed_derivative > 255) speed_derivative = 255;
71 if(speed_derivative < -255) speed_derivative = -255;
72
73 speed_der = Kd_speed*speed_derivative;
74
75 last_speed_error = speed_error;*/
76
77 // Duty Cycle calculation
78 increment = speed_proportional + speed_integ;
79 //increment += speed_der;
80 increment = increment >> K_speed_scal;
81
82 // Variable saturation
83 if(increment >= (S16)(255)) Duty = 255;
84 else
85 {
86 if(increment <= (S16)(0)) Duty = 0;
87 else Duty = (U8)(increment);
88 }
89
90 // return Duty Cycle
91 return Duty;
92 }
93
94 /************************************************************************************************************/
95 /* Current Regulation */
96 /************************************************************************************************************/
103 U8 mc_control_current(U8 cur_cmd)
104 {
105 U8 Duty = 0;
106 S32 increment = 0;
107
108 // Error calculation
109 cur_error = cur_cmd - (mci_get_measured_current());// value -255 <=> 255
110
111 // proportional term calculation
112 cur_proportional = Kp_cur*cur_error;
113
114 // integral term calculation
115 cur_integral = cur_integral + cur_error;
116
117 if(cur_integral > 255) cur_integral = 255;
118 if(cur_integral < -255) cur_integral = -255;
119
120 cur_integ = Ki_cur*cur_integral;
121
122 // derivative term calculation
123 /*cur_derivative = cur_error - last_cur_error;
124
125 if(cur_derivative > 255) cur_derivative = 255;
126 if(cur_derivative < -255) cur_derivative = -255;
127
128 cur_der = Kd_cur*cur_derivative;
129
130 last_cur_error = cur_error;*/
131
132 // Duty Cycle calculation
133 increment = cur_proportional + cur_integ;
134 //increment += cur_der;
135 increment = increment >> K_cur_scal;
136
137 // Variable saturation
138 if(increment >= (S16)(255)) Duty = 255;
139 else
140 {
141 if(increment <= (S16)(0)) Duty = 0;
142 else Duty = (U8)(increment);
143 }
144
145 // return Duty Cycle
146 return Duty;
147 }
148
149
150
151 /************************************************************************************************************/
152 /* Selection of Regulation Loop */
153 /************************************************************************************************************/
159 void mc_regulation_loop()
160 {
161 switch(regulation_type)
162 {
163 case OPEN_LOOP : duty_cycle = mc_get_motor_speed();break;
164 case SPEED_LOOP : duty_cycle = mc_control_speed(mc_get_motor_speed());break;
165 case CURRENT_LOOP : duty_cycle = mc_control_current(mc_get_potentiometer_value());break;
166 default : break;
167 }
168 }
169
175 void mc_set_Open_Loop(){regulation_type = OPEN_LOOP;}
176
182 void mc_set_Speed_Loop(){regulation_type = SPEED_LOOP;}
183
189 void mc_set_Current_Loop(){regulation_type = CURRENT_LOOP;}
190
191
197 U8 mc_get_Duty_Cycle()
198 {
199 return duty_cycle;
200 }
201
mc_interface.h
speed_integ
S16 speed_integ
Definition: mc_control.c:24
Ki_speed
#define Ki_speed
Definition: config_motor.h:57
last_cur_error
S16 last_cur_error
Variable for the last error.
Definition: mc_control.c:31
cur_derivative
S16 cur_derivative
Definition: mc_control.c:35
mci_get_measured_current
U16 mci_get_measured_current(void)
Get the current measured in the motor.
Definition: mc_interface.c:179
speed_error
S16 speed_error
Error calculation.
Definition: mc_control.c:21
mc_set_Speed_Loop
void mc_set_Speed_Loop()
set type of regulation
Definition: mc_control.c:182
Kp_speed
#define Kp_speed
Definition: config_motor.h:56
config.h
,v
cur_integ
S16 cur_integ
Definition: mc_control.c:33
duty_cycle
U8 duty_cycle
Parameter to set PWM Duty Cycle after regulation calculation.
Definition: mc_control.c:17
last_speed_error
S16 last_speed_error
Variable for the last error.
Definition: mc_control.c:22
mc_control_current
U8 mc_control_current(U8 cur_cmd)
use to control current , current regulation loop need parameter : Kp_cur, Ki_cur ,Kd_cur and K_cur_scal in config_motor.h need to call in Te ms
Definition: mc_control.c:103
mc_drv.h
speed_der
S16 speed_der
Definition: mc_control.c:27
K_speed_scal
#define K_speed_scal
Definition: config_motor.h:74
SPEED_LOOP
#define SPEED_LOOP
Definition: config_motor.h:47
cur_der
S16 cur_der
Definition: mc_control.c:36
speed_integral
S16 speed_integral
Definition: mc_control.c:23
speed_derivative
S16 speed_derivative
Definition: mc_control.c:26
CURRENT_LOOP
#define CURRENT_LOOP
Definition: config_motor.h:48
regulation_type
U8 regulation_type
Define the type of regulation (OPEN_LOOP or CLOSE_LOOP)
Definition: mc_control.c:18
Ki_cur
#define Ki_cur
Definition: config_motor.h:62
mc_set_Open_Loop
void mc_set_Open_Loop()
set type of regulation
Definition: mc_control.c:175
mc_control_speed
U8 mc_control_speed(U8 speed_cmd)
use to control speed , speed regulation loop need parameter : Kp_speed, Ki_speed ,Kd_speed and K_speed_scal in config_motor.h need to call in Te ms
Definition: mc_control.c:48
mc_get_Duty_Cycle
U8 mc_get_Duty_Cycle()
set type of regulation
Definition: mc_control.c:197
cur_error
S16 cur_error
Error calculation.
Definition: mc_control.c:30
mc_regulation_loop
void mc_regulation_loop()
launch speed control or no regulation
Definition: mc_control.c:159
Kp_cur
#define Kp_cur
Definition: config_motor.h:61
mci_get_measured_speed
U8 mci_get_measured_speed(void)
Measured of speed.
Definition: mc_interface.c:169
OPEN_LOOP
#define OPEN_LOOP
Definition: config_motor.h:46
K_cur_scal
#define K_cur_scal
Definition: config_motor.h:75
speed_proportional
S16 speed_proportional
Definition: mc_control.c:25
mc_get_motor_speed
U8 mc_get_motor_speed(void)
Definition: mc_interface.c:118
mc_control.h
cur_proportional
S16 cur_proportional
Definition: mc_control.c:34
mc_set_Current_Loop
void mc_set_Current_Loop()
set type of regulation
Definition: mc_control.c:189
mc_get_potentiometer_value
U8 mc_get_potentiometer_value(void)
Get the potentiometer value.
Definition: mc_interface.c:199
cur_integral
S16 cur_integral
Definition: mc_control.c:32