/
Servo.cpp
135 lines (103 loc) · 3.64 KB
/
Servo.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
124
125
126
127
128
129
130
131
132
133
134
135
#include "Servo.h"
#include "EZB.h"
#include <time.h>
ServoClass::ServoClass(EZB* ezb){
m_ezb = ezb;
struct timespec now;
clock_gettime(1, &now);
long nowms = (now.tv_sec * 1000) + (now.tv_nsec / 1000000);
for(int i = 0; i < NA; i++){
m_servos[i].curr_pos = 0;
m_servos[i].curr_speed = 0;
m_servos[i].last_move_time = nowms;
m_servos[i].isreleased = true;
}
}
double ServoClass::GetNumberOfSecondsSinceLastMove(ServoPortEnum servoPort){
struct timespec now;
clock_gettime(1, &now);
long nowms = (now.tv_sec * 1000) + (now.tv_nsec / 1000000);
double num_seconds = (double)(((double)nowms - (double)m_servos[servoPort].last_move_time) / 1000);
return num_seconds;
}
int ServoClass::GetServoPosition(ServoPortEnum servoPort){
return m_servos[servoPort].curr_pos;
}
int ServoClass::GetServoSpeed(ServoPortEnum servoPort){
return m_servos[servoPort].curr_speed;
}
bool ServoClass::IsServoReleased(ServoPortEnum servoPort){
return m_servos[servoPort].isreleased;
}
void ServoClass::SetServoPosition(ServoPortEnum servoPort, int position, int speed){
if(position > SERVO_MAX)
position = SERVO_MAX;
else if(position < SERVO_MIN)
position = SERVO_MIN;
unsigned char args[1];
if(speed > -1){
SetServoSpeed(servoPort, speed);
}
args[0] = position;
m_servos[servoPort].curr_pos = position;
m_servos[servoPort].isreleased = false;
struct timespec now;
clock_gettime(1, &now);
m_servos[servoPort].last_move_time = (now.tv_sec * 1000) + (now.tv_nsec / 1000000);
if(servoPort != NA)
m_ezb->SendCommand(servoPort + EZB::SetServoPosition, args, 1);
}
void ServoClass::SetServoPositionScalar(ServoPortEnum servoPort, int servoMin, int servoMax, int clientWidthMin, int clientWidthMax, int clientPosition, bool invert){
SetServoPositionScalar(servoPort, servoMin, servoMax, (float)clientWidthMin, (float)clientWidthMax, (float)clientPosition, invert);
}
void ServoClass::SetServoPositionScalar(ServoPortEnum servoPort, int servoMin, int servoMax, float clientWidthMin, float clientWidthMax, float clientPosition, bool invert)
{
if (clientWidthMin < 0.0f){
clientWidthMax += abs(clientWidthMin);
clientPosition += abs(clientWidthMin);
clientWidthMin = 0.0f;
}
float num = ((float)(servoMax - servoMin)) / (clientWidthMax - clientWidthMin);
int position = (int) (num * clientPosition);
if (invert)
position = servoMax - position;
else
position += servoMin;
if (position > servoMax)
position = servoMax;
if (position < servoMin)
position = servoMin;
SetServoPosition(servoPort, position);
}
void ServoClass::SetServoSpeed(ServoPortEnum servoPort, int speed){
if(speed > SERVO_SPEED_SLOWEST)
speed = SERVO_SPEED_SLOWEST;
else if(speed < SERVO_SPEED_FASTEST)
speed = SERVO_SPEED_FASTEST;
unsigned char args[1];
args[0] = speed;
m_servos[servoPort].curr_speed = speed;
if(servoPort != NA)
m_ezb->SendCommand(servoPort + EZB::SetServoSpeed, args, 1);
}
void ServoClass::ReleaseServo(ServoPortEnum servoPort){
if(!m_servos[servoPort].isreleased){
unsigned char args[1];
args[0] = SERVO_OFF;
if(servoPort != NA)
m_ezb->SendCommand(servoPort + EZB::SetServoPosition, args, 1);
m_servos[servoPort].isreleased = true;
struct timespec now;
clock_gettime(1, &now);
m_servos[servoPort].last_move_time = (now.tv_sec * 1000) + (now.tv_nsec / 1000000);
}
}
void ServoClass::ReleaseAllServos(){
m_ezb->SendCommand(EZB::ReleaseAllServos);
for(int i = 0; i < NA; i++){
m_servos[i].isreleased = true;
struct timespec now;
clock_gettime(1, &now);
m_servos[i].last_move_time = (now.tv_sec * 1000) + (now.tv_nsec / 1000000);
}
}