113 lines
3.3 KiB
C++
113 lines
3.3 KiB
C++
#include "Motor.h"
|
|
|
|
Motor::Motor(int dirPin, int stepPin, int HIGHLimit, int LOWLimit, float stepDistance, float speed) {
|
|
this->dirPin = dirPin;
|
|
this->stepPin = stepPin;
|
|
this->HIGHLimit = HIGHLimit;
|
|
this->LOWLimit = LOWLimit;
|
|
this->stepDistance = stepDistance;
|
|
this->speed = speed;
|
|
pinMode(dirPin, OUTPUT);
|
|
pinMode(stepPin, OUTPUT);
|
|
pinMode(HIGHLimit, INPUT_PULLUP);
|
|
pinMode(LOWLimit, INPUT_PULLUP);
|
|
stepper = AccelStepper(AccelStepper::DRIVER, stepPin, dirPin);
|
|
// stepper.setMaxSpeed(speed); //馬達最大速度
|
|
// stepper.setSpeed(speed); //馬達初始化速度
|
|
// stepper.setAcceleration(speed * speed); // 馬達加速度
|
|
float steps = speed / stepDistance;
|
|
int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5); // 四舍五入
|
|
stepper.setMaxSpeed(stepsOut); //馬達最大速度
|
|
stepper.setSpeed(stepsOut); //馬達初始化速度
|
|
stepper.setAcceleration(stepsOut * stepsOut); // 馬達加速度
|
|
// stepper.setMinPulseWidth(minPulseWidth);
|
|
_currentPosition = 0;
|
|
_targetPosition = 0;
|
|
}
|
|
|
|
// 移動指定距離
|
|
void Motor::move(float distance) {
|
|
float steps = distance / stepDistance;
|
|
int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5); // 四舍五入
|
|
stepper.move(stepsOut);
|
|
while (stepper.distanceToGo() != 0) {
|
|
if (distance > 0)
|
|
{
|
|
if (digitalRead(LOWLimit) == LOW) {
|
|
stepper.move(-5);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (digitalRead(HIGHLimit) == LOW)
|
|
{
|
|
stepper.move(10);
|
|
}
|
|
}
|
|
stepper.run();
|
|
}
|
|
_currentPosition = stepper.currentPosition() * stepDistance;
|
|
}
|
|
|
|
// 停止馬達運動
|
|
void Motor::stop() {
|
|
stepper.stop();
|
|
}
|
|
|
|
//獲取馬達目前位置
|
|
float Motor::getCurrentPosition() {
|
|
return _currentPosition;
|
|
}
|
|
|
|
//重新設置馬達零點
|
|
void Motor::resetPosition() {
|
|
_currentPosition = 0;
|
|
_targetPosition = 0;
|
|
stepper.setCurrentPosition(0);
|
|
}
|
|
|
|
void Motor::moveDis(float distance) {
|
|
float steps = distance / stepDistance;
|
|
int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5); // 四舍五入
|
|
stepper.move(stepsOut);
|
|
while (stepper.distanceToGo() != 0) {
|
|
if (digitalRead(HIGHLimit) == LOW && steps < 0) {
|
|
stepper.move(10);
|
|
}
|
|
if (digitalRead(LOWLimit) == LOW && steps > 0) {
|
|
stepper.move(-10);
|
|
}
|
|
stepper.run();
|
|
}
|
|
_currentPosition = stepper.currentPosition() * stepDistance;
|
|
}
|
|
|
|
//碰撞上極限 每次移動-0.01mm
|
|
void Motor::moveToHighLim() {
|
|
while (digitalRead(HIGHLimit) == HIGH ) {
|
|
float steps = 1 / stepDistance;
|
|
int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5); // 四舍五入
|
|
stepper.move(-stepsOut); //-0.01mm
|
|
while (stepper.distanceToGo() != 0) {
|
|
stepper.run();
|
|
}
|
|
stepper.stop();
|
|
}
|
|
_currentPosition = stepper.currentPosition() * stepDistance;
|
|
}
|
|
void Motor::motorMove(float distance) {
|
|
int steps = distance / stepDistance;
|
|
stepper.move(steps);
|
|
while (stepper.distanceToGo() != 0) {
|
|
stepper.run();
|
|
}
|
|
_currentPosition = stepper.currentPosition() * stepDistance;
|
|
}
|
|
void Motor::changeSpeed(int speed) {
|
|
float steps = speed / stepDistance;
|
|
int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5); // 四舍五入
|
|
stepper.setMaxSpeed(stepsOut); //馬達最大速度
|
|
stepper.setSpeed(stepsOut); //馬達初始化速度
|
|
stepper.setAcceleration(stepsOut * stepsOut); // 馬達加速度
|
|
}
|