88 lines
1.9 KiB
C++
88 lines
1.9 KiB
C++
#include "control_module.h"
|
|
|
|
ControlModule::ControlModule(): ControlModule(0.0, 1.0, 1.0)
|
|
{
|
|
}
|
|
|
|
ControlModule::ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed)
|
|
{
|
|
motors[0] = 0.0;
|
|
motors[1] = 0.0;
|
|
motors[2] = 0.0;
|
|
motors[3] = 0.0;
|
|
this->maxSpeed = maxSpeed;
|
|
this->rotateSpeed = rotateSpeed;
|
|
this->moveSideSpeed = moveSideSpeed;
|
|
}
|
|
|
|
ControlModule::~ControlModule()
|
|
{
|
|
}
|
|
|
|
void ControlModule::adjustSpeed(){
|
|
double factor = 0.0;
|
|
if(maxSpeed > 1.0)
|
|
{
|
|
factor = 1.0;
|
|
}
|
|
else if(maxSpeed < -1.0)
|
|
{
|
|
factor = -1.0;
|
|
}
|
|
else
|
|
{
|
|
factor = maxSpeed;
|
|
}
|
|
|
|
for(int i = 0; i < 4; i++)
|
|
{
|
|
motors[i] *= maxSpeed;
|
|
}
|
|
};
|
|
|
|
void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
|
|
double speed = moveSideSpeed * static_cast<double>(contourColumsMiddle - imageColumsMiddle)/static_cast<double>(imageColumsMiddle);
|
|
motors[0] += speed;
|
|
motors[1] -= speed;
|
|
motors[2] -= speed;
|
|
motors[3] += speed;
|
|
}
|
|
|
|
void ControlModule::rotate(double angle){
|
|
double speed = rotateSpeed * (angle + 90.0)/90.0;
|
|
motors[0] += speed;
|
|
motors[1] -= speed;
|
|
motors[2] += speed;
|
|
motors[3] -= speed;
|
|
}
|
|
|
|
void ControlModule::unit(){
|
|
double max = 1.0E-6;
|
|
for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
|
|
if(motors[i] > max)
|
|
max = motors[i];
|
|
}
|
|
|
|
//Avoid dividing by zero
|
|
if (max > 0.001)
|
|
{
|
|
for(int i = 0; i < 4; i++){
|
|
motors[i] /= max;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle){
|
|
std::unique_lock<std::mutex> lock(mtx);
|
|
//moveSide(imageColumsMiddle, contourColumsMiddle);
|
|
rotate(angle);
|
|
unit();
|
|
adjustSpeed();
|
|
}
|
|
|
|
std::vector<double> ControlModule::readMotors()
|
|
{
|
|
std::unique_lock<std::mutex> lock(mtx);
|
|
return std::vector<double> {motors[0], motors[1], motors[2], motors[3]};
|
|
}
|