hidaka sato / MotorControler

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

MotorControler.cpp@11:7493df5c5c40, 2021-02-21 (annotated)

Committer:
sgrsn
Date:
Sun Feb 21 10:34:19 2021 +0000
Branch:
use_base
Revision:
11:7493df5c5c40
Parent:
10:4413d4830297 
Change nSLP pin default output when using TEXNITISController

Who changed what in which revision?

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