MotorControler.cpp

00001 #include "MotorControler.h"
00002 
00003 /*example*******************************************************************
00004 
00005 #include "mbed.h"
00006 #include <math.h>
00007 #include "MotorControler.h"
00008 
00009 int main() {
00010     TEXNITISControler MOTOR(D8,D6,D7);
00011     Serial PC(USBTX, USBRX);
00012     
00013     MOTOR.setPwmFrequency(20000);
00014     MOTOR.enableDriver();
00015     
00016     for(int i = 0; i < 360; i++)
00017     {
00018         float speed = MOTOR = sin((float)i * 3.14/180);
00019         wait_ms(20);
00020         PC.printf("%f\r\n", speed);
00021     }
00022     
00023     MOTOR = 0;
00024         
00025     while(1)
00026     {
00027     }
00028 }
00029 
00030 ********************************************************************/
00031 
00032 
00033 
00034 MotorControler::MotorControler(PinName DIR, PinName PWM, PinName nSLP, ControlType control_type) : DIR_(DIR), PWM_(PWM), nSLP_(nSLP)
00035 {
00036     disableDriver();
00037     reverse_direction_ = 0;
00038     current_speed_ = 0;
00039     control_type_ = control_type;
00040 }
00041 
00042 float MotorControler::operator = (float speed) {
00043     setSpeed(speed);
00044     return current_speed_;
00045 }
00046 
00047 float MotorControler::operator + (float speed) {
00048     return current_speed_ + speed;
00049 }
00050 
00051 float MotorControler::operator += (float speed) {
00052     setSpeed(current_speed_ + speed);
00053     return current_speed_;
00054 }
00055 
00056 float MotorControler::operator - (float speed) {
00057     return current_speed_ - speed;
00058 }
00059 
00060 float MotorControler::operator -= (float speed) {
00061     setSpeed(current_speed_ - speed);
00062     return current_speed_;
00063 }
00064 
00065 float MotorControler::operator * (float val) {
00066     return current_speed_ * val;
00067 }
00068 
00069 float MotorControler::operator *= (float val) {
00070     setSpeed(current_speed_ * val);
00071     return current_speed_;
00072 }
00073 
00074 float MotorControler::operator / (float val) {
00075     return current_speed_ / val;
00076 }
00077 
00078 float MotorControler::operator /= (float val) {
00079     setSpeed(current_speed_ / val);
00080     return current_speed_;
00081 }
00082 
00083 
00084 void MotorControler::setSpeed(float speed)
00085 {
00086     uint8_t reverse = 0;
00087     // 最大デューティ比で制限
00088     if(speed < -1) speed = -1;
00089     else if(speed > 1) speed = 1; 
00090     current_speed_ = speed;
00091     
00092     if (speed < 0)
00093     {
00094         speed = -speed;  // Make speed a positive quantity
00095         reverse = 1;  // Preserve the direction
00096     }
00097     
00098     int8_t dir = 1;
00099     if (reverse ^ FLIP_MOTOR_DIR ^ reverse_direction_)
00100     {
00101         // CCW
00102         dir = -1;
00103     }
00104     else
00105     {
00106         // CW
00107         dir = 1;
00108     }
00109     
00110     if(control_type_ == SM)
00111     {
00112         setPinUsingSignMagnitude(dir, speed);
00113     }
00114     else if(control_type_ == LAP)
00115     {
00116         setPinusingLockedUntiPhase(dir, speed);
00117     }
00118 }
00119 
00120 void MotorControler::setPinUsingSignMagnitude(int8_t dir, float speed)
00121 {
00122     if(dir > 0) DIR_ = 0;
00123     else if(dir < 0) DIR_ = 1;
00124     PWM_ = speed;
00125 }
00126 
00127 void MotorControler::setPinusingLockedUntiPhase(int8_t dir, float speed)
00128 {
00129     PWM_ = 1.0;
00130     float dir_input = 0.5 + 0.5 * speed * dir;
00131     DIR_ = dir_input;
00132 }
00133 
00134 
00135 void MotorControler::setMotorDirection(MotorDirection dir)
00136 {
00137     reverse_direction_ = dir;
00138 }
00139 
00140 void MotorControler::setPwmFrequency(float frequency)
00141 {
00142     float period = 1.0/frequency;
00143     PWM_.period(period);
00144 }