forked from MyRobotLab/mrlcomm
-
Notifications
You must be signed in to change notification settings - Fork 0
/
MrlServo.cpp
123 lines (110 loc) · 2.82 KB
/
MrlServo.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#include <Servo.h>
#include "Msg.h"
#include "Device.h"
#include "MrlServo.h"
MrlServo::MrlServo(int deviceId) : Device(deviceId, DEVICE_TYPE_SERVO) {
isMoving = false;
isSweeping = false;
// create the servo
servo = new Servo();
lastUpdate = 0;
currentPos = 0.0;
targetPos = 0;
velocity = -1;
}
MrlServo::~MrlServo() {
if (servo){
servo->detach();
delete servo;
}
}
// this method "may" be called with a pin or pin & pos depending on
// config size
bool MrlServo::attach(byte pin, byte initPos, int initVelocity){
// msg->publishDebug("MrlServo.deviceAttach !!!");
servo->write(initPos);
currentPos = initPos;
targetPos = initPos;
velocity = initVelocity;
servo->attach(pin);
return true;
}
// This method is equivalent to Arduino's Servo.attach(pin) - (no pos)
void MrlServo::enablePwm(int pin){
servo->attach(pin);
servo->write((int)currentPos); //return to it's last know state (may be 0 if currentPos is not set)
// TODO-KW: we should always have a moveTo for safety, o/w we have no idea what angle we're going to start up at.. maybe
}
void MrlServo::disablePwm(){
servo->detach();
}
// FIXME - what happened to events ?
void MrlServo::update() {
//it may have an imprecision of +- 1 due to the conversion of currentPos to int
if (isMoving) {
if ((int)currentPos != targetPos) {
long deltaTime = millis() - lastUpdate;
float step = velocity * deltaTime;
step /= 1000; //for deg/ms;
if (isSweeping) {
step = sweepStep;
}
if (velocity < 0) { // when velocity < 0, it mean full speed ahead
step = targetPos - currentPos;
}
else if ((int)currentPos > targetPos) {
step *=-1;
}
currentPos += step;
if ((step > 0.0 && (int)currentPos > targetPos) || (step < 0.0 && (int)currentPos < targetPos)) {
currentPos = targetPos;
}
lastUpdate = millis();
servo->write((int)currentPos);
}
else {
if (isSweeping) {
if (targetPos == min) {
targetPos = max;
}
else {
targetPos = min;
}
sweepStep *= -1;
}
else {
isMoving = false;
}
}
}
}
void MrlServo::servoWrite(int position) {
if (servo == NULL)
return;
targetPos = position;
isMoving = true;
lastUpdate = millis();
}
void MrlServo::servoWriteMicroseconds(int position) {
if (servo) {
servo->writeMicroseconds(position);
}
}
void MrlServo::startSweep(int min, int max, int step) {
this->min = min;
this->max = max;
sweepStep = step;
targetPos = max;
isMoving = true;
isSweeping = true;
}
void MrlServo::stopSweep() {
isMoving = false;
isSweeping = false;
}
void MrlServo::setMaxVelocity(unsigned int velocity){
maxVelocity = velocity;
}
void MrlServo::setVelocity(unsigned int velocity) {
this->velocity = velocity;
}