/// <summary> /// Calculates the target position depending on data received from the camera. /// </summary> /// <param name="lineDetectList">The result of CV.</param> /// <returns>The resulting target position.</returns> public int CalcTargetPos(LineRecognitionResult lineDetectList) { int targetPos = 0; int maxAbsTargetPos = 10; double middle = 0.5; if (lineDetectList.ProminentBlock.X < middle) { targetPos = (int)-((middle - lineDetectList.ProminentBlock.X) / middle * maxAbsTargetPos); } else { targetPos = (int)((lineDetectList.ProminentBlock.X - middle) / middle * maxAbsTargetPos); } return(targetPos); }
/// <summary> /// Calculates the new speeds for the wheels depending on the perceived line position. /// </summary> /// <param name="lineDetectList">The result of CV.</param> /// <param name="rightSpeed">The new speed for the right wheel.</param> /// <param name="leftSpeed">The new speed for the left wheel.</param> public void DoPidLoop(LineRecognitionResult lineDetectList, bool backwardDrive, ref int rightSpeed, ref int leftSpeed) { // Determine target position by using the CV feedback int targetPos = CalcTargetPos(lineDetectList); // We have to turn the other way around if we want to drive backwards if (backwardDrive) { targetPos = -targetPos; } // Calculate corrected motor speeds int motorSpeed = CalcPid(targetPos); // Measure performance PIDLoopsDone++; // Init motor speeds int rightMotorSpeed = 0; int leftMotorSpeed = 0; // Check whether we want to go forward or backward if (!backwardDrive) { // Set motor speeds rightMotorSpeed = _baseSpeed - motorSpeed; leftMotorSpeed = _baseSpeed + motorSpeed; // Prevent the motor from going beyond max speed if (rightMotorSpeed > _maxSpeed) { rightMotorSpeed = _maxSpeed; } if (leftMotorSpeed > _maxSpeed) { leftMotorSpeed = _maxSpeed; } // Keep the motor speed positive if (rightMotorSpeed < 0) { rightMotorSpeed = 0; } if (leftMotorSpeed < 0) { leftMotorSpeed = 0; } } else { // Set motor speeds rightMotorSpeed = -(_baseSpeed / 2) + motorSpeed; leftMotorSpeed = -(_baseSpeed / 2) - motorSpeed; // Prevent the motor from going beyond max speed if (Math.Abs(rightMotorSpeed) > _maxSpeed) { rightMotorSpeed = -_maxSpeed; } if (Math.Abs(leftMotorSpeed) > _maxSpeed) { leftMotorSpeed = -_maxSpeed; } // Keep the motor speed negative if (rightMotorSpeed > 0) { rightMotorSpeed = 0; } if (leftMotorSpeed > 0) { leftMotorSpeed = 0; } } // Now move forward with appropriate speeds!!!! rightSpeed = rightMotorSpeed; leftSpeed = leftMotorSpeed; }