55 lines
1.3 KiB
C++
55 lines
1.3 KiB
C++
#include "control_module.h"
|
|
|
|
ControlModule::ControlModule(){
|
|
}
|
|
|
|
ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
|
|
{
|
|
this->forwardSpeed = forwardSpeed;
|
|
this->rotateSpeed = rotateSpeed;
|
|
this->moveSideSpeed = moveSideSpeed;
|
|
}
|
|
|
|
ControlModule::~ControlModule()
|
|
{
|
|
}
|
|
|
|
void ControlModule::moveForward(){
|
|
for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
|
|
motors[i] += forwardSpeed;
|
|
}
|
|
};
|
|
|
|
void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
|
|
float speed = moveSideSpeed * (contourColumsMiddle - imageColumsMiddle)/imageColumsMiddle;
|
|
motors[0] += speed;
|
|
motors[1] -= speed;
|
|
motors[2] -= speed;
|
|
motors[3] += speed;
|
|
}
|
|
|
|
void ControlModule::rotate(int angle){
|
|
float speed = rotateSpeed * (angle + 90)/90;
|
|
motors[0] += speed;
|
|
motors[1] -= speed;
|
|
motors[2] += speed;
|
|
motors[3] -= speed;
|
|
}
|
|
|
|
void ControlModule::unit(){
|
|
float max = 10E-12;
|
|
for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
|
|
if(motors[i] > max)
|
|
max = motors[i];
|
|
}
|
|
for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
|
|
motors[i] /= max;
|
|
}
|
|
}
|
|
|
|
void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
|
|
moveForward();
|
|
moveSide(imageColumsMiddle, contourColumsMiddle);
|
|
rotate(angle);
|
|
unit();
|
|
} |