Robocon 2021 / Mbed 2 deprecated DC_Stepper_Controller_Lib

Library version for DC_Stepper_Controller_Lib with PWM speed control

Dependencies:   mbed QEI PID

Dependents:   DR-ArmServoTest Auto_DC_pick_class MBed_TR1 ros_button_2021

DC_Motor_Controller.cpp@15:87df75ee8731, 2021-05-31 (annotated)

Committer:
stivending
Date:
Mon May 31 09:44:13 2021 +0000
Revision:
15:87df75ee8731
Parent:
14:c9611cf036ad
Child:
16:3a1b95e2ecb8
button to init;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stivending 9:49b59b308767 1 #include "DC_Motor_Controller.h"
stivending 9:49b59b308767 2
stivending 9:49b59b308767 3 //************************************************//
stivending 9:49b59b308767 4 // COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE //
stivending 9:49b59b308767 5 // //
stivending 15:87df75ee8731 6 #define DEBUG_MODE //
stivending 9:49b59b308767 7 // //
stivending 9:49b59b308767 8 //************************************************//
stivending 9:49b59b308767 9
stivending 9:49b59b308767 10
stivending 9:49b59b308767 11
stivending 13:675456f1f401 12 void DC_Motor_On_Off::goto_pulse(int tar_pulse, float speed){ // Move motor to specific angle from home point
stivending 9:49b59b308767 13 int cur_pulse = dc_motor.getPulses();
stivending 9:49b59b308767 14 int direction = 0;
stivending 9:49b59b308767 15
stivending 9:49b59b308767 16 #ifdef DEBUG_MODE
stivending 10:fe56f6800a72 17 Serial device(USBTX, USBRX); // tx, rx
stivending 10:fe56f6800a72 18 device.baud(9600);
stivending 9:49b59b308767 19 device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
stivending 9:49b59b308767 20 #endif
stivending 9:49b59b308767 21
stivending 9:49b59b308767 22 while ( tar_pulse != cur_pulse ){
stivending 9:49b59b308767 23 direction = tar_pulse > cur_pulse; // cw 1 ; acw 0
stivending 9:49b59b308767 24
stivending 9:49b59b308767 25 // if margin of error is less than +-3 (+- 1 degree), give up
stivending 10:fe56f6800a72 26 if(abs(tar_pulse - cur_pulse) < 10)
stivending 9:49b59b308767 27 {
stivending 9:49b59b308767 28 #ifdef DEBUG_MODE
stivending 9:49b59b308767 29 device.printf("GIVEUP ");
stivending 9:49b59b308767 30 #endif
stivending 9:49b59b308767 31 break;
stivending 9:49b59b308767 32 }
stivending 9:49b59b308767 33
stivending 9:49b59b308767 34 // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
stivending 10:fe56f6800a72 35 else if (direction != (out1 > 0))
stivending 9:49b59b308767 36 {
stivending 9:49b59b308767 37 out1 = out2 = 0;
stivending 9:49b59b308767 38 #ifdef DEBUG_MODE
stivending 9:49b59b308767 39 device.printf("PAUSE ");
stivending 9:49b59b308767 40 #endif
stivending 9:49b59b308767 41 wait(0.3);
stivending 9:49b59b308767 42 }
stivending 9:49b59b308767 43
stivending 10:fe56f6800a72 44 out1 = direction * speed;
stivending 10:fe56f6800a72 45 out2 = !direction * speed;
stivending 9:49b59b308767 46
stivending 9:49b59b308767 47 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
stivending 9:49b59b308767 48
stivending 9:49b59b308767 49 #ifdef DEBUG_MODE
stivending 15:87df75ee8731 50 device.printf("Pulse:%d\n", cur_pulse);
stivending 9:49b59b308767 51 #endif
stivending 9:49b59b308767 52
stivending 9:49b59b308767 53 }
stivending 9:49b59b308767 54 #ifdef DEBUG_MODE
stivending 9:49b59b308767 55 device.printf("\n----- done -----\n");
stivending 9:49b59b308767 56 #endif
stivending 9:49b59b308767 57 out1 = 0; // Stop motor
stivending 9:49b59b308767 58 out2 = 0;
stivending 9:49b59b308767 59 };
stivending 10:fe56f6800a72 60
stivending 15:87df75ee8731 61 void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd, double min = 0.05)
stivending 13:675456f1f401 62 {
stivending 13:675456f1f401 63 #ifdef DEBUG_MODE
stivending 13:675456f1f401 64 Serial device(USBTX, USBRX); // tx, rx
stivending 13:675456f1f401 65 device.baud(9600);
stivending 13:675456f1f401 66 device.printf("Kp: %.3f, Ki: %.3f, Kd: %.3f", Kp, Ki, Kd);
stivending 13:675456f1f401 67 #endif
stivending 15:87df75ee8731 68 this->pid=new PID(0.01,1,min,Kp,Kd,Ki);
stivending 13:675456f1f401 69 }
stivending 13:675456f1f401 70
stivending 13:675456f1f401 71
stivending 13:675456f1f401 72 void DC_Motor_PID::goto_pulse(int tar_pulse, float placeholder){ // Move motor to specific angle from home point
stivending 13:675456f1f401 73 int cur_pulse = dc_motor.getPulses();
stivending 13:675456f1f401 74
stivending 13:675456f1f401 75 #ifdef DEBUG_MODE
stivending 13:675456f1f401 76 Serial device(USBTX, USBRX); // tx, rx
stivending 13:675456f1f401 77 device.baud(9600);
stivending 13:675456f1f401 78 device.printf("\n------ PID current %d -> target %d -----\n", cur_pulse, tar_pulse);
stivending 13:675456f1f401 79 #endif
stivending 13:675456f1f401 80
stivending 13:675456f1f401 81 while ( tar_pulse != cur_pulse ){
stivending 15:87df75ee8731 82
stivending 15:87df75ee8731 83 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
stivending 13:675456f1f401 84 double speed = this->pid->calculate(tar_pulse,cur_pulse);
stivending 13:675456f1f401 85
stivending 13:675456f1f401 86 #ifdef DEBUG_MODE
stivending 13:675456f1f401 87 if(!this->pid)
stivending 13:675456f1f401 88 device.printf("PID NOT SET BUT CALLED! NULL POINTER ");
stivending 13:675456f1f401 89 #endif
stivending 13:675456f1f401 90 // if margin of error is less than +-3 (+- 1 degree), give up
stivending 15:87df75ee8731 91 if(abs(tar_pulse - cur_pulse) < 2)
stivending 13:675456f1f401 92 {
stivending 13:675456f1f401 93 #ifdef DEBUG_MODE
stivending 13:675456f1f401 94 device.printf("GIVEUP ");
stivending 13:675456f1f401 95 #endif
stivending 13:675456f1f401 96 break;
stivending 13:675456f1f401 97 }
stivending 13:675456f1f401 98
stivending 13:675456f1f401 99 out1 = speed > 0 ? speed : 0;
stivending 13:675456f1f401 100 out2 = speed > 0 ? 0 : -speed;
stivending 13:675456f1f401 101
stivending 15:87df75ee8731 102
stivending 13:675456f1f401 103 #ifdef DEBUG_MODE
stivending 15:87df75ee8731 104 device.printf("Speed:%.0f ", speed*1000);
stivending 15:87df75ee8731 105 device.printf("Pulse:%d \n", cur_pulse);
stivending 13:675456f1f401 106 #endif
stivending 13:675456f1f401 107
stivending 13:675456f1f401 108 }
stivending 13:675456f1f401 109 #ifdef DEBUG_MODE
stivending 13:675456f1f401 110 device.printf("\n----- PID done -----\n");
stivending 13:675456f1f401 111 #endif
stivending 13:675456f1f401 112 out1 = 0; // Stop motor
stivending 13:675456f1f401 113 out2 = 0;
stivending 13:675456f1f401 114 };
stivending 13:675456f1f401 115
stivending 13:675456f1f401 116
stivending 13:675456f1f401 117
stivending 13:675456f1f401 118 DC_Motor_Controller::DC_Motor_Controller(PinName out_M1, PinName out_M2, PinName in_A, PinName in_B, int PPR) :
stivending 13:675456f1f401 119 out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR)
stivending 13:675456f1f401 120 {
stivending 14:c9611cf036ad 121 _ppr = PPR;
stivending 15:87df75ee8731 122 out1.period(0.005);
stivending 15:87df75ee8731 123 out2.period(0.005);
stivending 13:675456f1f401 124 }
stivending 10:fe56f6800a72 125
stivending 10:fe56f6800a72 126 void DC_Motor_Controller::goto_angle(int angle, float speed){ // Move motor to specific angle from home point
stivending 10:fe56f6800a72 127
stivending 14:c9611cf036ad 128 goto_pulse(((double) angle / 360.0)* (2.0 * _ppr), speed); // Calculating target pulse number
stivending 10:fe56f6800a72 129 };
stivending 9:49b59b308767 130
stivending 9:49b59b308767 131 void DC_Motor_Controller::reset(){ // Setting home point for increment encoder
stivending 9:49b59b308767 132 out1 = 0;
stivending 9:49b59b308767 133 out2 = 0;
stivending 9:49b59b308767 134 dc_motor.reset(); // Reset pulse_
stivending 9:49b59b308767 135 wait(0.3);
stivending 9:49b59b308767 136 };
stivending 9:49b59b308767 137
stivending 10:fe56f6800a72 138 void DC_Motor_Controller::move_angle(int angle, float speed){ // move for relative distance
stivending 10:fe56f6800a72 139
stivending 14:c9611cf036ad 140 goto_pulse(get_pulse() + ((double) angle / 360.0)* (2.0 * _ppr), speed);
stivending 9:49b59b308767 141 };
stivending 9:49b59b308767 142
stivending 10:fe56f6800a72 143 void DC_Motor_Controller::set_out(float a, float b)
stivending 9:49b59b308767 144 {
stivending 9:49b59b308767 145 out1 = a;
stivending 9:49b59b308767 146 out2 = b;
stivending 9:49b59b308767 147 }
stivending 9:49b59b308767 148
stivending 9:49b59b308767 149 int DC_Motor_Controller::get_pulse()
stivending 9:49b59b308767 150 {
stivending 9:49b59b308767 151 return dc_motor.getPulses();
stivending 9:49b59b308767 152 }