-
Notifications
You must be signed in to change notification settings - Fork 1
/
cameras.cpp
55 lines (48 loc) · 1.44 KB
/
cameras.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
#ifndef CAMERAS_CPP
#define CAMERAS_CPP
#include "cameras.h"
#include "api/Common.h"
#include "config.h"
#include "logger.h"
#include <cctype>
Cameras::Cameras(bool launch, bool test) {
if (!launch) {
inactive = true;
Logger::info(F("Rotary cameras init cancelled\n\r"));
return;
}
Logger::info(F("Rotary cameras init\n\r"));
pinMode(config::cameras::multiplexer_pin, OUTPUT);
m_backServo.attach(config::cameras::back);
m_frontServo.attach(config::cameras::front);
if (test) {
Logger::info(F("Testing cameras rotation\n\r"));
for (byte pos = 0; pos <= 180; pos++) {
m_backServo.write(pos);
m_frontServo.write(pos);
delay(15);
}
}
m_backServo.write(angles[0]);
m_frontServo.write(angles[1]);
}
void Cameras::set_angle_delta(int idx, int angle_delta) {
if (inactive)
return;
int servo_idx = (idx == 0) ? config::cameras::servos::front
: config::cameras::servos::back;
if ((millis() - m_lastUpdateTimes[idx]) > 17) {
m_lastUpdateTimes[idx] = millis();
angles[idx] = constrain(angles[idx] + angle_delta, 0, 180);
if (idx == 0)
m_frontServo.write(angles[idx]);
else
m_backServo.write(angles[idx]);
}
}
void Cameras::select_cam(int index) {
if (inactive)
return;
digitalWrite(config::cameras::multiplexer_pin, index);
}
#endif