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@9:49b59b308767, 2021-05-25 (annotated)

Committer:
stivending
Date:
Tue May 25 06:17:55 2021 +0000
Revision:
9:49b59b308767
Child:
10:fe56f6800a72
good version may 25

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