Basketball Drills - ROBOTC

//Basketball Drills [IN PROGRESS] by Sebastian1314 and Steve Von Fedak

#pragma config(Sensor, in2,    rEnc,                sensorQuadEncoder, int5)
#pragma config(Sensor, in3,    lEnc,                sensorQuadEncoder, int6)
#pragma config(Motor,  port2,           rMotor,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           lMotor,        tmotorNormal, openLoop)
#pragma config(Motor,  port6,           topMotor,      tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void initProg(int waitTime)
{
    wait1Msec(waitTime);
    bMotorReflected[rMotor] = 1;
}

void resetEncoders()
{
    SensorValue[lEnc] = 0;
    SensorValue[rEnc] = 0;
}

void moveForward(int rotationLength)
{
  while(SensorValue[rEnc] >= (rotationLength*-1))
    {
      if(SensorValue[lEnc] == SensorValue[rEnc])
        {
          motor[port3] = 63;
          motor[port2] = 63;
        }
        else if(SensorValue[lEnc] < SensorValue[rEnc])
        {
          motor[port3] = 43;
          motor[port2] = 63;
        }
        else
        {
          motor[port3] = 63;
          motor[port2] = 43;
        }
    }
}

void moveBackwards  (int rotationLength)
{
  while(SensorValue[rEnc] <= rotationLength)
    {
      if(SensorValue[lEnc] == SensorValue[rEnc])
        {
          motor[port3] = -63;
          motor[port2] = -63;
        }
        else if(SensorValue[lEnc] > SensorValue[rEnc])
        {
          motor[port3] = -43;
          motor[port2] = -63;
        }
        else
        {
          motor[port3] = -63;
          motor[port2] = -43;
        }
    }
}

task main()
{
  initProg(2000);
  resetEncoders();

  moveForward(1200) ; resetEncoders();

  moveBackwards(900); resetEncoders();

  moveForward(2200) ; resetEncoders();

  moveBackwards(2150); resetEncoders();
}