#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  motorA,          Arm1,          tmotorNormal, PIDControl)
#pragma config(Motor,  motorB,          Arm2,          tmotorNormal, PIDControl)
#pragma config(Motor,  motorC,          Arm3,          tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Servo,  servo1,          wrist1,               tServoNormal)
#pragma config(Servo,  servo2,          wrist2,               tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void startup()
{
  nxtDisplayCenteredTextLine(3, "Begin test mode?");
  while(nNxtButtonPressed == -1)
  {
    wait1Msec(1);
  }
  return;
}

void Servotest() <--this part won't work
{
  eraseDisplay();
  nxtDisplayCenteredTextLine(3, "Press square button");
  nxtDisplayCenteredTextLine(4, "to test system");
  while(nNxtButtonPressed == -1)
  {
    wait1Msec(1);
  }
  eraseDisplay();
  nxtDisplayCenteredTextLine(3, "IMMA FIRIN MAH...");
  servo[wrist1] = 255;
  servo[wrist2] = 0;
  ClearTimer(T1);
  while(time100[T1] < 10)
  {
    //do nothing
  }
  eraseDisplay();
  nxtDisplayCenteredBigTextLine(4, "...LAZAR!!!");
  servo[wrist1] = 128;
  servo[wrist2] = 128;
  wait10Msec(100);
  return;
}

void NXTtest()
{
  eraseDisplay();
  nxtDisplayCenteredTextLine(3, "So I heard you like");
  wait10Msec(100);
  eraseDisplay();
  nxtDisplayCenteredBigTextLine(4, "Never mind...");
  PlaySoundFile("! Startup.rso");
  wait10Msec(100);
  motor[Arm1] = 100;
  motor[Arm2] = 100;
  motor[Arm3] = 100;
  wait10Msec(50);
  motor[Arm1] = -100;
  motor[Arm2] = -100;
  motor[Arm3] = -100;
  wait10Msec(50);
  motor[Arm1] = 0;
  motor[Arm2] = 0;
  motor[Arm3] = 0;
  return;
}

task main()
{
  startup();
  Servotest();
  NXTtest();
}