SentryRTC-SentrySideCpp/ServoHandler.cpp at main · jedhutto/SentryRTC-SentrySideCpp · GitHub

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
#include "ServoHandler.h"
#include <iostream>
#include <cstdlib>

ServoHandler::ServoHandler(int pi, int channel, PCA9685 pca9685)
{
	this->channel = channel;
	this->pi = pi;
	this->servoDriver = pca9685;
}

ServoHandler::~ServoHandler()
{
}

void ServoHandler::start()
{
	this->mhThread = std::thread(&ServoHandler::MovementLoop, std::ref(read), std::ref(ss), std::ref(servoDriver), std::ref(channel));
	this->mhThread.detach();
}

void ServoHandler::setSignal(ServoSignal)
{
}

void ServoHandler::MovementLoop(bool& read, ServoSignal& ss, PCA9685& servoDriver, int& channel)
{
	int16_t currentPulseWidth[5] = { 1500, 1250, 1250, 1250, 1250 };
	int16_t maxPulseWidthChange[5] = { 10, 15, 20, 20, 20 };

	while (true) {
		for (int i = 0; i < 5; i++) {
			if (500 <= ss.pulseWidths[i] && ss.pulseWidths[i] <= 2500) {
				read = false;
				if (currentPulseWidth[i] != ss.pulseWidths[i]) {
					float diff = abs(currentPulseWidth[i] - ss.pulseWidths[i]);
					if (diff > maxPulseWidthChange[i]) {
						if (currentPulseWidth[i] < ss.pulseWidths[i]) {
							currentPulseWidth[i] += maxPulseWidthChange[i];
						}
						else {
							currentPulseWidth[i] -= maxPulseWidthChange[i];
						}
					}
					else {
						currentPulseWidth[i] = ss.pulseWidths[i];
					}
					servoDriver.SetDutyCycle(i, currentPulseWidth[i]);
				}
			}
		}
		//if (500 <= ss.pulseWidth0 && ss.pulseWidth0 <= 2500) {
		//	read = false;
		//	if (currentPulseWidth[0] != ss.pulseWidth0){
		//		float diff = abs(currentPulseWidth[0] - ss.pulseWidth0);
		//		if (diff > maxPulseWidthChange[0]) {
		//			if (currentPulseWidth[0] < ss.pulseWidth0) {
		//				currentPulseWidth[0] += maxPulseWidthChange[0];
		//			}
		//			else {
		//				currentPulseWidth[0] -= maxPulseWidthChange[0];
		//			}
		//		}
		//		else {
		//			currentPulseWidth[0] = ss.pulseWidth0;
		//		}
		//		servoDriver.SetDutyCycle(channel, currentPulseWidth[0]);
		//	}
		//}

		std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
	}
}