File content as of revision 11:7493df5c5c40:

#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);
}