task SpeedAdjust();

task main()
{
  nxtDisplayCenteredTextLine(0, "Do you want");
  nxtDisplayCenteredTextLine(1, "PID control?");
  nxtDisplayCenteredTextLine(2, "This will give");
  nxtDisplayCenteredTextLine(3, "speeds like the");
  nxtDisplayCenteredTextLine(4, "MindStorms NXT");
  nxtDisplayCenteredTextLine(5, "Servo Motor.");
  nxtDisplayCenteredTextLine(7, "No.        Yes.");

  while(nNxtButtonPressed == kNoButton);

  if(nNxtButtonPressed == kLeftButton) nMotorPIDSpeedCtrl[motorA] = mtrNoReg;
  else nMotorPIDSpeedCtrl[motorA] = mtrSpeedReg;

  while(nNxtButtonPressed != kNoButton);

  eraseDisplay();

  nxtDisplayCenteredTextLine(7, "DEC         INC");

  StartTask(SpeedAdjust);

  while(true)
  {
    time1[T1] = 0;

    nMotorEncoder[motorA] = 0;

    while(nMotorEncoder[motorA] < 360 && nMotorEncoder[motorA] > -360);

    nxtDisplayCenteredTextLine(2, "Mtr RPM = %d", 60000 / time1[T1]);
    nxtDisplayCenteredTextLine(3, "Mtr RPS = %d", 1000 / time1[T1]);
  }
}

task SpeedAdjust()
{
  int speed = 0;

  while(true)
  {
    if(nNxtButtonPressed == kLeftButton) speed -= 1;
    if(nNxtButtonPressed == kRightButton) speed += 1;

    if(speed > 100) speed = 100;
    if(speed < -100) speed = -100;

    nxtDisplayCenteredTextLine(0, "Mtr speed = %d", speed);
    nxtDisplayCenteredTextLine(1, "Mtr PWM = %d", motorPWMLevel[motorA]);

    motor[motorA] = speed;

    wait1Msec(100);
  }
}

#pragma config(Motor,  motorC,          ,              tmotorNormal, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
  long MtrBTtlEncdr = 0;
  long MtrCTtlEncdr = 0;

  for(int i = 1; i <= 1000; i++)
  {
    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;

    nMotorEncoderTarget[motorB] = 360;
    nMotorEncoderTarget[motorC] = 360;

    motor[motorB] = 50;
    motor[motorC] = 50;

    while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle);

    motor[motorB] = 0;
    motor[motorC] = 0;

    MtrBTtlEncdr += nMotorEncoder[motorB];
    MtrCTtlEncdr += nMotorEncoder[motorC];

    nxtDisplayCenteredTextLine(2, "NXT Encdr: %d", MtrBTtlEncdr);
    nxtDisplayCenteredTextLine(3, "PF Encdr: %d", MtrCTtlEncdr);
    nxtDisplayCenteredTextLine(5, "Revolutions: %d", i);
  }
}