drivetask

task driveTask()
{
    float driveStraightkp;
    float turnSynckp;
    if(lDist == rDist && abs(lDist) > 400)
    {
        driveStraightkp = 0.35;
    }
    else
    {
        driveStraightkp = 0;
    }
    if(lDist == -rDist && abs(lDist) > 30)
    {
        turnSynckp = 1;
    }
    else
    {
        turnSynckp = 0;
    }

    float leftDeriv = 0;
    float rightDeriv = 0;
    float dFilter = 0.1;
    prevLeftError = leftDriveTarget  - SensorValue[leftDriveEncoder]; //error values we save to calculate the derivative
    prevRightError = rightDriveTarget - SensorValue[rightDriveEncoder];
    int stoppedCounter = 0; //counter for when the robot has stopped moving
    float leftInt = 0;
    float rightInt = 0;
    driveTargetReached = false;
    while(true)
    {
        leftError = leftDriveTarget - SensorValue[leftDriveEncoder]; //Calculate the error in the difference between leftError and rightError
        rightError = rightDriveTarget - SensorValue[rightDriveEncoder];

        leftInt += leftError;
        rightInt += rightError;

        if(abs(leftError) > 100)//if the robot is far from the target keep integral at 0
        {
            leftInt = 0;
        }
        if(abs(rightError) > 100)
        {
            rightInt = 0;
        }
        if(sgn(leftError) != sgn(leftInt))//if the robot has crossed the target reset integral
        {
            leftInt = 0;
        }
        if(sgn(rightError) != sgn(rightInt))
        {
            rightInt = 0;
        }
        if(abs(leftInt) > intCap/ki)// if the integral is greater than the cap divided by the integral constant set integral to its maximum
        {
            leftInt = (intCap/ki)*sgn(leftInt);
        }
        if(abs(rightInt) > intCap/ki)
        {
            rightInt = (intCap/ki)*sgn(rightInt);
        }

        if(abs(leftError) < 100 || abs(rightError) < 100) //if the error is small don't correct drive difference
        {
            driveStraightkp = 0;
        }

        if(reverseDrive)
        {
            if(abs(leftError) < 100 || abs(leftDriveTarget - lDist- SensorValue[leftDriveEncoder]) < 200)
            {
                speedLimiter = 40;
            }
            else
            {
                speedLimiter = maxSpeed;
            }
        }

        if(lDist == rDist && !reverseDrive)
        {
            if(abs(leftError) < 80)
            {
                speedLimiter = 35;
            }
            else if(abs(leftError) < 150)
            {
                speedLimiter = 75;
            }
            else
            {
                speedLimiter = maxSpeed;
            }
        }

        leftDeriv = dFilter * (leftError - prevLeftError) + (1 - dFilter) * leftDeriv;
        rightDeriv = dFilter * (rightError - prevRightError) + (1 - dFilter) * rightDeriv;

        float leftDrivePower = kp * leftError + kd * leftDeriv + ki * leftInt;
        float rightDrivePower = kp * rightError + kd * rightDeriv + ki * rightInt;

        if(leftDrivePower > speedLimiter)leftDrivePower = speedLimiter;
        if(leftDrivePower < -speedLimiter)leftDrivePower = -speedLimiter;
        if(rightDrivePower > speedLimiter)rightDrivePower = speedLimiter;
        if(rightDrivePower < -speedLimiter)rightDrivePower = -speedLimiter;


        if(abs(leftError) > 50)
        {
            leftDrivePower += driveStraightkp *(leftError-rightError); //correct drive difference by adjusting power
            rightDrivePower -= driveStraightkp * (leftError-rightError);

            leftDrivePower += turnSynckp *(leftError+rightError); //correct drive difference by adjusting power
            rightDrivePower += turnSynckp * (leftError+rightError);
        }

        setDrive(leftDrivePower, rightDrivePower);//set the drive motors to their calculated power

        if (leftError < 0 || rightError < 0){ //if the robot has passed the target set the target reached value to true
            driveTargetReached = true;
        }

        if(abs(leftDeriv) < 1.0 && abs(rightDeriv) < 1 && time1[T4] > 500){ //if the robot isnt moving or is barely moving increase the stopped counter
            stoppedCounter++;
        }
        if(stoppedCounter > accuracy || time1[T4] > driveTimeout){ //if the stopped counter reaches a threshold or the 5 second drive timeout is reached start end of drive code
            if(abs(leftError) < 200 && abs(rightError) < 200){//if error is near target then finish the drive
                setDrive(0,0);
                driveFinished = true;
                driveTargetReached = true;
                setDrive(0,0);
                stopTask(driveTask);
            }
            else //if the drive reaches timeout stop the drive and abort auton.
            {
                setDrive(0,0);
                driveFinished = true;
                startTask(abort);//start the abort task
                stopTask(driveTask);
            }
        }
        prevLeftError = leftError; //reset Previus Error for the derivative
        prevRightError = rightError;
        delay(10);
    }
}