/// <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;
}
