kusano kiyoshige / motor_drive

first

Dependents:   17robo_fuzi 17robo_tokyo_kaede

motor_drive.cpp

Committer:
echo_piyo
Date:
2017-09-24
Revision:
0:aefa71eaf32a

File content as of revision 0:aefa71eaf32a:

#include "motor_drive.h"

MotorDrive::MotorDrive (PinName Direction,PinName Pwm) : direction(Direction), pwm(Pwm){
    direction = 0;
    pwm    = 0;
}

void MotorDrive::setup(float frequency, float acceleration, float timerCycle){
    float period = 1/frequency;
    pwm.period(period);
    a = acceleration*timerCycle;
}

void MotorDrive::output(float targetDuty){
    if (fabs(targetDuty-duty) <= a) {
        duty = targetDuty;
    } else if (duty < targetDuty) {
        duty = duty + a;
    } else if (duty > targetDuty) {
        duty = duty - a;
    }
    
    if (duty < 0) {
        direction  = 1;
        if (duty > DUTY_LIMIT) {
            duty = DUTY_LIMIT;
        }
    } else {
        direction  = 0;
        if (duty < -DUTY_LIMIT) {
            duty = -DUTY_LIMIT;
        }
    }

    pwm = fabs(duty);
}