spdis
61 lines
1.5 KiB
C++
61 lines
1.5 KiB
C++
#include <iostream>
|
|
#include <fstream>
|
|
#include <opencv2/opencv.hpp>
|
|
#include <string>
|
|
#include <csignal>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <thread>
|
|
|
|
#include "global.h"
|
|
#include "camera.h"
|
|
#include "control.h"
|
|
|
|
std::atomic<bool> running(true);
|
|
void signalHandler(int signal)
|
|
{
|
|
running.store(false);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
std::signal(SIGINT, signalHandler);
|
|
|
|
double dest_fps = readDoubleFromFile(destfps_file);
|
|
if (dest_fps <= 0) dest_fps = 30.0;
|
|
|
|
if (CameraInit(0, dest_fps, 320, 240) < 0) {
|
|
std::cerr << "CameraInit failed" << std::endl;
|
|
return -1;
|
|
}
|
|
ControlInit();
|
|
std::cout << "All services started (single-thread mode)" << std::endl;
|
|
|
|
int tick = 0;
|
|
while (running.load())
|
|
{
|
|
if (CameraHandler() < 0) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|
continue;
|
|
}
|
|
|
|
if (++tick % 15 == 0)
|
|
{
|
|
target_speed = readDoubleFromFile(speed_file);
|
|
mortor_kp = readDoubleFromFile(mortor_kp_file);
|
|
mortor_ki = readDoubleFromFile(mortor_ki_file);
|
|
mortor_kd = readDoubleFromFile(mortor_kd_file);
|
|
kp = readDoubleFromFile(kp_file);
|
|
ki = readDoubleFromFile(ki_file);
|
|
kd = readDoubleFromFile(kd_file);
|
|
}
|
|
}
|
|
|
|
std::cout << "Stopping..." << std::endl;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
ControlExit();
|
|
cameraDeInit();
|
|
std::cout << "Stopped." << std::endl;
|
|
return 0;
|
|
}
|