hidaka sato / MotorControler

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

MotorControler.cpp@9:a5d6835f7168, 2021-02-21 (annotated)

Committer:
sgrsn
Date:
Sun Feb 21 09:23:01 2021 +0000
Branch:
use_base
Revision:
9:a5d6835f7168
Parent:
8:ad953e0e3c0f
Child:
10:4413d4830297
Use base class

Who changed what in which revision?

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