#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(contourColumsMiddle - imageColumsMiddle)/static_cast(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 lock(mtx); //moveSide(imageColumsMiddle, contourColumsMiddle); rotate(angle); unit(); adjustSpeed(); } std::vector ControlModule::readMotors() { std::unique_lock lock(mtx); return std::vector {motors[0], motors[1], motors[2], motors[3]}; }