[code] CODE HERE [/code]

#pragma config(Sensor, dgtl1,  LeftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  RightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  SonarSensor,    sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  touchSensor,    sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorNormal, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]          [Name]              [Type]                [Description]                     *|
|*    Motor Port 2        rightMotor          VEX 3-wire module     Right side motor                  *|
|*    Motor Port 3        leftMotor           VEX 3-wire module     Left side motor                   *|
|*    Digital Port 6      touchSensor         VEX Bumper Switch     Top mounted, facing forward.      *|
|*    Digital Port 7      distanceSensor      VEX range sensor      Front mounted                     *|
\*-----------------------------------------------------------------------------------------------4246-*/
//#include <speedToPwm.h>


//______________________________Declaration of Variables_______________________________________________


float SPH=150;                  //setpoint high limit
float SPL=0;                   //setpoint low limit
int y=0;
float leftCountb4=0;
float rightCountb4=0;
float leftCount1sec=0;
float rightCount1sec=0;
float ynLeft=0;                 //current output sample of encoder counts for left wheel
float ynRight=0;
float KpLeft=1.2;                   
float KpRight=1.2;
float KdLeft=0.1;
float KdRight=0.14;
float KiLeft=0.05;                         
float KiRight=0.03;                         
float sampleRate=0.1;           //sample time of 0.1secs
float enLeft=0;                 //current sample
float enRight=0;
float en1Left=0;                //last error (n-1)
float en1Right=0;
float outLeft=0;                //controller output
float outRight=0;
float integratorOutputLeft=0;
float integratorOutputRight=0;
float intFactorLeft=1;         //anti windup switch
float intFactorRight=1;
int leftSpeed;
int rightSpeed;
int pwmOutLeft1=0;
int pwmOutRight1=0;
int speedSetpoint=60;
int offset=0;

/*--------------------------------------------------------------
Function to measure encoder counts within a certain time
----------------------------------------------------------------*/
int encoderCountsInTime()
{
  ClearTimer(T1);
  leftCountb4 = SensorValue[LeftEncoder];        //taking encoders counts as soon as timer begins
  rightCountb4= SensorValue[RightEncoder];      // (offset)
  while(y==0)
  {
    if(time1[T1]==100)
    {
      leftCount1sec=SensorValue[LeftEncoder];       //sampling counts after 0.1 sec
      rightCount1sec= SensorValue[RightEncoder];
      y=1;
    }
  }
  rightSpeed= rightCount1sec-rightCountb4;
  leftSpeed=leftCount1sec-leftCountb4;

  return rightSpeed;                              //return speeds
  return leftSpeed;
}


/*--------------------------------------------------------------
PID code for both motors to move at same speed
----------------------------------------------------------------*/

void pid( float setPoint)
{
  //store previous errors
  en1Left=enLeft;
  en1Right=enRight;

  //Sample Output
  ynLeft= leftSpeed;      //measured speeds
  ynRight=rightSpeed;
 
  ynRight= (-1)*ynRight; //multiplying by -1 since encoder for right drive counts backwards

 
  //checking setpoint limits

  if (setPoint> SPH)
  {
    setPoint=SPH;
  }
  else
  {
    setPoint = setPoint;
  }

  if (setPoint< SPL)
  {
    setPoint=SPL;
  }
  else
  {
    setPoint=setPoint;
  }


  //Determining current error

  enLeft = setPoint - ynLeft;
  enRight= setPoint - ynRight;


  //_______________________Compensate for windup_______________________________

  if (enLeft==en1Left)                  //windup if last and current error are the same
  {
    intFactorLeft=0;                   //switch integrator term off
  }


  if (enRight==en1Right)                  //windup if last and current error are the same
  {
    intFactorRight=0;                     //switch integrator term off
  }



  //___________________Calculating controller output______________________

  integratorOutputLeft= integratorOutputLeft + (intFactorLeft*enLeft);
  integratorOutputRight=integratorOutputRight+ (intFactorRight*enRight);

  outLeft= KpLeft*enLeft + KiLeft*integratorOutputLeft*sampleRate + (KdLeft*(enLeft-en1Left)/sampleRate);
  outRight=(KpRight*enRight)+(KiRight*integratorOutputRight*sampleRate) + (KdRight*(enRight-en1Right)/sampleRate);

//Calculating new output PWM value
 
  pwmOutLeft1=pwmOutLeft1+ (outLeft/3);
  pwmOutRight1=pwmOutRight1+ (outRight/3);


  //______________________Checking output limits___________________________

  if (pwmOutLeft1> 127)
  {
    pwmOutLeft1=127;
  }
  if(pwmOutRight1>127)
  {
    pwmOutRight1=127;
  }

  if (pwmOutLeft1<0)
  {
    pwmOutLeft1=0;
  }
  if(pwmOutRight1<0)
  {
    pwmOutRight1=0;
  }

  motor[leftMotor] =-pwmOutLeft1+40;
  motor[rightMotor]=-pwmOutRight1-40;

}

//-----------------------------------------------------------------------------------


task main ()

  intFactorLeft=1;
  intFactorRight=1;
 
  motor[leftMotor]=pwmOutLeft1;
  motor[rightMotor]=pwmOutRight1;
  wait1Msec(1000);

  while(1)
  {
    wait1Msec(100);
    y=0;
    encoderCountsInTime();             //check encoder counts within 100ms
    pid(speedSetpoint);


  }
}