hidaka sato / MotorControler

Simple motor controller library, using DIR, PWM, nSLP pin like pololu.

MotorControler.cpp@6:7955a025adee, 2021-02-02 (annotated)

Committer:
sgrsn
Date:
Tue Feb 02 15:19:09 2021 +0000
Branch:
SM_LAP_Switchng
Revision:
6:7955a025adee
Parent:
5:b46d1e179eb7
Child:
8:ad953e0e3c0f
Add switching SM or LAP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:e3207dd7809a 1 #include "MotorControler.h"
sgrsn 0:e3207dd7809a 2
sgrsn 6:7955a025adee 3 MotorControler::MotorControler(PinName DIR, PinName PWM, PinName nSLP, DriverType md_type, ControlType control_type) : DIR_(DIR), PWM_(PWM), nSLP_(nSLP)
sgrsn 0:e3207dd7809a 4 {
sgrsn 4:a60052db674c 5 disableDriver();
sgrsn 3:7acc824ca344 6 reverse_direction_ = 0;
sgrsn 5:b46d1e179eb7 7 current_speed_ = 0;
sgrsn 4:a60052db674c 8 md_type_ = md_type;
sgrsn 6:7955a025adee 9 control_type_ = control_type;
sgrsn 0:e3207dd7809a 10 }
sgrsn 0:e3207dd7809a 11
sgrsn 5:b46d1e179eb7 12 float MotorControler::operator = (float speed) {
sgrsn 5:b46d1e179eb7 13 setSpeed(speed);
sgrsn 5:b46d1e179eb7 14 return current_speed_;
sgrsn 5:b46d1e179eb7 15 }
sgrsn 5:b46d1e179eb7 16
sgrsn 5:b46d1e179eb7 17 float MotorControler::operator + (float speed) {
sgrsn 5:b46d1e179eb7 18 return current_speed_ + speed;
sgrsn 5:b46d1e179eb7 19 }
sgrsn 5:b46d1e179eb7 20
sgrsn 5:b46d1e179eb7 21 float MotorControler::operator += (float speed) {
sgrsn 5:b46d1e179eb7 22 setSpeed(current_speed_ + speed);
sgrsn 5:b46d1e179eb7 23 return current_speed_;
sgrsn 5:b46d1e179eb7 24 }
sgrsn 5:b46d1e179eb7 25
sgrsn 5:b46d1e179eb7 26 float MotorControler::operator - (float speed) {
sgrsn 5:b46d1e179eb7 27 return current_speed_ - speed;
sgrsn 5:b46d1e179eb7 28 }
sgrsn 5:b46d1e179eb7 29
sgrsn 5:b46d1e179eb7 30 float MotorControler::operator -= (float speed) {
sgrsn 5:b46d1e179eb7 31 setSpeed(current_speed_ - speed);
sgrsn 5:b46d1e179eb7 32 return current_speed_;
sgrsn 5:b46d1e179eb7 33 }
sgrsn 5:b46d1e179eb7 34
sgrsn 5:b46d1e179eb7 35 float MotorControler::operator * (float val) {
sgrsn 5:b46d1e179eb7 36 return current_speed_ * val;
sgrsn 5:b46d1e179eb7 37 }
sgrsn 5:b46d1e179eb7 38
sgrsn 5:b46d1e179eb7 39 float MotorControler::operator *= (float val) {
sgrsn 5:b46d1e179eb7 40 setSpeed(current_speed_ * val);
sgrsn 5:b46d1e179eb7 41 return current_speed_;
sgrsn 5:b46d1e179eb7 42 }
sgrsn 5:b46d1e179eb7 43
sgrsn 5:b46d1e179eb7 44 float MotorControler::operator / (float val) {
sgrsn 5:b46d1e179eb7 45 return current_speed_ / val;
sgrsn 5:b46d1e179eb7 46 }
sgrsn 5:b46d1e179eb7 47
sgrsn 5:b46d1e179eb7 48 float MotorControler::operator /= (float val) {
sgrsn 5:b46d1e179eb7 49 setSpeed(current_speed_ / val);
sgrsn 5:b46d1e179eb7 50 return current_speed_;
sgrsn 5:b46d1e179eb7 51 }
sgrsn 5:b46d1e179eb7 52
sgrsn 0:e3207dd7809a 53 void MotorControler::enableDriver()
sgrsn 0:e3207dd7809a 54 {
sgrsn 4:a60052db674c 55 if(md_type_ == POLOLU)
sgrsn 4:a60052db674c 56 {
sgrsn 4:a60052db674c 57 nSLP_ = 1;
sgrsn 4:a60052db674c 58 }
sgrsn 4:a60052db674c 59 else if(md_type_ == TEXNITIS)
sgrsn 4:a60052db674c 60 {
sgrsn 4:a60052db674c 61 // do nothing;
sgrsn 4:a60052db674c 62 }
sgrsn 0:e3207dd7809a 63 }
sgrsn 0:e3207dd7809a 64
sgrsn 0:e3207dd7809a 65 void MotorControler::disableDriver()
sgrsn 0:e3207dd7809a 66 {
sgrsn 4:a60052db674c 67 if(md_type_ == POLOLU)
sgrsn 4:a60052db674c 68 nSLP_ = 0;
sgrsn 4:a60052db674c 69 else if(md_type_ == TEXNITIS)
sgrsn 4:a60052db674c 70 PWM_ = 0;
sgrsn 0:e3207dd7809a 71 }
sgrsn 0:e3207dd7809a 72
sgrsn 0:e3207dd7809a 73 void MotorControler::setSpeed(float speed)
sgrsn 0:e3207dd7809a 74 {
sgrsn 0:e3207dd7809a 75 uint8_t reverse = 0;
sgrsn 5:b46d1e179eb7 76 // 最大デューティ比で制限
sgrsn 5:b46d1e179eb7 77 if(speed < -1) speed = -1;
sgrsn 5:b46d1e179eb7 78 else if(speed > 1) speed = 1;
sgrsn 5:b46d1e179eb7 79 current_speed_ = speed;
sgrsn 5:b46d1e179eb7 80
sgrsn 0:e3207dd7809a 81 if (speed < 0)
sgrsn 0:e3207dd7809a 82 {
sgrsn 0:e3207dd7809a 83 speed = -speed; // Make speed a positive quantity
sgrsn 0:e3207dd7809a 84 reverse = 1; // Preserve the direction
sgrsn 0:e3207dd7809a 85 }
sgrsn 6:7955a025adee 86
sgrsn 6:7955a025adee 87 int8_t dir = 1;
sgrsn 3:7acc824ca344 88 if (reverse ^ FLIP_MOTOR_DIR ^ reverse_direction_)
sgrsn 0:e3207dd7809a 89 {
sgrsn 6:7955a025adee 90 // CCW
sgrsn 6:7955a025adee 91 dir = -1;
sgrsn 0:e3207dd7809a 92 }
sgrsn 0:e3207dd7809a 93 else
sgrsn 0:e3207dd7809a 94 {
sgrsn 6:7955a025adee 95 // CW
sgrsn 6:7955a025adee 96 dir = 1;
sgrsn 6:7955a025adee 97 }
sgrsn 6:7955a025adee 98
sgrsn 6:7955a025adee 99 if(control_type_ == SM)
sgrsn 6:7955a025adee 100 {
sgrsn 6:7955a025adee 101 setPinUsingSignMagnitude(dir, speed);
sgrsn 6:7955a025adee 102 }
sgrsn 6:7955a025adee 103 else if(control_type_ == LAP)
sgrsn 6:7955a025adee 104 {
sgrsn 6:7955a025adee 105 setPinusingLockedUntiPhase(dir, speed);
sgrsn 0:e3207dd7809a 106 }
sgrsn 1:ed3dfe4ecc8a 107 }
sgrsn 1:ed3dfe4ecc8a 108
sgrsn 6:7955a025adee 109 void MotorControler::setPinUsingSignMagnitude(int8_t dir, float speed)
sgrsn 6:7955a025adee 110 {
sgrsn 6:7955a025adee 111 if(dir > 0) DIR_ = 0;
sgrsn 6:7955a025adee 112 else if(dir < 0) DIR_ = 1;
sgrsn 6:7955a025adee 113 PWM_ = speed;
sgrsn 6:7955a025adee 114 }
sgrsn 6:7955a025adee 115
sgrsn 6:7955a025adee 116 void MotorControler::setPinusingLockedUntiPhase(int8_t dir, float speed)
sgrsn 6:7955a025adee 117 {
sgrsn 6:7955a025adee 118 PWM_ = 1.0;
sgrsn 6:7955a025adee 119 float dir_input = 0.5 + 0.5 * speed * dir;
sgrsn 6:7955a025adee 120 DIR_ = dir_input;
sgrsn 6:7955a025adee 121 }
sgrsn 6:7955a025adee 122
sgrsn 6:7955a025adee 123
sgrsn 1:ed3dfe4ecc8a 124 void MotorControler::setMotorDirection(MotorDirection dir)
sgrsn 1:ed3dfe4ecc8a 125 {
sgrsn 3:7acc824ca344 126 reverse_direction_ = dir;
sgrsn 2:543ff0150de1 127 }
sgrsn 2:543ff0150de1 128
sgrsn 2:543ff0150de1 129 void MotorControler::setPwmFrequency(float frequency)
sgrsn 2:543ff0150de1 130 {
sgrsn 2:543ff0150de1 131 float period = 1.0/frequency;
sgrsn 3:7acc824ca344 132 PWM_.period(period);
sgrsn 0:e3207dd7809a 133 }