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.h@7:6e59ed00a6a9, 2021-05-24 (annotated)

Committer:
stivending
Date:
Mon May 24 08:17:09 2021 +0000
Revision:
7:6e59ed00a6a9
Parent:
5:c040faf21e07
Child:
8:703502486434
f margin of error is less than +-3 pulses (+-0.5 degree), give up; future direction diff from current motion, pause for unexpected outputs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stivending 5:c040faf21e07 1 #include "mbed.h"
stivending 5:c040faf21e07 2 #include "QEI.h"
stivending 7:6e59ed00a6a9 3 #include <cmath>
stivending 5:c040faf21e07 4
stivending 5:c040faf21e07 5 #ifndef DC_MOTOR_CONTROLLER_H
stivending 5:c040faf21e07 6 #define DC_MOTOR_CONTROLLER_H
stivending 5:c040faf21e07 7
stivending 5:c040faf21e07 8 class DC_Motor_Controller {
stivending 5:c040faf21e07 9
stivending 5:c040faf21e07 10 private:
stivending 5:c040faf21e07 11
stivending 5:c040faf21e07 12 DigitalOut out1, out2; // Motor direction control pin 2
stivending 5:c040faf21e07 13 QEI dc_motor; //(D8,D7,NC,792); // Create QEI object for increment encoder
stivending 5:c040faf21e07 14 void goto_angle(int angle){ // Move motor to specific angle from home point
stivending 7:6e59ed00a6a9 15
stivending 7:6e59ed00a6a9 16 //Serial device(USBTX, USBRX); // tx, rx
stivending 7:6e59ed00a6a9 17 //device.baud(19200);
stivending 7:6e59ed00a6a9 18
stivending 5:c040faf21e07 19 int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0); // Calculating target pulse number
stivending 5:c040faf21e07 20 int cur_pulse = dc_motor.getPulses();
stivending 7:6e59ed00a6a9 21 int direction = 0;
stivending 5:c040faf21e07 22
stivending 7:6e59ed00a6a9 23 //device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
stivending 5:c040faf21e07 24 while ( tar_pulse != cur_pulse ){
stivending 7:6e59ed00a6a9 25 direction = tar_pulse > cur_pulse; // cw 1 ; acw 0
stivending 7:6e59ed00a6a9 26
stivending 7:6e59ed00a6a9 27 // if margin of error is less than +-3 (+- 1 degree), give up
stivending 7:6e59ed00a6a9 28 if(abs(tar_pulse - cur_pulse) < 3)
stivending 7:6e59ed00a6a9 29 {
stivending 7:6e59ed00a6a9 30 //device.printf("GIVEUP ");
stivending 7:6e59ed00a6a9 31 break;
stivending 5:c040faf21e07 32 }
stivending 7:6e59ed00a6a9 33
stivending 7:6e59ed00a6a9 34 // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
stivending 7:6e59ed00a6a9 35 else if (direction != out1)
stivending 7:6e59ed00a6a9 36 {
stivending 7:6e59ed00a6a9 37 out1 = out2 = 0;
stivending 7:6e59ed00a6a9 38 //device.printf("PAUSE ");
stivending 7:6e59ed00a6a9 39 wait(0.3);
stivending 5:c040faf21e07 40 }
stivending 7:6e59ed00a6a9 41
stivending 7:6e59ed00a6a9 42 out1 = direction;
stivending 7:6e59ed00a6a9 43 out2 = !direction;
stivending 7:6e59ed00a6a9 44 //device.printf(direction ? "> " : "< ");
stivending 7:6e59ed00a6a9 45
stivending 5:c040faf21e07 46 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
stivending 7:6e59ed00a6a9 47
stivending 7:6e59ed00a6a9 48 //device.printf("%d ", cur_pulse);
stivending 7:6e59ed00a6a9 49
stivending 5:c040faf21e07 50 }
stivending 7:6e59ed00a6a9 51 //device.printf("\n----- done -----\n");
stivending 5:c040faf21e07 52 out1 = 0; // Stop motor
stivending 5:c040faf21e07 53 out2 = 0;
stivending 5:c040faf21e07 54 };
stivending 5:c040faf21e07 55
stivending 5:c040faf21e07 56 public:
stivending 7:6e59ed00a6a9 57 DC_Motor_Controller(PinName out1_p, PinName out2_p, PinName in1_p, PinName in2_p, int PPR) :
stivending 7:6e59ed00a6a9 58 out1(out1_p), out2(out2_p), dc_motor(in1_p, in2_p, NC, PPR){}
stivending 5:c040faf21e07 59
stivending 5:c040faf21e07 60 void reset(){ // Setting home point for increment encoder
stivending 5:c040faf21e07 61 /*while (home_button == 0){ // Continue to turn clockwise until home button pressed
stivending 5:c040faf21e07 62 out1 = 1;
stivending 5:c040faf21e07 63 out2 = 0;
stivending 5:c040faf21e07 64 }*/
stivending 7:6e59ed00a6a9 65 out1 = 0;
stivending 7:6e59ed00a6a9 66 out2 = 0;
stivending 5:c040faf21e07 67 dc_motor.reset(); // Reset pulse_
stivending 7:6e59ed00a6a9 68 wait(0.3);
stivending 5:c040faf21e07 69 };
stivending 5:c040faf21e07 70
stivending 5:c040faf21e07 71 void move_angle(int angle){ // move for relative distance
stivending 5:c040faf21e07 72 reset();
stivending 5:c040faf21e07 73 goto_angle(angle);
stivending 5:c040faf21e07 74 reset();
stivending 5:c040faf21e07 75 };
stivending 7:6e59ed00a6a9 76
stivending 7:6e59ed00a6a9 77 void set_out(int a, int b)
stivending 7:6e59ed00a6a9 78 {
stivending 7:6e59ed00a6a9 79 out1 = a;
stivending 7:6e59ed00a6a9 80 out2 = b;
stivending 7:6e59ed00a6a9 81 }
stivending 7:6e59ed00a6a9 82 int get_pulse()
stivending 7:6e59ed00a6a9 83 {
stivending 7:6e59ed00a6a9 84 return dc_motor.getPulses();
stivending 7:6e59ed00a6a9 85 }
stivending 5:c040faf21e07 86 };
stivending 5:c040faf21e07 87 #endif