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@13:675456f1f401, 2021-05-27 (annotated)

Committer:
stivending
Date:
Thu May 27 05:43:48 2021 +0000
Revision:
13:675456f1f401
Parent:
11:e880912260b5
Child:
14:c9611cf036ad
PID supported, read manual first

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 13:675456f1f401 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 13:675456f1f401 50 device.printf("[%d] ", 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 13:675456f1f401 61 void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd)
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 13:675456f1f401 68 this->pid=new PID(0.01,1,0.05,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 13:675456f1f401 82 double speed = this->pid->calculate(tar_pulse,cur_pulse);
stivending 13:675456f1f401 83
stivending 13:675456f1f401 84 #ifdef DEBUG_MODE
stivending 13:675456f1f401 85 if(!this->pid)
stivending 13:675456f1f401 86 device.printf("PID NOT SET BUT CALLED! NULL POINTER ");
stivending 13:675456f1f401 87 #endif
stivending 13:675456f1f401 88 // if margin of error is less than +-3 (+- 1 degree), give up
stivending 13:675456f1f401 89 if(abs(tar_pulse - cur_pulse) < 0)
stivending 13:675456f1f401 90 {
stivending 13:675456f1f401 91 #ifdef DEBUG_MODE
stivending 13:675456f1f401 92 device.printf("GIVEUP ");
stivending 13:675456f1f401 93 #endif
stivending 13:675456f1f401 94 break;
stivending 13:675456f1f401 95 }
stivending 13:675456f1f401 96
stivending 13:675456f1f401 97 out1 = speed > 0 ? speed : 0;
stivending 13:675456f1f401 98 out2 = speed > 0 ? 0 : -speed;
stivending 13:675456f1f401 99
stivending 13:675456f1f401 100 #ifdef DEBUG_MODE
stivending 13:675456f1f401 101 device.printf("%.3f ", speed);
stivending 13:675456f1f401 102 #endif
stivending 13:675456f1f401 103
stivending 13:675456f1f401 104 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
stivending 13:675456f1f401 105
stivending 13:675456f1f401 106 #ifdef DEBUG_MODE
stivending 13:675456f1f401 107 device.printf("[%d] ", cur_pulse);
stivending 13:675456f1f401 108 #endif
stivending 13:675456f1f401 109
stivending 13:675456f1f401 110 }
stivending 13:675456f1f401 111 #ifdef DEBUG_MODE
stivending 13:675456f1f401 112 device.printf("\n----- PID done -----\n");
stivending 13:675456f1f401 113 #endif
stivending 13:675456f1f401 114 out1 = 0; // Stop motor
stivending 13:675456f1f401 115 out2 = 0;
stivending 13:675456f1f401 116 };
stivending 13:675456f1f401 117
stivending 13:675456f1f401 118
stivending 13:675456f1f401 119
stivending 13:675456f1f401 120 DC_Motor_Controller::DC_Motor_Controller(PinName out_M1, PinName out_M2, PinName in_A, PinName in_B, int PPR) :
stivending 13:675456f1f401 121 out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR)
stivending 13:675456f1f401 122 {
stivending 13:675456f1f401 123 out1.period(0.002);
stivending 13:675456f1f401 124 out2.period(0.002);
stivending 13:675456f1f401 125 }
stivending 10:fe56f6800a72 126
stivending 10:fe56f6800a72 127 void DC_Motor_Controller::goto_angle(int angle, float speed){ // Move motor to specific angle from home point
stivending 10:fe56f6800a72 128
stivending 10:fe56f6800a72 129 goto_pulse(((double) angle / 360.0)* (2.0 * 792.0), speed); // Calculating target pulse number
stivending 10:fe56f6800a72 130 };
stivending 9:49b59b308767 131
stivending 9:49b59b308767 132 void DC_Motor_Controller::reset(){ // Setting home point for increment encoder
stivending 9:49b59b308767 133 out1 = 0;
stivending 9:49b59b308767 134 out2 = 0;
stivending 9:49b59b308767 135 dc_motor.reset(); // Reset pulse_
stivending 9:49b59b308767 136 wait(0.3);
stivending 9:49b59b308767 137 };
stivending 9:49b59b308767 138
stivending 10:fe56f6800a72 139 void DC_Motor_Controller::move_angle(int angle, float speed){ // move for relative distance
stivending 10:fe56f6800a72 140
stivending 10:fe56f6800a72 141 goto_pulse(get_pulse() + ((double) angle / 360.0)* (2.0 * 792.0), speed);
stivending 9:49b59b308767 142 };
stivending 9:49b59b308767 143
stivending 10:fe56f6800a72 144 void DC_Motor_Controller::set_out(float a, float b)
stivending 9:49b59b308767 145 {
stivending 9:49b59b308767 146 out1 = a;
stivending 9:49b59b308767 147 out2 = b;
stivending 9:49b59b308767 148 }
stivending 9:49b59b308767 149
stivending 9:49b59b308767 150 int DC_Motor_Controller::get_pulse()
stivending 9:49b59b308767 151 {
stivending 9:49b59b308767 152 return dc_motor.getPulses();
stivending 9:49b59b308767 153 }