![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Library version for DC_Stepper_Controller_Lib with PWM speed control
Dependents: DR-ArmServoTest Auto_DC_pick_class MBed_TR1 ros_button_2021
DC_Motor_Controller.cpp
- Committer:
- stivending
- Date:
- 2021-05-27
- Revision:
- 13:675456f1f401
- Parent:
- 11:e880912260b5
- Child:
- 14:c9611cf036ad
File content as of revision 13:675456f1f401:
#include "DC_Motor_Controller.h" //************************************************// // COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE // // // #define DEBUG_MODE // // // //************************************************// void DC_Motor_On_Off::goto_pulse(int tar_pulse, float speed){ // Move motor to specific angle from home point int cur_pulse = dc_motor.getPulses(); int direction = 0; #ifdef DEBUG_MODE Serial device(USBTX, USBRX); // tx, rx device.baud(9600); device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse); #endif while ( tar_pulse != cur_pulse ){ direction = tar_pulse > cur_pulse; // cw 1 ; acw 0 // if margin of error is less than +-3 (+- 1 degree), give up if(abs(tar_pulse - cur_pulse) < 10) { #ifdef DEBUG_MODE device.printf("GIVEUP "); #endif break; } // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs else if (direction != (out1 > 0)) { out1 = out2 = 0; #ifdef DEBUG_MODE device.printf("PAUSE "); #endif wait(0.3); } out1 = direction * speed; out2 = !direction * speed; 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----- done -----\n"); #endif out1 = 0; // Stop motor out2 = 0; }; void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd) { #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); } void DC_Motor_PID::goto_pulse(int tar_pulse, float placeholder){ // Move motor to specific angle from home point int cur_pulse = dc_motor.getPulses(); #ifdef DEBUG_MODE Serial device(USBTX, USBRX); // tx, rx device.baud(9600); device.printf("\n------ PID current %d -> target %d -----\n", cur_pulse, tar_pulse); #endif while ( tar_pulse != cur_pulse ){ double speed = this->pid->calculate(tar_pulse,cur_pulse); #ifdef DEBUG_MODE if(!this->pid) 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) { #ifdef DEBUG_MODE device.printf("GIVEUP "); #endif break; } out1 = speed > 0 ? speed : 0; out2 = speed > 0 ? 0 : -speed; #ifdef DEBUG_MODE device.printf("%.3f ", speed); #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"); #endif out1 = 0; // Stop motor out2 = 0; }; DC_Motor_Controller::DC_Motor_Controller(PinName out_M1, PinName out_M2, PinName in_A, PinName in_B, int PPR) : out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR) { out1.period(0.002); out2.period(0.002); } void DC_Motor_Controller::goto_angle(int angle, float speed){ // Move motor to specific angle from home point goto_pulse(((double) angle / 360.0)* (2.0 * 792.0), speed); // Calculating target pulse number }; void DC_Motor_Controller::reset(){ // Setting home point for increment encoder out1 = 0; out2 = 0; dc_motor.reset(); // Reset pulse_ wait(0.3); }; void DC_Motor_Controller::move_angle(int angle, float speed){ // move for relative distance goto_pulse(get_pulse() + ((double) angle / 360.0)* (2.0 * 792.0), speed); }; void DC_Motor_Controller::set_out(float a, float b) { out1 = a; out2 = b; } int DC_Motor_Controller::get_pulse() { return dc_motor.getPulses(); }