59 lines
1.6 KiB
C++
59 lines
1.6 KiB
C++
#include "ServoMotorComponent.h"
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////
|
|
// Functions for setting up the ServoMotor
|
|
/////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/**
|
|
* Constructor.
|
|
* Prepares the ServoMotor.
|
|
*/
|
|
ServoMotorComponent::ServoMotorComponent(int PIN, unsigned long updatePeriod, float step) {
|
|
this->PIN = PIN;
|
|
this->myservo.attach(PIN);
|
|
this->desiredposition = Position::MIDDLE;
|
|
this->currentPosition = 32;
|
|
this->lastUpTime = millis();
|
|
this->updatePeriod = updatePeriod;
|
|
this->step = step;
|
|
this->myservo.write(this->PIN, this->currentPosition);
|
|
}
|
|
|
|
/**
|
|
* Set the desired position
|
|
* @desiredPosition: Give desired position
|
|
*/
|
|
void ServoMotorComponent::setDesiredPosition(Position desiredPosition) {
|
|
switch (desiredPosition) {
|
|
case Position::LEFT:
|
|
this->desiredposition = 52;
|
|
break;
|
|
case Position::MIDDLE:
|
|
this->desiredposition = 32;
|
|
break;
|
|
case Position::RIGHT:
|
|
this->desiredposition = 18;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Write a new servoMotor position when it's necessary
|
|
*/
|
|
void ServoMotorComponent::refresh() {
|
|
if (this->desiredposition == this->currentPosition
|
|
|| millis() - this->lastUpTime <= this->updatePeriod) return;
|
|
|
|
if (this->currentPosition > this->desiredposition) {
|
|
this->currentPosition--;
|
|
}
|
|
if (this->currentPosition < this->desiredposition) {
|
|
this->currentPosition++;
|
|
}
|
|
this->lastUpTime = millis();
|
|
this->myservo.write(this->PIN, this->currentPosition);
|
|
} |