Library version for DC_Stepper_Controller_Lib with PWM speed control
Dependents: DR-ArmServoTest Auto_DC_pick_class MBed_TR1 ros_button_2021
Diff: DC_Motor_Controller.cpp
- Revision:
- 15:87df75ee8731
- Parent:
- 14:c9611cf036ad
- Child:
- 16:3a1b95e2ecb8
diff -r c9611cf036ad -r 87df75ee8731 DC_Motor_Controller.cpp --- a/DC_Motor_Controller.cpp Thu May 27 07:32:49 2021 +0000 +++ b/DC_Motor_Controller.cpp Mon May 31 09:44:13 2021 +0000 @@ -3,7 +3,7 @@ //************************************************// // COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE // // // -// #define DEBUG_MODE // + #define DEBUG_MODE // // // //************************************************// @@ -47,7 +47,7 @@ cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number #ifdef DEBUG_MODE - device.printf("[%d] ", cur_pulse); + device.printf("Pulse:%d\n", cur_pulse); #endif } @@ -58,14 +58,14 @@ out2 = 0; }; -void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd) +void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd, double min = 0.05) { #ifdef DEBUG_MODE Serial device(USBTX, USBRX); // tx, rx device.baud(9600); device.printf("Kp: %.3f, Ki: %.3f, Kd: %.3f", Kp, Ki, Kd); #endif - this->pid=new PID(0.01,1,0.05,Kp,Kd,Ki); + this->pid=new PID(0.01,1,min,Kp,Kd,Ki); } @@ -79,6 +79,8 @@ #endif while ( tar_pulse != cur_pulse ){ + + cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number double speed = this->pid->calculate(tar_pulse,cur_pulse); #ifdef DEBUG_MODE @@ -86,7 +88,7 @@ device.printf("PID NOT SET BUT CALLED! NULL POINTER "); #endif // if margin of error is less than +-3 (+- 1 degree), give up - if(abs(tar_pulse - cur_pulse) < 0) + if(abs(tar_pulse - cur_pulse) < 2) { #ifdef DEBUG_MODE device.printf("GIVEUP "); @@ -97,16 +99,12 @@ out1 = speed > 0 ? speed : 0; out2 = speed > 0 ? 0 : -speed; + #ifdef DEBUG_MODE - device.printf("%.3f ", speed); + device.printf("Speed:%.0f ", speed*1000); + device.printf("Pulse:%d \n", cur_pulse); #endif - cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number - - #ifdef DEBUG_MODE - device.printf("[%d] ", cur_pulse); - #endif - } #ifdef DEBUG_MODE device.printf("\n----- PID done -----\n"); @@ -121,8 +119,8 @@ out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR) { _ppr = PPR; - out1.period(0.002); - out2.period(0.002); + out1.period(0.005); + out2.period(0.005); } void DC_Motor_Controller::goto_angle(int angle, float speed){ // Move motor to specific angle from home point