hidaka sato / MotorControler

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

MotorControler.cpp@5:b46d1e179eb7, 2021-02-02 (annotated)

Committer:
sgrsn
Date:
Tue Feb 02 14:53:38 2021 +0000
Revision:
5:b46d1e179eb7
Parent:
4:a60052db674c
Child:
6:7955a025adee
Add operator overloading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:e3207dd7809a 1 #include "MotorControler.h"
sgrsn 0:e3207dd7809a 2
sgrsn 4:a60052db674c 3 MotorControler::MotorControler(PinName DIR, PinName PWM, PinName nSLP, DriverType md_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 0:e3207dd7809a 9 }
sgrsn 0:e3207dd7809a 10
sgrsn 5:b46d1e179eb7 11 float MotorControler::operator = (float speed) {
sgrsn 5:b46d1e179eb7 12 setSpeed(speed);
sgrsn 5:b46d1e179eb7 13 return current_speed_;
sgrsn 5:b46d1e179eb7 14 }
sgrsn 5:b46d1e179eb7 15
sgrsn 5:b46d1e179eb7 16 float MotorControler::operator + (float speed) {
sgrsn 5:b46d1e179eb7 17 return current_speed_ + speed;
sgrsn 5:b46d1e179eb7 18 }
sgrsn 5:b46d1e179eb7 19
sgrsn 5:b46d1e179eb7 20 float MotorControler::operator += (float speed) {
sgrsn 5:b46d1e179eb7 21 setSpeed(current_speed_ + speed);
sgrsn 5:b46d1e179eb7 22 return current_speed_;
sgrsn 5:b46d1e179eb7 23 }
sgrsn 5:b46d1e179eb7 24
sgrsn 5:b46d1e179eb7 25 float MotorControler::operator - (float speed) {
sgrsn 5:b46d1e179eb7 26 return current_speed_ - speed;
sgrsn 5:b46d1e179eb7 27 }
sgrsn 5:b46d1e179eb7 28
sgrsn 5:b46d1e179eb7 29 float MotorControler::operator -= (float speed) {
sgrsn 5:b46d1e179eb7 30 setSpeed(current_speed_ - speed);
sgrsn 5:b46d1e179eb7 31 return current_speed_;
sgrsn 5:b46d1e179eb7 32 }
sgrsn 5:b46d1e179eb7 33
sgrsn 5:b46d1e179eb7 34 float MotorControler::operator * (float val) {
sgrsn 5:b46d1e179eb7 35 return current_speed_ * val;
sgrsn 5:b46d1e179eb7 36 }
sgrsn 5:b46d1e179eb7 37
sgrsn 5:b46d1e179eb7 38 float MotorControler::operator *= (float val) {
sgrsn 5:b46d1e179eb7 39 setSpeed(current_speed_ * val);
sgrsn 5:b46d1e179eb7 40 return current_speed_;
sgrsn 5:b46d1e179eb7 41 }
sgrsn 5:b46d1e179eb7 42
sgrsn 5:b46d1e179eb7 43 float MotorControler::operator / (float val) {
sgrsn 5:b46d1e179eb7 44 return current_speed_ / val;
sgrsn 5:b46d1e179eb7 45 }
sgrsn 5:b46d1e179eb7 46
sgrsn 5:b46d1e179eb7 47 float MotorControler::operator /= (float val) {
sgrsn 5:b46d1e179eb7 48 setSpeed(current_speed_ / val);
sgrsn 5:b46d1e179eb7 49 return current_speed_;
sgrsn 5:b46d1e179eb7 50 }
sgrsn 5:b46d1e179eb7 51
sgrsn 0:e3207dd7809a 52 void MotorControler::enableDriver()
sgrsn 0:e3207dd7809a 53 {
sgrsn 4:a60052db674c 54 if(md_type_ == POLOLU)
sgrsn 4:a60052db674c 55 {
sgrsn 4:a60052db674c 56 nSLP_ = 1;
sgrsn 4:a60052db674c 57 }
sgrsn 4:a60052db674c 58 else if(md_type_ == TEXNITIS)
sgrsn 4:a60052db674c 59 {
sgrsn 4:a60052db674c 60 // do nothing;
sgrsn 4:a60052db674c 61 }
sgrsn 0:e3207dd7809a 62 }
sgrsn 0:e3207dd7809a 63
sgrsn 0:e3207dd7809a 64 void MotorControler::disableDriver()
sgrsn 0:e3207dd7809a 65 {
sgrsn 4:a60052db674c 66 if(md_type_ == POLOLU)
sgrsn 4:a60052db674c 67 nSLP_ = 0;
sgrsn 4:a60052db674c 68 else if(md_type_ == TEXNITIS)
sgrsn 4:a60052db674c 69 PWM_ = 0;
sgrsn 0:e3207dd7809a 70 }
sgrsn 0:e3207dd7809a 71
sgrsn 0:e3207dd7809a 72 void MotorControler::setSpeed(float speed)
sgrsn 0:e3207dd7809a 73 {
sgrsn 0:e3207dd7809a 74 uint8_t reverse = 0;
sgrsn 5:b46d1e179eb7 75
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 0:e3207dd7809a 86
sgrsn 3:7acc824ca344 87 PWM_ = speed;
sgrsn 3:7acc824ca344 88 if (reverse ^ FLIP_MOTOR_DIR ^ reverse_direction_)
sgrsn 0:e3207dd7809a 89 {
sgrsn 3:7acc824ca344 90 DIR_ = 1;
sgrsn 0:e3207dd7809a 91 }
sgrsn 0:e3207dd7809a 92 else
sgrsn 0:e3207dd7809a 93 {
sgrsn 3:7acc824ca344 94 DIR_ = 0;
sgrsn 0:e3207dd7809a 95 }
sgrsn 1:ed3dfe4ecc8a 96 }
sgrsn 1:ed3dfe4ecc8a 97
sgrsn 1:ed3dfe4ecc8a 98 void MotorControler::setMotorDirection(MotorDirection dir)
sgrsn 1:ed3dfe4ecc8a 99 {
sgrsn 3:7acc824ca344 100 reverse_direction_ = dir;
sgrsn 2:543ff0150de1 101 }
sgrsn 2:543ff0150de1 102
sgrsn 2:543ff0150de1 103 void MotorControler::setPwmFrequency(float frequency)
sgrsn 2:543ff0150de1 104 {
sgrsn 2:543ff0150de1 105 float period = 1.0/frequency;
sgrsn 3:7acc824ca344 106 PWM_.period(period);
sgrsn 0:e3207dd7809a 107 }