BeRTOS
|
00001 00062 #include <cfg/cfg_dc_motor.h> 00063 00064 #include <cfg/debug.h> 00065 // Define logging setting (for cfg/log.h module). 00066 #define LOG_LEVEL DC_MOTOR_LOG_LEVEL 00067 #define LOG_VERBOSITY DC_MOTOR_LOG_FORMAT 00068 #include <cfg/log.h> 00069 00070 00071 #include <algo/pid_control.h> 00072 00073 #include <drv/timer.h> 00074 #include <drv/dc_motor.h> 00075 #include <drv/adc.h> 00076 #include <drv/pwm.h> 00077 #include <drv/dc_motor.h> 00078 00079 #include <kern/proc.h> 00080 00081 #include <cpu/irq.h> 00082 00083 #include <verstag.h> 00084 #include <buildrev.h> 00085 00086 static DCMotorConfig motor = 00087 { 00088 /* PID */ 00089 { 00090 .kp = 1, /* Proportional coefficient */ 00091 .ki = 4, /* Integral coefficient */ 00092 .kd = 0.008, /* Derivate coefficient */ 00093 .i_max = 2E33, /* Integrale max error value */ 00094 .i_min = -2E33, /* Integrale min error value */ 00095 .out_max = 65535, /* Max output value */ 00096 .out_min = 0, /* Min output value */ 00097 .sample_period = 0 /* Millisecod between 2 output singal sampling */ 00098 }, 00099 .pid_enable = true, /* Enable or disable pid control */ 00100 00101 /* PWM */ 00102 .pwm_dev = 2, /* PWM channel */ 00103 .freq = 3000, /* Frquency of PWM output waveform */ 00104 00105 /* ADC */ 00106 .adc_ch = 2, /* ADC channel */ 00107 .adc_max = 65535, /* Max range value for ADC */ 00108 .adc_min = 0, /* Min range value for ADC */ 00109 00110 /* DC Motor */ 00111 .dir = 1, /* Default spin direction of DC motor */ 00112 .braked = true, 00113 00114 .speed = 10000, /* Fixed speed value for seldc_motor_enableect DC motor, if enable_dev_speed flag is false */ 00115 .speed_dev_id = 7, /* Index of the device where read speed */ 00116 }; 00117 00118 int dc_motor_testSetUp(void) 00119 { 00120 IRQ_ENABLE; 00121 kdbg_init(); 00122 timer_init(); 00123 proc_init(); 00124 #if !CFG_PWM_ENABLE_OLD_API 00125 pwm_init(); 00126 #endif 00127 adc_init(); 00128 00129 return 0; 00130 } 00131 00132 #define MOTOR 2 00133 00134 void NORETURN dc_motor_testRun(void) 00135 { 00136 dc_motor_init(); 00137 00138 /* 00139 * Assign the configuration to motor. 00140 */ 00141 dc_motor_setup(MOTOR, &motor); 00142 00143 while (1) 00144 { 00145 /* 00146 * Using enable and disable 00147 */ 00148 dc_motor_setDir(MOTOR, 1); 00149 dc_motor_setSpeed(MOTOR, 10000); 00150 dc_motor_enable(MOTOR, true); 00151 timer_delay(500); 00152 dc_motor_enable(MOTOR, false); 00153 00154 00155 dc_motor_setDir(MOTOR, 0); 00156 dc_motor_setSpeed(MOTOR, 60000); 00157 dc_motor_enable(MOTOR, true); 00158 timer_delay(150); 00159 dc_motor_enable(MOTOR, false); 00160 00161 /* 00162 * Using timer 00163 */ 00164 dc_motor_setDir(MOTOR, 1); 00165 dc_motor_setSpeed(MOTOR, 60000); 00166 dc_motor_startTimer(MOTOR, 150); 00167 dc_motor_waitStop(MOTOR); 00168 00169 dc_motor_setDir(MOTOR, 0); 00170 dc_motor_setSpeed(MOTOR, 10000); 00171 dc_motor_startTimer(MOTOR, 500); 00172 dc_motor_waitStop(MOTOR); 00173 } 00174 00175 } 00176 00177 int dc_motor_testTearDown(void) 00178 { 00179 return 0; 00180 }