00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 //_____ I N C L U D E S ___________________________________________________ 00012 #include "config.h" 00013 #include "mc_control.h" 00014 #include "mc_drv.h" 00015 #include "mc_interface.h" 00016 00017 00018 U8 duty_cycle = 0; 00019 U8 regulation_type = OPEN_LOOP; 00020 00021 /* Speed regulation variables */ 00022 S16 speed_error = 0; 00023 S16 last_speed_error = 0; 00024 S16 speed_integral = 0; 00025 S16 speed_integ = 0; 00026 S16 speed_proportional = 0; 00027 S16 speed_derivative = 0; 00028 S16 speed_der = 0; 00029 00030 /* Current regulation variables */ 00031 S16 cur_error = 0; 00032 S16 last_cur_error = 0; 00033 S16 cur_integral = 0; 00034 S16 cur_integ = 0; 00035 S16 cur_proportional = 0; 00036 S16 cur_derivative = 0; 00037 S16 cur_der = 0; 00038 00039 00040 /************************************************************************************************************/ 00041 /* Speed Regulation */ 00042 /************************************************************************************************************/ 00049 U8 mc_control_speed(U8 speed_cmd) 00050 { 00051 U8 Duty = 0; 00052 S32 increment = 0; 00053 00054 // Error calculation 00055 speed_error = speed_cmd - mci_get_measured_speed();// value -255 <=> 255 00056 00057 // proportional term calculation 00058 speed_proportional = Kp_speed*speed_error; 00059 00060 // integral term calculation 00061 speed_integral = speed_integral + speed_error; 00062 00063 if(speed_integral > 255) speed_integral = 255; 00064 if(speed_integral < -255) speed_integral = -255; 00065 00066 speed_integ = Ki_speed*speed_integral; 00067 00068 // derivative term calculation 00069 /*speed_derivative = speed_error - last_speed_error; 00070 00071 if(speed_derivative > 255) speed_derivative = 255; 00072 if(speed_derivative < -255) speed_derivative = -255; 00073 00074 speed_der = Kd_speed*speed_derivative; 00075 00076 last_speed_error = speed_error;*/ 00077 00078 // Duty Cycle calculation 00079 increment = speed_proportional + speed_integ; 00080 //increment += speed_der; 00081 increment = increment >> K_speed_scal; 00082 00083 // Variable saturation 00084 if(increment >= (S16)(255)) Duty = 255; 00085 else 00086 { 00087 if(increment <= (S16)(0)) Duty = 0; 00088 else Duty = (U8)(increment); 00089 } 00090 00091 // return Duty Cycle 00092 return Duty; 00093 } 00094 00095 /************************************************************************************************************/ 00096 /* Current Regulation */ 00097 /************************************************************************************************************/ 00104 U8 mc_control_current(U8 cur_cmd) 00105 { 00106 U8 Duty = 0; 00107 S32 increment = 0; 00108 00109 // Error calculation 00110 cur_error = cur_cmd - (mci_get_measured_current());// value -255 <=> 255 00111 00112 // proportional term calculation 00113 cur_proportional = Kp_cur*cur_error; 00114 00115 // integral term calculation 00116 cur_integral = cur_integral + cur_error; 00117 00118 if(cur_integral > 255) cur_integral = 255; 00119 if(cur_integral < -255) cur_integral = -255; 00120 00121 cur_integ = Ki_cur*cur_integral; 00122 00123 // derivative term calculation 00124 /*cur_derivative = cur_error - last_cur_error; 00125 00126 if(cur_derivative > 255) cur_derivative = 255; 00127 if(cur_derivative < -255) cur_derivative = -255; 00128 00129 cur_der = Kd_cur*cur_derivative; 00130 00131 last_cur_error = cur_error;*/ 00132 00133 // Duty Cycle calculation 00134 increment = cur_proportional + cur_integ; 00135 //increment += cur_der; 00136 increment = increment >> K_cur_scal; 00137 00138 // Variable saturation 00139 if(increment >= (S16)(255)) Duty = 255; 00140 else 00141 { 00142 if(increment <= (S16)(0)) Duty = 0; 00143 else Duty = (U8)(increment); 00144 } 00145 00146 // return Duty Cycle 00147 return Duty; 00148 } 00149 00150 00151 00152 /************************************************************************************************************/ 00153 /* Selection of Regulation Loop */ 00154 /************************************************************************************************************/ 00160 void mc_regulation_loop() 00161 { 00162 // measure 00163 Set_EXT3(); 00164 00165 switch(regulation_type) 00166 { 00167 case OPEN_LOOP : duty_cycle = mc_get_motor_speed();break; 00168 case SPEED_LOOP : duty_cycle = mc_control_speed(mc_get_motor_speed());break; 00169 case CURRENT_LOOP : duty_cycle = mc_control_current(mc_get_potentiometer_value());break; 00170 default : break; 00171 } 00172 00173 // measure 00174 Clear_EXT3(); 00175 } 00176 00182 void mc_set_Open_Loop(){regulation_type = OPEN_LOOP;} 00183 00189 void mc_set_Speed_Loop(){regulation_type = SPEED_LOOP;} 00190 00196 void mc_set_Current_Loop(){regulation_type = CURRENT_LOOP;} 00197 00198 00204 U8 mc_get_Duty_Cycle() 00205 { 00206 return duty_cycle; 00207 } 00208
1.5.7.1