Browse Source

P-Controller Concept

master
Tim Zeuner 1 year ago
parent
commit
0f5c2e3f42
1 changed files with 13 additions and 1 deletions
  1. 13
    1
      AutonomousMode/ControlModule/control_module.cpp

+ 13
- 1
AutonomousMode/ControlModule/control_module.cpp View File

@@ -49,7 +49,19 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
}

void ControlModule::rotate(double angle){
double speed = rotateSpeed * (angle + 90.0)/90.0;
double minAngularSpeed = -1.0;
double maxAngularSpeed = 1.0;
double speedRange = maxAngularSpeed - minAngularSpeed;

double minAngle = -90.0;
double maxAngle = 90.0;
double angularRange = maxAngle - minAngle;

double progress = angle - minAngle;
double progressPercent = progress/angularRange;

double speed = minAngularSpeed + progressPercent*speedRange;

motors[0] += speed;
motors[1] -= speed;
motors[2] += speed;

Loading…
Cancel
Save