|
|
@@ -4,7 +4,7 @@ ControlModule::ControlModule(): ControlModule(0.0, 1.0, 1.0) |
|
|
|
{ |
|
|
|
} |
|
|
|
|
|
|
|
ControlModule::ControlModule(float maxSpeed, float rotateSpeed, float moveSideSpeed) |
|
|
|
ControlModule::ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed) |
|
|
|
{ |
|
|
|
motors[0] = 0.0; |
|
|
|
motors[1] = 0.0; |
|
|
@@ -41,15 +41,15 @@ void ControlModule::adjustSpeed(){ |
|
|
|
}; |
|
|
|
|
|
|
|
void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){ |
|
|
|
float speed = moveSideSpeed * static_cast<float>(contourColumsMiddle - imageColumsMiddle)/static_cast<float>(imageColumsMiddle); |
|
|
|
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(int angle){ |
|
|
|
float speed = rotateSpeed * (static_cast<float>(angle) + 90.0f)/90.0f; |
|
|
|
void ControlModule::rotate(double angle){ |
|
|
|
double speed = rotateSpeed * (angle + 90.0)/90.0; |
|
|
|
motors[0] += speed; |
|
|
|
motors[1] -= speed; |
|
|
|
motors[2] += speed; |
|
|
@@ -57,7 +57,7 @@ void ControlModule::rotate(int angle){ |
|
|
|
} |
|
|
|
|
|
|
|
void ControlModule::unit(){ |
|
|
|
float max = 1.0E-6; |
|
|
|
double max = 1.0E-6; |
|
|
|
for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){ |
|
|
|
if(motors[i] > max) |
|
|
|
max = motors[i]; |
|
|
@@ -73,7 +73,7 @@ void ControlModule::unit(){ |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){ |
|
|
|
void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle){ |
|
|
|
std::unique_lock<std::mutex> lock(mtx); |
|
|
|
//moveSide(imageColumsMiddle, contourColumsMiddle); |
|
|
|
rotate(angle); |
|
|
@@ -81,8 +81,8 @@ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, i |
|
|
|
adjustSpeed(); |
|
|
|
} |
|
|
|
|
|
|
|
std::vector<float> ControlModule::readMotors() |
|
|
|
std::vector<double> ControlModule::readMotors() |
|
|
|
{ |
|
|
|
std::unique_lock<std::mutex> lock(mtx); |
|
|
|
return std::vector<float> {motors[0], motors[1], motors[2], motors[3]}; |
|
|
|
return std::vector<double> {motors[0], motors[1], motors[2], motors[3]}; |
|
|
|
} |