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@11:e880912260b5, 2021-05-25 (annotated)

Committer:
stivending
Date:
Tue May 25 08:59:55 2021 +0000
Revision:
11:e880912260b5
Parent:
10:fe56f6800a72
Child:
13:675456f1f401
library

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 11:e880912260b5 6 // #define DEBUG_MODE //
stivending 9:49b59b308767 7 // //
stivending 9:49b59b308767 8 //************************************************//
stivending 9:49b59b308767 9
stivending 10:fe56f6800a72 10 DC_Motor_Controller::DC_Motor_Controller(PinName out_M1, PinName out_M2, PinName in_A, PinName in_B, int PPR) :
stivending 10:fe56f6800a72 11 out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR)
stivending 10:fe56f6800a72 12 {
stivending 10:fe56f6800a72 13 out1.period(0.002);
stivending 10:fe56f6800a72 14 out2.period(0.002);
stivending 10:fe56f6800a72 15 }
stivending 9:49b59b308767 16
stivending 9:49b59b308767 17
stivending 10:fe56f6800a72 18 void DC_Motor_Controller::goto_pulse(int tar_pulse, float speed){ // Move motor to specific angle from home point
stivending 9:49b59b308767 19 int cur_pulse = dc_motor.getPulses();
stivending 9:49b59b308767 20 int direction = 0;
stivending 9:49b59b308767 21
stivending 9:49b59b308767 22 #ifdef DEBUG_MODE
stivending 10:fe56f6800a72 23 Serial device(USBTX, USBRX); // tx, rx
stivending 10:fe56f6800a72 24 device.baud(9600);
stivending 9:49b59b308767 25 device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
stivending 9:49b59b308767 26 #endif
stivending 9:49b59b308767 27
stivending 9:49b59b308767 28 while ( tar_pulse != cur_pulse ){
stivending 9:49b59b308767 29 direction = tar_pulse > cur_pulse; // cw 1 ; acw 0
stivending 9:49b59b308767 30
stivending 9:49b59b308767 31 // if margin of error is less than +-3 (+- 1 degree), give up
stivending 10:fe56f6800a72 32 if(abs(tar_pulse - cur_pulse) < 10)
stivending 9:49b59b308767 33 {
stivending 9:49b59b308767 34 #ifdef DEBUG_MODE
stivending 9:49b59b308767 35 device.printf("GIVEUP ");
stivending 9:49b59b308767 36 #endif
stivending 9:49b59b308767 37 break;
stivending 9:49b59b308767 38 }
stivending 9:49b59b308767 39
stivending 9:49b59b308767 40 // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
stivending 10:fe56f6800a72 41 else if (direction != (out1 > 0))
stivending 9:49b59b308767 42 {
stivending 9:49b59b308767 43 out1 = out2 = 0;
stivending 9:49b59b308767 44 #ifdef DEBUG_MODE
stivending 9:49b59b308767 45 device.printf("PAUSE ");
stivending 9:49b59b308767 46 #endif
stivending 9:49b59b308767 47 wait(0.3);
stivending 9:49b59b308767 48 }
stivending 9:49b59b308767 49
stivending 10:fe56f6800a72 50 out1 = direction * speed;
stivending 10:fe56f6800a72 51 out2 = !direction * speed;
stivending 9:49b59b308767 52
stivending 9:49b59b308767 53 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
stivending 9:49b59b308767 54
stivending 9:49b59b308767 55 #ifdef DEBUG_MODE
stivending 9:49b59b308767 56 device.printf("%d ", cur_pulse);
stivending 9:49b59b308767 57 #endif
stivending 9:49b59b308767 58
stivending 9:49b59b308767 59 }
stivending 9:49b59b308767 60 #ifdef DEBUG_MODE
stivending 9:49b59b308767 61 device.printf("\n----- done -----\n");
stivending 9:49b59b308767 62 #endif
stivending 9:49b59b308767 63 out1 = 0; // Stop motor
stivending 9:49b59b308767 64 out2 = 0;
stivending 9:49b59b308767 65 };
stivending 10:fe56f6800a72 66
stivending 10:fe56f6800a72 67
stivending 10:fe56f6800a72 68 void DC_Motor_Controller::goto_angle(int angle, float speed){ // Move motor to specific angle from home point
stivending 10:fe56f6800a72 69
stivending 10:fe56f6800a72 70 goto_pulse(((double) angle / 360.0)* (2.0 * 792.0), speed); // Calculating target pulse number
stivending 10:fe56f6800a72 71 };
stivending 9:49b59b308767 72
stivending 9:49b59b308767 73 void DC_Motor_Controller::reset(){ // Setting home point for increment encoder
stivending 9:49b59b308767 74 out1 = 0;
stivending 9:49b59b308767 75 out2 = 0;
stivending 9:49b59b308767 76 dc_motor.reset(); // Reset pulse_
stivending 9:49b59b308767 77 wait(0.3);
stivending 9:49b59b308767 78 };
stivending 9:49b59b308767 79
stivending 10:fe56f6800a72 80 void DC_Motor_Controller::move_angle(int angle, float speed){ // move for relative distance
stivending 10:fe56f6800a72 81
stivending 10:fe56f6800a72 82 goto_pulse(get_pulse() + ((double) angle / 360.0)* (2.0 * 792.0), speed);
stivending 9:49b59b308767 83 };
stivending 9:49b59b308767 84
stivending 10:fe56f6800a72 85 void DC_Motor_Controller::set_out(float a, float b)
stivending 9:49b59b308767 86 {
stivending 9:49b59b308767 87 out1 = a;
stivending 9:49b59b308767 88 out2 = b;
stivending 9:49b59b308767 89 }
stivending 9:49b59b308767 90
stivending 9:49b59b308767 91 int DC_Motor_Controller::get_pulse()
stivending 9:49b59b308767 92 {
stivending 9:49b59b308767 93 return dc_motor.getPulses();
stivending 9:49b59b308767 94 }