hidaka sato / MotorControler

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

MotorControler.cpp@8:ad953e0e3c0f, 2021-02-20 (annotated)

Committer:
sgrsn
Date:
Sat Feb 20 08:05:15 2021 +0000
Revision:
8:ad953e0e3c0f
Parent:
6:7955a025adee
Child:
9:a5d6835f7168
If MD type is TEXNITIS, output 0V on nSLP(RESET pin).

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