Simple motor controller library, using DIR, PWM, nSLP pin like pololu.
MotorControler.cpp
- Committer:
- sgrsn
- Date:
- 2021-02-21
- Branch:
- use_base
- Revision:
- 10:4413d4830297
- Parent:
- 9:a5d6835f7168
File content as of revision 10:4413d4830297:
#include "MotorControler.h" /*example******************************************************************* #include "mbed.h" #include <math.h> #include "MotorControler.h" int main() { TEXNITISControler MOTOR(D8,D6,D7); Serial PC(USBTX, USBRX); MOTOR.setPwmFrequency(20000); MOTOR.enableDriver(); for(int i = 0; i < 360; i++) { float speed = MOTOR = sin((float)i * 3.14/180); wait_ms(20); PC.printf("%f\r\n", speed); } MOTOR = 0; while(1) { } } ********************************************************************/ MotorControler::MotorControler(PinName DIR, PinName PWM, PinName nSLP, ControlType control_type) : DIR_(DIR), PWM_(PWM), nSLP_(nSLP) { disableDriver(); reverse_direction_ = 0; current_speed_ = 0; control_type_ = control_type; } float MotorControler::operator = (float speed) { setSpeed(speed); return current_speed_; } float MotorControler::operator + (float speed) { return current_speed_ + speed; } float MotorControler::operator += (float speed) { setSpeed(current_speed_ + speed); return current_speed_; } float MotorControler::operator - (float speed) { return current_speed_ - speed; } float MotorControler::operator -= (float speed) { setSpeed(current_speed_ - speed); return current_speed_; } float MotorControler::operator * (float val) { return current_speed_ * val; } float MotorControler::operator *= (float val) { setSpeed(current_speed_ * val); return current_speed_; } float MotorControler::operator / (float val) { return current_speed_ / val; } float MotorControler::operator /= (float val) { setSpeed(current_speed_ / val); return current_speed_; } void MotorControler::setSpeed(float speed) { uint8_t reverse = 0; // 最大デューティ比で制限 if(speed < -1) speed = -1; else if(speed > 1) speed = 1; current_speed_ = speed; if (speed < 0) { speed = -speed; // Make speed a positive quantity reverse = 1; // Preserve the direction } int8_t dir = 1; if (reverse ^ FLIP_MOTOR_DIR ^ reverse_direction_) { // CCW dir = -1; } else { // CW dir = 1; } if(control_type_ == SM) { setPinUsingSignMagnitude(dir, speed); } else if(control_type_ == LAP) { setPinusingLockedUntiPhase(dir, speed); } } void MotorControler::setPinUsingSignMagnitude(int8_t dir, float speed) { if(dir > 0) DIR_ = 0; else if(dir < 0) DIR_ = 1; PWM_ = speed; } void MotorControler::setPinusingLockedUntiPhase(int8_t dir, float speed) { PWM_ = 1.0; float dir_input = 0.5 + 0.5 * speed * dir; DIR_ = dir_input; } void MotorControler::setMotorDirection(MotorDirection dir) { reverse_direction_ = dir; } void MotorControler::setPwmFrequency(float frequency) { float period = 1.0/frequency; PWM_.period(period); }