first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
Diff: motor_drive.cpp
- Revision:
- 0:aefa71eaf32a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_drive.cpp Sun Sep 24 05:24:19 2017 +0000 @@ -0,0 +1,36 @@ +#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); +} \ No newline at end of file